IMU和GPS融合定位（ESKF）

说明

1.本文理论部分参考文章https://zhuanlan.zhihu.com/p/152662055和https://blog.csdn.net/brightming/article/details/118057262
ROS下的实践参考https://blog.csdn.net/qinqinxiansheng/article/details/107108475和https://zhuanlan.zhihu.com/p/163038275

理论

坐标系
在这里插入图片描述
error-state
在这里插入图片描述
ESKF GPS更新
在这里插入图片描述
初始化
初始位置为0,方向的roll和pitch可由加速度的重力方向确定,bias设置为0.

代码

代码请见github：https://github.com/ydsf16/imu_gps_localization
在这里插入图片描述
总体流程如下：接收IMU数据，进行积分，更新系统方程，接收GPS数据，计算K，更新状态量

代码整体框架说明
代码具体结构框架如下图所示：
在这里插入图片描述
在这里插入图片描述
主要函数介绍
1）状态定义：

struct State {
    double timestamp;
    
    Eigen::Vector3d lla;       // WGS84 position.
    Eigen::Vector3d G_p_I;     // The original point of the IMU frame in the Global frame.
    Eigen::Vector3d G_v_I;     // The velocity original point of the IMU frame in the Global frame.
    Eigen::Matrix3d G_R_I;     // The rotation from the IMU frame to the Global frame.
    Eigen::Vector3d acc_bias;  // The bias of the acceleration sensor.
    Eigen::Vector3d gyro_bias; // The bias of the gyroscope sensor.

    // Covariance.
    Eigen::Matrix<double, 15, 15> cov;

    // The imu data.
    ImuDataPtr imu_data_ptr; 
};

包含：

Eigen::Vector3d G_p_I;     // The original point of the IMU frame in the Global frame.
    Eigen::Vector3d G_v_I;     // The velocity original point of the IMU frame in the Global frame.
    Eigen::Matrix3d G_R_I;     // The rotation from the IMU frame to the Global frame.
    Eigen::Vector3d acc_bias;  // The bias of the acceleration sensor.
    Eigen::Vector3d gyro_bias; // The bias of the gyroscope sensor.

5个状态量，在协方差表示时，旋转也用三维的旋转角表示，所以，其协方差矩阵为15。

在这里插入图片描述

2） LocalizationWrapper构造函数
LocalizationWrapper构造函数主要实现加载参数、添加数据保存位置、订阅话题和发布话题

LocalizationWrapper::LocalizationWrapper(ros::NodeHandle& nh) {
    // Load configs.
    double acc_noise, gyro_noise, acc_bias_noise, gyro_bias_noise;
    nh.param("acc_noise",       acc_noise, 1e-2);
    nh.param("gyro_noise",      gyro_noise, 1e-4);
    nh.param("acc_bias_noise",  acc_bias_noise, 1e-6);
    nh.param("gyro_bias_noise", gyro_bias_noise, 1e-8);

    double x, y, z;
    nh.param("I_p_Gps_x", x, 0.);
    nh.param("I_p_Gps_y", y, 0.);
    nh.param("I_p_Gps_z", z, 0.);
    const Eigen::Vector3d I_p_Gps(x, y, z);

    std::string log_folder
        = "/home/sunshine/catkin_imu_gps_localization/src/imu_gps_localization";
    ros::param::get("log_folder", log_folder);

    // Log.
    file_state_.open(log_folder + "/state.csv");
    file_gps_.open(log_folder +"/gps.csv");

    // Initialization imu gps localizer.
    imu_gps_localizer_ptr_ = 
        std::make_unique<ImuGpsLocalization::ImuGpsLocalizer>(acc_noise, gyro_noise,
                                                              acc_bias_noise, gyro_bias_noise,
                                                              I_p_Gps);

    // Subscribe topics.
    imu_sub_ = nh.subscribe("/imu/data", 10,  &LocalizationWrapper::ImuCallback, this);
    //TODO 运行数据集需要修改的地方： gps的话题为/fix
    gps_position_sub_ = nh.subscribe("/fix", 10,  &LocalizationWrapper::GpsPositionCallback, this);
    
    //发布融合后的轨迹
    state_pub_ = nh.advertise<nav_msgs::Path>("fused_path", 10);
}

3）滤波算法进行预测

