使用误差状态卡尔曼滤波器融合GPS和IMU，实现更高精度的定位

Related tags

Miscellaneous eskf-gps-imu-fusion
Overview

ESKF融合IMU与GPS数据

融合IMU数据之后的GPS轨迹效果

绿色轨迹：ground truth 蓝色轨迹：fuse IMU and GPS 红色轨迹：GPS

实现方法请参考我的博客《【附源码+代码注释】误差状态卡尔曼滤波(error-state Kalman Filter)实现GPS+IMU融合，EKF ESKF GPS+IMU》

1. 依赖库

Eigen

sudo apt-get install libeigen3-dev

Yaml

sudo apt-get install libyaml-cpp-dev

2. 编译

cd eskf-gps-imu-fusion
mkdir build
cd build
cmake ..
make

3. 运行

cd eskf-gps-imu-fusion/build
./gps_imu_fusion

4.轨迹显示

执行完./gps_imu_fusion会生成轨迹文件

cd eskf-gps-imu-fusion/data
evo_traj kitti fused.txt gt.txt measured.txt -p

需要安装evo，可以参考我的博客中的介绍

You might also like...
Comments
  • Correct函数中符号问题？

    Correct函数中符号问题？

    博主，你好，这份代码思路清晰，给了我很大的帮助。但还是有些地方不明白，在Correct函数中有如下两个问题： 1、Y_ = pose_.block<3,1>(0,3) - curr_gps_data.position_ned; 测量误差为什么是估计值减去测量值？ 2、计算输出结果时，EliminateError();函数中： pose_.block<3,1>(0,3) = pose_.block<3,1>(0,3) - X_.block<3,1>(INDEX_STATE_POSI, 0);//因为 velocity_ = velocity_ - X_.block<3,1>(INDEX_STATE_VEL, 0); Eigen::Matrix3d C_nn = Sophus::SO3d::exp(X_.block<3,1>(INDEX_STATE_ORI, 0)).matrix(); pose_.block<3,3>(0,0) = C_nn * pose_.block<3,3>(0,0); gyro_bias_ = gyro_bias_ - X_.block<3,1>(INDEX_STATE_GYRO_BIAS, 0); accel_bias_ = accel_bias_ - X_.block<3,1>(INDEX_STATE_ACC_BIAS, 0); 为什么是减号，我看《Quaternion kinematics for the error-state Kalman filter》这篇论文，说最终状态的输出是主成分分量加上误差项，但此处为什么是减号？

    opened by litingpan 0
  • 线速度转到角速度处理这里是不是一个bug？

    线速度转到角速度处理这里是不是一个bug？

    这里：https://github.com/zm0612/eskf-gps-imu-fusion/blob/main/src/eskf.cpp#L194

    `
    constexpr double rm = 6353346.18315; constexpr double rn = 6384140.52699; Eigen::Vector3d w_en_n(-velocity_[1] / (rm + curr_gps_data_.position_lla[2]), velocity_[0] / (rn + curr_gps_data_.position_lla[2]), velocity_[0] / (rn + curr_gps_data_.position_lla[2]) * std::tan(curr_gps_data_.position_lla[0] * kDegree2Radian)); Eigen::Vector3d w_in_n = w_en_n + w_;

    `

    w_en_n 的EU上的分量，是根据东向线速度转换的，它是平行于极轴的旋转分量，因此投影到EU上的分量，应该是： velocity_[0] / (rn + curr_gps_data_.position_lla[2]) * std::sin(curr_gps_data_.position_lla[0] * kDegree2Radian) 和 velocity_[0] / (rn + curr_gps_data_.position_lla[2]) * std::cos(curr_gps_data_.position_lla[0] * kDegree2Radian)

    opened by shockjiang 0
  • 该方法可能严重依赖速度的初值

    该方法可能严重依赖速度的初值

    • 当使用 init_velocity_ = curr_gps_data.true_velocity; 的时候，结果图如文档所示
    • 当将初始化速度设为0时， init_velocity_ = Eigen::Vector3d::Zero()，发现结果不是太好，偏差会有几十米

    不知道是我的用法问题，还是程序对初值依赖的原因。

    见图：

    image

    image

    opened by shockjiang 1
  • pdf version of blog post

    pdf version of blog post

    it is not easy for foreigners to read csdn blog post (https://blog.csdn.net/u011341856/article/details/114262451). it is good to include a PDF version of blog in repo

    opened by iceberg1369 0
Owner
null
GitHub