IMU和GPS融合定位（ESKF）

news2024/9/30 13:26:23

说明

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_);

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

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

三年测试，月薪才12k，想跳槽又不太敢.....

三年测试，月薪才12k，想跳槽又不太敢.....

在我们的身边&#xff0c;存在一个普遍现象&#xff1a;很多人从事软件测试岗&#xff0c;不计其数&#xff0c;经历的心酸难与外人道也。可是技术确难以提升、止步不前&#xff0c;薪资也只能看着别人水涨船高&#xff0c;自己却没有什么起色。 虽然在公司里属于不可缺少的一…
阅读更多...
java学习笔记

java学习笔记

java学习笔记 直接写出来的人可以理解的数据&#xff0c;在java中叫做字面量。 字面量分类&#xff1a; 数据类型分类&#xff1a; 不同的数据类型分配了不同的内存空间&#xff0c;不同的内存空间&#xff0c;所存储的数据大小是不一样的。 数据类型内存占用和取值范围…
阅读更多...
JavaSE入门必读篇——详解数组

JavaSE入门必读篇——详解数组

文章目录 数组的概念1.什么是数组呢&#xff1f;2.如何创建数组3.遍历数组4.扩展&#xff1a;快速批量初始化 数组原理内存图1. 内存概述2.Java虚拟机的内存划分3.其存储方式图4.认识null 二维数组二维数组初始化遍历二维数组 数组常见异常1. 数组越界异常2. 数组空指针异常 Ja…
阅读更多...
Windows下编译安装gRPC

Windows下编译安装gRPC

gRPC是Google基于HTTP/2协议开发的一套开源、跨平台的高性能RPC框架&#xff0c;可用于连接微服务架构内的各种服务&#xff0c;亦可以连接客户端与后端服务。 Ref. from gRPC gRPC is a modern open source high performance Remote Procedure Call (RPC) framework that can…
阅读更多...
代码随想录算法训练营第二十四天|理论基础 77. 组合

代码随想录算法训练营第二十四天|理论基础 77. 组合

文章目录 理论基础77.组合思路代码总结 理论基础 回溯算法&#xff1a;一种暴力搜索方式 回溯是递归的副产品&#xff0c;只要有递归就会有回溯。 回溯法&#xff0c;一般可以解决如下几种问题&#xff1a; 组合问题&#xff1a;N个数里面按一定规则找出k个数的集合切割问题…
阅读更多...
数据安全技术工作部成员动态 | 鸿翼联合天空卫士打造“基于内容的敏感信息处理”解决方案

数据安全技术工作部成员动态 | 鸿翼联合天空卫士打造“基于内容的敏感信息处理”解决方案

据2022年统计数据表明&#xff0c;因IT故障、人为错误、供应链攻击、破坏性攻击和勒索软件攻击等原因导致的数据泄露事件频繁发生&#xff0c;信息安全问题比以往任何一个时代都更为突出。信息泄漏造成的危害体现在多方面&#xff0c;不法分子通过各种途径收集的公司的某些重要…
阅读更多...
大数据法律监督模型优势特色及应用场景

大数据法律监督模型优势特色及应用场景

大数据法律监督平台是基于监督数据整合管理平台、监督模型构建平台、内置模型库以及法律监督线索管理平台打造的一套服务于检察机关法律监督工作的专业化系统。通过数据采集、融合、挖掘、建模、展现等一系列能力&#xff0c;辅助检察官从纷繁复杂的数据中&#xff0c;开展多维…
阅读更多...
基于可靠姿态图初始化和历史重加权的鲁棒多视图点云配准

基于可靠姿态图初始化和历史重加权的鲁棒多视图点云配准

论文作者 | Haiping Wang, Haiping Wang,Haiping Wang,etal 论文来源 | CVPR2023 文章解读 | William 1 摘要 之前的多视图配准方法依赖于穷举成对配准构造密集连接的位姿图&#xff0c;并在位姿图上应用迭代重加权最小二乘(IRLS)计算扫描位姿。但是&#xff0c;构造一个密集…
阅读更多...
产品经理必读丨如何找准产品定位？

产品经理必读丨如何找准产品定位？

我们都知道&#xff0c;当一款新产品开始立项之前&#xff0c;势必需要经过谨慎的市场调研才能整合资源启动新的项目&#xff0c;但除此之外&#xff0c;作为产品经理还需要做好一件关键的事情——找准产品在市场中的定位。 什么是产品定位 百度百科对产品定位的解释是非常准确…
阅读更多...
重磅新闻！！！ ChatGPT手机 app上线苹果app store

重磅新闻！！！ ChatGPT手机 app上线苹果app store

就在刚才&#xff0c;打开Tor访问openAI官网准备看看有什么新闻&#xff0c;结果吓我一跳啊&#xff01; &#x1f447;&#x1f447;&#x1f447; ChatGPT app上线APP store了&#xff01; 我马上拿出手机&#xff0c;一搜索&#xff0c;哎~出现了&#xff1a; chatgpt ios …
阅读更多...
想端起“铁饭碗”，你最好先学会这个！