void ImuProcessor::Predict(const ImuDataPtr last_imu, const ImuDataPtr cur_imu, State* state) {
    // Time.
    const double delta_t = cur_imu->timestamp - last_imu->timestamp;
    const double delta_t2 = delta_t * delta_t;

    // Set last state.
    State last_state = *state;
    // mid_point integration methods
    // Acc and gyro.
    const Eigen::Vector3d acc_unbias = 0.5 * (last_imu->acc + cur_imu->acc) - last_state.acc_bias;
    const Eigen::Vector3d gyro_unbias = 0.5 * (last_imu->gyro + cur_imu->gyro) - last_state.gyro_bias;

    // Normal state. 
    // Using P58. of "Quaternion kinematics for the error-state Kalman Filter".
    state->G_p_I = last_state.G_p_I + last_state.G_v_I * delta_t + 
                   0.5 * (last_state.G_R_I * acc_unbias + gravity_) * delta_t2;
    state->G_v_I = last_state.G_v_I + (last_state.G_R_I * acc_unbias + gravity_) * delta_t;
    const Eigen::Vector3d delta_angle_axis = gyro_unbias * delta_t;
    if (delta_angle_axis.norm() > 1e-12) {
        // std::cout << "norm" << delta_angle_axis.norm() << "normlized"
        //           << delta_angle_axis.normalized() << std::endl;
        state->G_R_I = last_state.G_R_I
            * Eigen::AngleAxisd(delta_angle_axis.norm(),
                                delta_angle_axis.normalized())
                  .toRotationMatrix();
    }
    // Error-state. Not needed.

    // Covariance of the error-state.   
    Eigen::Matrix<double, 15, 15> Fx = Eigen::Matrix<double, 15, 15>::Identity();
    Fx.block<3, 3>(0, 3)   = Eigen::Matrix3d::Identity() * delta_t;
    Fx.block<3, 3>(3, 6)   = - state->G_R_I * GetSkewMatrix(acc_unbias) * delta_t;
    Fx.block<3, 3>(3, 9)   = - state->G_R_I * delta_t;
    if (delta_angle_axis.norm() > 1e-12) {
        Fx.block<3, 3>(6, 6) = Eigen::AngleAxisd(delta_angle_axis.norm(), delta_angle_axis.normalized()).toRotationMatrix().transpose();
    } else {
        Fx.block<3, 3>(6, 6).setIdentity();
    }
    Fx.block<3, 3>(6, 12)  = - Eigen::Matrix3d::Identity() * delta_t;

    Eigen::Matrix<double, 15, 12> Fi = Eigen::Matrix<double, 15, 12>::Zero();
    Fi.block<12, 12>(3, 0) = Eigen::Matrix<double, 12, 12>::Identity();

    Eigen::Matrix<double, 12, 12> Qi = Eigen::Matrix<double, 12, 12>::Zero();
    Qi.block<3, 3>(0, 0) = delta_t2 * acc_noise_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(3, 3) = delta_t2 * gyro_noise_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(6, 6) = delta_t * acc_bias_noise_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(9, 9) = delta_t * gyro_bias_noise_ * Eigen::Matrix3d::Identity();
    //协方差预测
    state->cov = Fx * last_state.cov * Fx.transpose() + Fi * Qi * Fi.transpose();

    // Time and imu.
    state->timestamp = cur_imu->timestamp;
    state->imu_data_ptr = cur_imu;
}

predict主要是对nominal state的运动学估计，以及对协方差的递推（为了在观测值来的时候，结合两者的协方差算出K值）。

协方差传递的Fx的计算，与《Quaternion Kinematics for the error-state KF》中的一致：
（误差的传递与nominal state的传递都是一样的，遵循的都是相同的运动学模型，只是误差会在之前的数值的基础上继续包括新增的各种运动误差，如imu的bias，随机游走等，而nominal则不管这些值，按照正常的运动学模型递推。）

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

accelerometer_noise_density: 0.012576 #continous 
accelerometer_random_walk: 0.000232  
gyroscope_noise_density: 0.0012615 #continous 
gyroscope_random_walk: 0.0000075

代码中是这样设置的：

    Eigen::Matrix<double, 12, 12> Qi = Eigen::Matrix<double, 12, 12>::Zero();
    Qi.block<3, 3>(0, 0) = delta_t2 * acc_noise_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(3, 3) = delta_t2 * gyro_noise_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(6, 6) = delta_t * acc_bias_noise_ * Eigen::Matrix3d::Identity();
    Qi.block<3, 3>(9, 9) = delta_t * gyro_bias_noise_ * Eigen::Matrix3d::Identity();

