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

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