想端起“铁饭碗”，你最好先学会这个！

正文共 886 字&#xff0c;阅读大约需要 3 分钟 公务员必备技巧&#xff0c;您将在3分钟后获得以下超能力&#xff1a; 快速生成推荐材料 Beezy评级 &#xff1a;B级 *经过简单的寻找&#xff0c; 大部分人能立刻掌握。主要节省时间。 ●图片由Lexica 生成&#xff0c;输入&a…
阅读更多...
Linux---cd命令、pwd命令、mkdir命令

Linux---cd命令、pwd命令、mkdir命令

1. cd命令 当Linux终端&#xff08;命令行&#xff09;打开的时候&#xff0c;会默认以用户的HOME目录作为当前的工作目录 我们可以通过cd命令&#xff0c;更改当前所在的工作目录。 cd命令来自英文&#xff1a;Change Directory 语法&#xff1a;cd [ linux路径] cd命令…
阅读更多...
OpenAI被曝将发布全新开源大模型，网友：GPT平替？

OpenAI被曝将发布全新开源大模型，网友：GPT平替？

来源 | 量子位 | 公众号 QbitAI OpenAI终于要“Open”了&#xff01; 最新爆料&#xff0c;他们正准备发布全新的开源语言模型。 GPT-2之后&#xff0c;这尚属四年来首次。 不少网友戳戳手表示期待&#xff1a;这是要发自己的开源平替了吗&#xff1f; 毕竟目前最好的开源…
阅读更多...
单片机--SPI协议

单片机--SPI协议

目录 【1】SPI协议 1.SPI协议 2.SPI时序 【2】LCD液晶显示屏 【3】点亮LCD显示屏 图片显示 汉字显示 【1】SPI协议 1.SPI协议 SPI(Serial Peripheral Interface)是 摩托罗拉公司(Motorola)首先提出的全双工同步串行外设接口&#xff0c;采用主从模式&#xff08;Master、…
阅读更多...
webpack核心原理

webpack核心原理

背景 Webpack 特别难学&#xff01;&#xff01;&#xff01; 时至 5.0 版本之后&#xff0c;Webpack 功能集变得非常庞大&#xff0c;包括&#xff1a;模块打包、代码分割、按需加载、HMR、Tree-shaking、文件监听、sourcemap、Module Federation、devServer、DLL、多进程等…
阅读更多...
传感云智慧公厕综合解决方案，实现公厕精细化管理

传感云智慧公厕综合解决方案，实现公厕精细化管理

随着国家“公厕革命”建设工作的持续推动与科技创新技术的不断进步&#xff0c;人们对“方便”与“卫生”的要求越来越高&#xff0c;智慧公厕已然成为智慧城市建设规范下的公共厕所新形态&#xff0c;不仅可以解决传统公厕的脏乱差、异味和管理难题&#xff0c;同时可以为用户…
阅读更多...
前端 之 FormData对象进阶

前端 之 FormData对象进阶

一、进阶知识 在前一篇博客中&#xff0c;我讲解了FormData对象的基础概念、相关方法和基本用法&#xff0c;本篇博客我将讲解一些FormDate对象相关的进阶知识&#xff0c;主要包含FormData与其他对象结合使用的各类场景&#xff0c;以及一些使用技巧。 1、FormData对象、JSO…
阅读更多...
在一个不小但很美的公司里工作

在一个不小但很美的公司里工作

在这个公司里学到了什么 电商交易前业务&#xff1a;商品&#xff0c;库存&#xff0c;物流&#xff0c;会员 &#xff1b; 电商广告部分业务&#xff1a; 网红&#xff0c;联盟&#xff1b; 并且对这些业务里的核心流程&#xff0c;核心技术 有过总结。核心技术问题采用对应…
阅读更多...
HTTP.sys远程代码执行

HTTP.sys远程代码执行

本文转载与&#xff1a;https://blog.csdn.net/weixin_47723270/article/details/129472716 01 漏洞描述 HTTP.sys是Microsoft Windows处理HTTP请求的内核驱动程序&#xff0c;为了优化IIS服务器性能&#xff0c;从IIS6.0引入&#xff0c;IIS服务进程依赖HTTP.sys。HTTP.sys远程…
阅读更多...
keycloak异常关闭报错username ‘admin‘ already added时卡死无法重启的问题处理

keycloak异常关闭报错username ‘admin‘ already added时卡死无法重启的问题处理

问题现象 使用docker部署keycloak服务&#xff0c;使用docker-compose进行配置管理&#xff0c;配置如下&#xff1a; keycloak:image: jboss/keycloak:16.1.0 container_name: keycloakcommand:[-b,0.0.0.0,-Dkeycloak.migration.actionimport,-Dkeycloak.migration.provider…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023