这些参数，代码中是在初始化时设置的：

LocalizationWrapper::LocalizationWrapper(ros::NodeHandle& nh) {
    // Load configs.
    double acc_noise, gyro_noise, acc_bias_noise, gyro_bias_noise;
    nh.param("acc_noise",       acc_noise, 1e-2);
    nh.param("gyro_noise",      gyro_noise, 1e-4);
    nh.param("acc_bias_noise",  acc_bias_noise, 1e-6);
    nh.param("gyro_bias_noise", gyro_bias_noise, 1e-8);

<launch>
    <param name="acc_noise"       type="double" value="1e-2" />
    <param name="gyro_noise"      type="double" value="1e-4" />
    <param name="acc_bias_noise"  type="double" value="1e-6" />
    <param name="gyro_bias_noise" type="double" value="1e-8" />

    <param name="I_p_Gps_x"       type="double" value="0.0" />
    <param name="I_p_Gps_y"       type="double" value="0.0" />
    <param name="I_p_Gps_z"       type="double" value="0.0" />

    <param name="log_folder"      type="string" value="$(find imu_gps_localization)" />

    <node name="nmea_topic_driver" pkg="nmea_navsat_driver" type="nmea_topic_driver" output="screen" />
    <node name="imu_gps_localization_node" pkg="imu_gps_localization" type="imu_gps_localization_node" output="screen" />

    <node pkg="rviz" type="rviz" name="rviz" output="screen" 
      args="-d $(find imu_gps_localization)/ros_wrapper/rviz/default.rviz" required="true">
    </node>

</launch>

在这里插入图片描述
代码中：

state->cov = Fx * last_state.cov * Fx.transpose() + Fi * Qi * Fi.transpose();

在这里插入图片描述
也就是以下代码实现：

void GpsProcessor::ComputeJacobianAndResidual(const Eigen::Vector3d& init_lla,  
                                              const GpsPositionDataPtr gps_data, 
                                              const State& state,
                                              Eigen::Matrix<double, 3, 15>* jacobian,
                                              Eigen::Vector3d* residual) {
    const Eigen::Vector3d& G_p_I   = state.G_p_I;
    const Eigen::Matrix3d& G_R_I   = state.G_R_I;

    // Convert wgs84 to ENU frame.
    Eigen::Vector3d G_p_Gps;//测量值
    ConvertLLAToENU(init_lla, gps_data->lla, &G_p_Gps);

    // Compute residual.
    //I_p_Gps_在imu坐标系下的位移？是固定值？
    //G_p_I + G_R_I * I_p_Gps_预测的状态计算出来的坐标点
    *residual = G_p_Gps - (G_p_I + G_R_I * I_p_Gps_);

    // Compute jacobian.
    jacobian->setZero();
    jacobian->block<3, 3>(0, 0)  = Eigen::Matrix3d::Identity();
    jacobian->block<3, 3>(0, 6)  = - G_R_I * GetSkewMatrix(I_p_Gps_);
}

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
第二项，其实就是真值对误差项目的求导，看下表：
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

   // Compute jacobian.
    jacobian->setZero();
    jacobian->block<3, 3>(0, 0)  = Eigen::Matrix3d::Identity();
    jacobian->block<3, 3>(0, 6)  = - G_R_I * GetSkewMatrix(I_p_Gps_);

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：/a/21949.html

如若内容造成侵权/违法违规/事实不符，请联系我们进行投诉反馈qq邮箱809451989@qq.com，一经查实，立即删除！

相关文章

Python中的自定义函数创建方法和应用举例

Python中的自定义函数创建方法和应用举例

Python中的自定义函数创建方法和应用举例 在Python语言中&#xff0c;函数是一组能够完成特定任务的语句模块&#xff0c;可分为内置函数、第三方模块函数和自定义函数。其中&#xff0c;内置函数是Python系统自带的函数&#xff1b;模块函数是NumPy等库中的函数。 1.自定义函…
阅读更多...
如何让数据安全管理工作化繁为简？uDSP 十问十答

如何让数据安全管理工作化繁为简？uDSP 十问十答

数据安全管理工作与国家数据安全、企业资产保护以及个人信息保护工作息息相关。复杂、多元、流通的数据也给数据安全带来了更多的威胁和挑战&#xff0c;如数据资产管理、分类分级问题&#xff0c;数据安全集中管控问题&#xff0c;数据共享与流通问题等。原点安全一体化数据安…
阅读更多...
开源之夏 2023 | 欢迎报名Rust相关项目

开源之夏 2023 | 欢迎报名Rust相关项目

开源之夏是中国科学院软件研究所联合openEuler发起的开源软件供应链点亮计划系列暑期活动&#xff0c;旨在鼓励在校学生积极参与开源软件的开发维护&#xff0c;促进优秀开源软件社区的蓬勃发展。活动联合各大开源社区&#xff0c;针对重要开源软件的开发与维护提供项目&#x…
阅读更多...
Mongodb Shell 常用操作命令

Mongodb Shell 常用操作命令

目录 一、启动与关闭mongodb服务 二、进入shell操作 三、常用shell命令 一、启动与关闭mongodb服务 启动:命令: ./mongod -config ../data/mongodb.conf 关闭命令: ./mongod -config ../data/mongodb.conf -shutdown 二、进入shell操作 命令:./mongo 三、常用shell命令 sh…
阅读更多...
企业型OV，增强型EV证书开通审核流程

企业型OV，增强型EV证书开通审核流程

选购 OV、EV型证书需要是企业用户&#xff0c;申请过程中需要提交企业资料。 选择证书品牌及型号 管理中心补全审核资料 下载“确认函”签字盖章并扫描&#xff0c;再上传。 审核部门联系确认企业信息&#xff0c;&#xff08;过需要7-10个工作日&#xff09; 审核通过&…
阅读更多...
京东商品详情API调用说明 京东商品库存销量接口

京东商品详情API调用说明 京东商品库存销量接口

尊敬的开发人员&#xff1a; 感谢您选择使用京东API进行开发。下面为您提供一份简要的API调用说明&#xff0c;帮助您快速上手并实现所需功能。 1.注册京东开放平台账户并创建应用 首先&#xff0c;您需要在 https://o0b.cn/jennif/ 网站上注册一个京东开放平台的账户&#…
阅读更多...
【黑马2023大数据实战教程】VMWare虚拟机部署HDFS集群详细过程

【黑马2023大数据实战教程】VMWare虚拟机部署HDFS集群详细过程

文章目录 部署HDFS集群1.配置workers:2.配置hadoop-env.sh文件3.配置core-site.xml文件4.配置hdfs-site.xml文件准备数据目录分发Hadoop文件夹配置环境变量授权为hadoop用户格式化文件系统错误排查方法!! 视频&#xff1a;黑马2023 VMWare虚拟机部署HDFS集群 注意&#xff01;这…
阅读更多...
Jmeter性能测试工具之性能测试的概念

Jmeter性能测试工具之性能测试的概念

性能测试的概念 性能测试是指通过特定方式&#xff0c;对被测系统按照一定策略施加压力&#xff0c;获取系统 响应时间、TPS&#xff08;Transaction Per Second&#xff09;、吞吐量、资源利用率等性能指标&#xff0c;以期保证生产系统的性能能够满足用户需求的过程。 性能…
阅读更多...
opencv_c++学习（十六）

opencv_c++学习（十六）

一、线性滤波 均值滤波&#xff1a; blur(InputArray src, utputArray dst,Size ksize, Point anchor Point(-i,-1), int borderType BoRDER_DEFAULT)src:待均值滤波的图像&#xff0c;图像的数据类型必须是CV_8U、CV_16U、CV_16S、CV_32F和CV_64F这五种数据类型之一。 ds…
阅读更多...
ESP8266连接 TLink 云平台

ESP8266连接 TLink 云平台

1.硬件准备 &#xff08;1&#xff09;正点原子 ATK-ESP-01 WIFI 模块 &#xff08;2&#xff09;正点原子 STM32F103ZET6精英板子 &#xff08;3&#xff09;USB转TTL模块 2.烧录固件 &#xff08;1&#xff09;烧录软件和固件都可以在正点原子增值资料包找到。 &#xff08;2…
阅读更多...
【大数据学习篇8】 热门品类Top10分析

【大数据学习篇8】 热门品类Top10分析

在HBase命令行工具中执行“list”命令&#xff0c;查看HBase数据库中的所有数据表。学习目标/Target 掌握热门品类Top10分析实现思路 掌握如何创建Spark连接并读取数据集 掌握利用Spark获取业务数据 掌握利用Spark统计品类的行为类型 掌握利用Spark过滤品类的行为类型 掌握利用…
阅读更多...
English Learning - L3 作业打卡 Lesson2 Day14 2023.5.18 周四

English Learning - L3 作业打卡 Lesson2 Day14 2023.5.18 周四

English Learning - L3 作业打卡 Lesson2 Day14 2023.5.18 周四 引言&#x1f349;句1: A brown out is an expression for a reduction in electric power.成分划分弱读连读爆破语调 &#x1f349;句2: Brown outs happen when there is too much demand for electricity.成分…
阅读更多...
微服务之以nacos注册中心,以gateway路由转发服务调用实例(第一篇)

微服务之以nacos注册中心,以gateway路由转发服务调用实例(第一篇)

实现以nacos为注册中心,网关路由转发调用 项目版本汇总项目初始化新建仓库拉取仓库项目父工程pom初始化依赖版本选择pom文件如下 网关服务构建pom文件启动类配置文件YMLnacos启动新建命名空间配置网关yml(nacos)网关服务启动 用户服务构建pom文件启动类配置文件YML新增url接口配…
阅读更多...
行业分析——半导体行业

行业分析——半导体行业

半导体行业是现代高科技产业和新兴战略产业&#xff0c;是现代信息技术、电子技术、通信技术、信息化等产业的基础之一。我国政府先后制定了《中国集成电路产业发展规划》和《中国人工智能发展规划》&#xff0c;明确提出要支持半导体和人工智能等产业的发展&#xff0c;为半导…
阅读更多...
DRMS-关于开展防范风险整改工作的工作计划

DRMS-关于开展防范风险整改工作的工作计划

防范风险整改工作 工作计划 &#xff12;&#xff10;&#xff12;&#xff13;年&#xff15;月&#xff11;&#xff10;日 尊敬的【DRMS】集群用户&#xff1a; 根据河南省郑州市国家高新技术产业开发区市场监督管理局《关于河南数权数字信息科技研究院网络违规整改通知》及…
阅读更多...
微信小程序nodejs+vue高校食堂餐厅点餐订餐系统ja221

微信小程序nodejs+vue高校食堂餐厅点餐订餐系统ja221

本文以实际运用为开发背景&#xff0c;运用软件工程原理和开发方法&#xff0c;它主要是采用 语言 node.js 框架&#xff1a;Express 前端:Vue.js 数据库&#xff1a;mysql 数据库工具&#xff1a;Navicat 开发软件&#xff1a;VScode 前端vueelementui, (1) vue引入elementu…
阅读更多...
【IDEA使用码云教程】

【IDEA使用码云教程】

IDEA使用码云教程 一、下载、安装git二、配置Gitee插件三、克隆项目四、上传项目五、推送项目六、更新项目 一、下载、安装git 1.打开git官网&#xff0c;选择你的操作系统 官网下载地址&#xff1a;https://git-scm.com/downloads 2.根据你的系统位数选择相应的版本下载 系统…
阅读更多...
Spring Cloud Alibaba 集成 sentinel ，sentinel控制台不能检测到服务，但是在命令行配置启动参数就能看到服务

Spring Cloud Alibaba 集成 sentinel ，sentinel控制台不能检测到服务，但是在命令行配置启动参数就能看到服务

问题背景 Spring Cloud Alibaba 集成 sentinel &#xff0c;sentinel代码写的限流降级的功能都是好的&#xff0c;但是sentinel控制台不能检测到服务&#xff0c;在程序启动时配置JVM启动参数&#xff08;-Dcsp.sentinel.dashboard.serverlocalhost:18080 -Dproject.namename-…
阅读更多...
成绩管理系统

成绩管理系统

系列文章 任务28 成绩管理系统 文章目录 系列文章一、实践目的与要求1、目的2、要求 二、课题任务三、总体设计1.存储结构及数据类型定义2.程序结构3.所实现的功能函数4、程序流程图 四、小组成员及分工五、 测试读入数据浏览全部信息增加学生信息保存数据删除学生信息修改学生…
阅读更多...
【活动预告】数据集成引擎BitSail遇上CDC

【活动预告】数据集成引擎BitSail遇上CDC

BitSail是字节跳动开源数据集成引擎&#xff0c;于2022年10月26日宣布开源&#xff0c;可支持多种异构数据源间的数据同步&#xff0c;并提供离线、实时、全量、增量场景下全域数据集成解决方案。BitSail支撑了字节内部众多的业务线&#xff0c;支持多种数据源之间的批式/流式/…
阅读更多...
最新文章

Copyright @ 2022~2023