程序员经验分享-hwhale

IMU与GPS传感器ESKF融合定位

2023-05-16

IMU与GPS传感器ESKF融合定位

文章目录

  • IMU与GPS传感器ESKF融合定位
    • 1、代码整体框架说明
    • 2、主要函数介绍
      • 2.1 LocalizationWrapper构造函数
      • 2.2 滤波算法进行预测
      • 2.3 通过GPS位置测量数据更新系统的状态
    • 3. 结果评价
    • 参考文献：

在这里插入图片描述
分别在两个终端下运行命令： 

rosbag play utbm_robocar_dataset_20180502_noimage1.bag
roslaunch imu_gps_localization imu_gps_localization.launch

1、代码整体框架说明

代码具体结构框架如下图所示：
在这里插入图片描述

cpp文件文件具体实现作用备注
localization_node.cpp主函数文件，实现初始化ros，初始化LocalizationWrapper类
localization_wrapper.hLocalizationWrapper类定义，主要声明构造函数、析构函数以及imu回调函数、gps回调函数、打印状态、打印gps数据，把状态格式数据转换成ros话题机制
localization_wrapper.cpp构造函数中主要是加载参数，保存数据到文件夹下，订阅imu和gps的话题然后发布融合后的轨迹；析构函数主要是释放空间；imu回调函数主要是读取imu数据，然后对数据进行处理，转变状态发布成ros话题，打印状态；gps回调函数主要是实现读取数据，然后进行处理，最后打印。
imu_gps_localizer.cppProcessImuData()函数初始化imu数据，对imu数据进行预测，从ENU坐标系转换到GPS坐标系，发布融合后的状态；ProcessGpsPositionData()函数主要实现初始化数据以及位置的更新。
imu_gps_localizer.hImuGpsLocalizer类构造函数，声明ProcessImuData()函数，声明ProcessGpsPositionData()函数
initializer.h初始化类，主要是构造函数，添加imu数据，添加gps数据，计算imu数据到gps数据的旋转矩阵这里参考的是openvins
imu_processor.cpp滤波融合的预测部分Predict()函数具体实现
imu_processor.hImuProcessor类主要是滤波融合的预测部分Predict()函数声明
gps_processor.hGpsProcessor类通过GPS位置更新系统的状态UpdateStateByGpsPosition()以及计算残差以及雅可比矩阵和把得到的误差更新状态
base_type.h主要是imu数据、GPS数据以及系统状态的数据结构
utils.h主要包括弧度和角度之间的相互转换、ENU坐标系与LLA坐标系的转换以及得到反对称矩阵

2、主要函数介绍

2.1 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);
}

2.2 滤波算法进行预测

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;
}

2.3 通过GPS位置测量数据更新系统的状态

bool GpsProcessor::UpdateStateByGpsPosition(const Eigen::Vector3d& init_lla, const GpsPositionDataPtr gps_data_ptr, State* state) {
    Eigen::Matrix<double, 3, 15> H;
    Eigen::Vector3d residual;
    ComputeJacobianAndResidual(init_lla, gps_data_ptr, *state, &H, &residual);
    const Eigen::Matrix3d& V = gps_data_ptr->cov;

    // EKF.
    const Eigen::MatrixXd& P = state->cov;
    //P：状态的协方差矩阵；V：gps数据的协方差矩阵；H：误差状态的雅可比矩阵
    const Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + V).inverse();
    const Eigen::VectorXd delta_x = K * residual;

    // Add delta_x to state.
    AddDeltaToState(delta_x, state);

    // Covarance.
    const Eigen::MatrixXd I_KH = Eigen::Matrix<double, 15, 15>::Identity() - K * H;
    state->cov = I_KH * P * I_KH.transpose() + K * V * K.transpose();
}
//计算雅克比矩阵以及残差项 P61 Quaternion kinematics for the error-state Kalman filter
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.
    *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_);
}
//添加误差项到状态
void AddDeltaToState(const Eigen::Matrix<double, 15, 1>& delta_x, State* state) {
    state->G_p_I     += delta_x.block<3, 1>(0, 0);
    state->G_v_I     += delta_x.block<3, 1>(3, 0);
    state->acc_bias  += delta_x.block<3, 1>(9, 0);
    state->gyro_bias += delta_x.block<3, 1>(12, 0);

    if (delta_x.block<3, 1>(6, 0).norm() > 1e-12) {
        state->G_R_I *= Eigen::AngleAxisd(delta_x.block<3, 1>(6, 0).norm(), delta_x.block<3, 1>(6, 0).normalized()).toRotationMatrix();
    }
}

3. 结果评价

参考文献：

IMU & GPS定位 Quaternion kinematics for ESKF[part 3]

EU Long-term Dataset with Multiple Sensors for Autonomous Driving

Quaternion kinematics for the error-state Kalman filter

Woosik Lee, Intermittent GPS-aided VIO: Online Initialization and Calibration.

A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors

A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

IMU

GPS

ESKF

融合定位

IMU与GPS传感器ESKF融合定位 的相关文章

  • 如何从Python中的GPS不分段时间获取当前日期和时间

    我有这样的 GPS 未分段时间 Tgps 1092121243 0 我想知道那是什么日期和时间 GPS时间的起始时间是1980年1月6日 Python函数 datetime utcfromtimestamp 可以给出 1970 年 1 月

  • 东向北转纬度经度

    我有东向 北向格式的位置坐标 但我需要将其转换为正确的经纬度 以使其在 bing 地图中居中 有任何公式或详细信息如何将东距 北距转换为纬度 经度吗 编辑 更具体地说 我需要将 SVY21 坐标转换为 WGS84 东距和北距分别是基点向东和

  • 我无法在 Android 真实手机上获取位置

    public class LocationService extends Service private Handler mHandler new Handler private Timer mTimer null private int

  • Android 手机和模拟器中的mapView不同

    关于应用程序 这是一个简单的应用程序 可以查找用户当前位置 问题 该应用程序在模拟器上运行良好 请参见图片 但在手机中它没有显示MapView 请看图片 请告诉我手机出了什么问题 在手机中 它只下载巨大的 20 MB 数据 但不显示实际地图

  • 如何在用户行走时跟踪 GPS 坐标，来自 iOS 平台的 xamarin.forms

    我刚刚经历过这个link http developer xamarin com recipes ios multitasking track significant location change 当用户使用 xamarin forms 应

  • Android 将阿拉伯数字转换为英文数字

    我从 GPS 收到以下错误 Fatal Exception java lang NumberFormatException Invalid double 现在 这是我通过 Fabric 从用户处收到的错误 它看起来像阿拉伯语 所以我猜只有当

  • Android GPS 路由系统

    我正在开发一个 Android 应用程序 它可以定位用户 假设用户在路上 并且该应用程序将为用户创建最短路线 以便能够到达用户选择的目的地 定位用户不是问题 因为这里有一个 API http www vogella com articles

  • C#：GPS跟踪系统[关闭]

    Closed 这个问题需要多问focused help closed questions 目前不接受答案 如何在 C net 中构建带有移动设备 带 GPS 的 GPS 跟踪系统 场景是 通过支持 GPS 的手机跟踪用户 服务工程师 这里没

  • GPS坐标：一个点周围1平方公里

    我希望有人能给我提供一个方程来计算给定点周围 1 公里的平方 X 从 a aaa 到 b bbb Y 从 c ccc 到 c ccc 例如lat 53 38292839 and lon 6 1843984 我还需要围绕一个点 2 公里 5

  • 设置模拟位置时 GPS 提供商未知错误？

    我正在尝试设置我的模拟位置 但是 我收到以下错误 提供商 gps 未知 并且不确定出了什么问题 我已经获得了在manifest xml 中声明的所有权限以及所有参数 模拟定位法 Initiates the method to set the

  • Android GPS 的准确度如何？ [关闭]

    Closed 这个问题是无关 help closed questions 目前不接受答案 我好像在某处读过Android的GPS精度约为10厘米 任何人都可以验证或更正这个吗 原因是我正在尝试开发的应用程序会跟踪用户访问过的位置 这将极大地

  • Google 地图 (Android) 中的位置更新率

    我正在编写一个简单的基于 GPS 的应用程序 用于位置感知 每当启用 GPS 时 应用程序都会请求位置更新并以格式打印纬度和经度 TextView 如果 GPS 被禁用 位置提供商会回退到LocationManager NETWORK PR

  • C# - LINQ - GPS 纬度和经度的最短距离

    我有数据库 其中有带有 GPS 坐标的一流酒店 我想获得距离我选择的坐标最近的地方 我认为它应该看起来像这样 我在这里找到了很多示例代码 就像这样 var coord new GeoCoordinate latitude longitude

  • 移动应用程序在后台时的 GPS 位置（使用 ionicframework）

    我需要实现一个应用程序来存储用户从 A 移动到 B 时的旅程 路径 现在 我知道 ionicframework 可以使用 GPS 但是当我的应用程序转到后台时会发生什么 我的应用程序如何继续存储用户位置 这可能吗 有没有我可以使用的插件 请

  • 使用纬度/经度计算从 A 点到线段的距离

    我正在开发一个使用 GPS 的 Android 应用程序 我想知道如果 新位置 C 点 距离线段 AB 太远 是否有办法可以丢弃 GPS 位置数据 我正在使用发现的点到线段公式在维基百科上 http en wikipedia org wik

  • Android：计算两个位置之间距离的最佳方法

    我在这个主题上做了一些研究 但有很多观点并没有给出一个清晰的图像 我的问题是这样的 我正在为 Android 开发一个基于 GPS 的应用程序 在其中我想实时了解 Android LocationManager 指定的当前位置与其他位置之间

  • 谷歌地图定位是如何工作的？

    我的问题是谷歌地图或移动 GPS 如何找到我的当前位置 读完本文后我的高层次理解article http www physics org article questions asp id 55就是 GPS接收器通过这些卫星获取位置坐标 该位

  • 如何在 Android 中像 Google 地图一样获得持续的位置更新？

    我正在构建一个朋友跟踪 Android 应用程序 当我的朋友激活应用程序并带着他的 GPS 和蜂窝数据离开时 我需要在我的设备上跟踪他 这就是这个概念 我已经实现了 LocationListener 类 现在我可以从 Gps 或网络获取最后

  • 检测wifi是否启用（无论是否连接）

    对于 GPS 跟踪应用程序来说 在打开 WIFI 的情况下记录位置信号会导致数据非常不精确或存在间隙 在开始跟踪之前 我已使用可达性查询来检测 wifi 是否可用 问题是 如果进行该查询时 wifi 已启用但未连接到网络 则表明无法通过 w

  • 如何创建在 React-Native 中检测自动位置的地图

    我已经在react native中创建了地图 参考https github com lelandrichardson react native maps https github com lelandrichardson react nat

随机推荐

  • vue3 使用 swiper轮播库

    文章目录 前言一 Swiper引入方式1 HTML标签引入方式2 CommonJs引入方式3 ES引入方式 xff08 采用 xff09 二 使用Swiper总结 前言 轮播图在前端开发中 xff0c 是常见需求 而Swiper库是最受前端

  • vue-cli2 老项目webpack3升级webpack5过程总结

    文章目录 背景一 webpack5环境要求二 升级步骤1 脚手架webpack cli2 升级webpack包3 更新webpack相关插件3 1 不推荐或被移除的插件3 2 升级babel到7版本3 3 更新插件 4 修改配置4 1 新增

  • 前端下载文件

    文章目录 前言二进制流前端核心实现下载功能有 xff1a 一 a标签 43 download属性二 window open url 34 blank 34 三 form表单四 接口请求 43 blob 43 a标签 43 download属

  • 前端JS 云打印 LODOP实践

    文章目录 前言一 Lodop是什么 xff1f 二 如何使用Lodop1 下载打印插件2 配置打印机3 html中植入打印控件4 调用Lodop对应的JS相关方法接口实现打印功能 三 Lodop主要方法接口三 注意点总结 前言 一般B S系

  • axios源码——工具函数utils.js

    文章目录 前言一 工具函数所在目录二 判定数据类型的函数1 isArray 判定数组 2 isString 判定字符串 3 isNumber 判定数值 4 isObject 判定对象 5 isPlainObject 判定纯对象 6 isUn

  • 源码阅读——validate-npm-package-name

    文章目录 前言一 源码阅读工具二 阅读源码1 目录结构2 package json3 index js 三 使用该包1 vue cli中使用2 create react app 中使用 总结 前言 validate npm package

  • 学习rtklib

    数据下载 日期转换和一些常用数据下载 http www gnsscalendar com index html year 61 2019 多系统精密星历和精密钟差下载 2021年10月25日更新 xff1a 单GPS精密星历文件要在这里下载

  • echarts 绘制多条折线图（横坐标，折线图条数不确定）

    项目场景 xff1a 使用echarts做业务图表统计 xff0c 记录一下在项目中图表接口返回的数据处理问题 需求描述 1 一个统计图中实现多条折现图的显示 xff0c 如下图所示 2 后台返回的数据结构 span class token

  • 线性二次型调节器(LQR)原理详解

    文章目录 前言算法解释代价函数的意义 推导过程可控性LQR控制实例参考资料 前言 LQR Linaer Quadratic Regulator xff0c 即线性二次型调节器 xff0c 是一种现代控制理论中设计状态反馈控制器 State

  • 嵌入式软件工程师常见面试问题

    嵌入式软件工程师面试题 1 stm32启动方式 xff1f 有三种 xff1a 从Flash启动 xff0c 将Flash地址0x0800 0000映射到0x00000000 这样启动以后就相当于从0x0800 0000开始的 xff0c

  • 使用python爬虫把自己的CSDN文章爬取下来并保存到MD文件

    导言 爬虫作为一个敏感技术 xff0c 千万要把握好 xff0c 如果人家不让爬那就不要头铁去试了 如何确定某个网站是否允许爬虫 在域名后面加上 robots txt查看即可 xff0c 例如 xff1a https blog csdn n

  • 寻找矩阵中的指定路径

    给定一个矩阵和一个要找的字符串 xff0c 如果有的话找出矩阵中的字符串路径 测试用例 功能测试 xff1a 在多行多列的矩阵中存在或者不存在路径 边界值测试 xff1a 矩阵只有一行或一列 xff1b 矩阵和路径中的所有字母都是相同的 特

  • Kalibr标定Intel D435i相机

    Kalibr标定Intel D435i相机 文章目录 Kalibr标定Intel D435i相机1 相机标定2 IMU标定3 相机 43 IMU联合标定4 标定结果评价 系统环境 xff1a ubuntu16 04 43 roskineti

  • VINS初始化

    VINS初始化 VINS初始化之外参在线标定 前面主要分析了外参标定出来旋转矩阵 xff0c 接下来接着分析初始化 if solver flag 61 61 INITIAL if frame count 61 61 WINDOW SIZE

  • ROS中关于两个话题时间同步遇到的问题 message_filters

    ROS中关于两个话题时间同步遇到的问题 message filters 参考链接 CMakeFiles imu data dir src imu data cpp o xff1a 在函数 message filters Synchroniz

  • ROS之多传感器融合算法实现

    ROS之多传感器融合算法实现 文章目录 1 motivation2 method2 1 订阅ROS的多个话题并对数据进行处理2 2 订阅ROS的多个话题并发布成一个话题 1 motivation IntelRealsenseD435i传感器

  • ignav代码阅读笔记

    整个代码还是根据rtklib进行改的 xff0c 功能很完善 xff0c 但是我主要只关注ppp ins紧组合 代码链接 https github com Erensu ignav 代码功能 可以完成ppp和ins的紧组合 xff0c 把c

  • 【泡泡Docker乐园】使用泡泡Docker基础镜像放心大胆地开发吧！

    泡泡Docker乐园 使用泡泡Docker基础镜像放心大胆地开发吧 xff01 2020 4 7 泡泡推广 amp 编辑组 泡泡Docker乐园 xff0c 带你进入Docker的狂欢派对 简介 xff1a 泡泡Docker乐园 本次将推出

  • 【泡泡Docker乐园】Dockerfile简易教程 & LARVIO镜像

    泡泡Docker乐园 Dockerfile简易教程 amp LARVIO镜像 亲测完美 简介 xff1a 泡泡Docker乐园 第二期来啦 xff01 本期我们将简要介绍使用Dockerfile进行image构建的方法 利用Dockerfi

  • IMU与GPS传感器ESKF融合定位

    IMU与GPS传感器ESKF融合定位 文章目录 IMU与GPS传感器ESKF融合定位1 代码整体框架说明2 主要函数介绍2 1 LocalizationWrapper构造函数2 2 滤波算法进行预测2 3 通过GPS位置测量数据更新系统的状

热门标签