zm0612 / eskf-gps-imu-fusion Goto Github PK

楼主你好，
看了你的博客，受益匪浅；这里有个问题想请教一下；
我想使用encoder轮式里程计和imu进行融合，获取一个DR推导；请问有什么建议吗？
目前没有想好，该用哪些做预测，哪些做观测？
encoder反馈有位姿pose，线速度，角速度；imu反馈有线加速度和角速度，
感谢！

您好 ，我看到您对通过imu和gps传感器模拟定位有很多经验 ，我有一个问题很困扰想从您这得到帮助。
我是游戏开发工程师 ，希望通过手机的imu和gps模拟手机的姿态和位置出现在unity(游戏引擎)中的地图上行驶。 但是我根据imu的横摆角来模拟时和gps经纬发生了很大的偏离 ，您是否有使用gps来纠正imu yaw角 来修正朝向的经验？ 我希望能够及时的响应imu的朝向 ，但不要偏离gps位置太多 。

黄色箭头是gps位置，红色箭头是根据imu航向和速度模拟的，朝向偏差导致分道扬镳了。

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

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

见图：

博主，你好，这份代码思路清晰，给了我很大的帮助。但还是有些地方不明白，在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》这篇论文，说最终状态的输出是主成分分量加上误差项，但此处为什么是减号？

这里：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)

您好，非常感谢您分享的优秀项目。我们在自己的工程上实现了非常好的应用效果，但是在系统初始化阶段，轨迹是会有一定漂移的，那我们该如何去判断ESKF达到了收敛的状态，也就是说当前融合的轨迹趋于稳定了呢？

eskf.cpp(当前主分支192行)，第一分量用了Rn，第二分量用了Rm，这两个曲率半径是不是写反了？

博主您好，正如您所说的“每次使用卡尔曼迭代完成之后，都需要将最终得到的误差，重新带回到预测的位置、速度、方向中消除掉这个误差”，为何在消除偏差的过程中使用的是加号而不是减号，如：velocity_ = velocity_ + X_.block<3, 1>(INDEX_STATE_VEL, 0);
我还有一个问题：在SO3Exp函数中，将等效旋转矢量转换为旋转矩阵，为何不是常见的罗德里格斯公式。
我是才开始研究这一领域的萌新，还请博主不吝赐教（可能问题比较愚蠢）

测量数据.zip

void ErrorStateKalmanFilter::ComputePosition(const Eigen::Vector3d &last_vel, const Eigen::Vector3d &curr_vel,
                                             const IMUData &imu_data_0,
                                             const IMUData &imu_data_1) {
    double delta_t = imu_data_1.time - imu_data_0.time;

    pose_.block<3, 1>(0, 3) += 0.5 * delta_t * (curr_vel + last_vel) +
                               0.25 * (imu_data_0.linear_accel + imu_data_1.linear_accel) * delta_t * delta_t;
}

是不是应该改为

g_ = [0,0,-.9.8]
pose_.block<3, 1>(0, 3) += 0.5 * delta_t * (curr_vel + last_vel) + 0.25 * (imu_data_0.linear_accel - g_+ imu_data_1.linear_accel - g_) * delta_t * delta_t;

这里是不是写错了呀

您好，感谢您分享的开源ESKF工程，我打算用ros实现该工程，对于博客中提到的gt真值，请问是如何提供的，是依靠仿真数据吗，有没有在实物上做过gt的采集和测试，我们打算用自己的RTK设备试试。

感谢大佬的工作，有代码也有解释。
为了进一步让更多做IMU GPS SLAM Robot的同学能够了解并使用ESKF
可不可以出一个ROS的版本，数据不使用gins-sim的仿真数据，而是使用不同人自己录制的bag数据（/imu, /gps）
感谢🙏

如题。

另外，对于普通双频GPS模块，非差分类型，适合做eskf吗？ 普通双频GPS的定位精度相对于来说比较差一些，开放天空1.x米的定位精度水平。

请问: 我考虑直接将GPS数据转化到UTM坐标系, 但是fuse.txt的结果跑飞了, gt.txt没问题
可能
1 是与init_pose_有关系,
2 是UTM坐标系 是投影导致的?

谢谢

您的介绍里面说GPS的经纬高是NED坐标系是什么意思?ECEF坐标系,就是经纬高讶?

【一】F、B矩阵的推导问题

请问UpdateErrorState()中的jaccobian矩阵的推导是如何来的？
源代码中的实现为:

F_.block<3, 3>(INDEX_STATE_POSI, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity();
F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ORI) = F_23;
F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_ORI) = F_33;
F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = pose_.block<3, 3>(0, 0);
F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_GYRO_BIAS) = -pose_.block<3, 3>(0, 0);

B_.block<3, 3>(INDEX_STATE_VEL, 3) = pose_.block<3, 3>(0, 0);
B_.block<3, 3>(INDEX_STATE_ORI, 0) = -pose_.block<3, 3>(0, 0);

但是参考《Quaternion kinematics for the error-state Kalman filter》一文中的推导，实现应该修改如下（忽略地球自转影响）：

F_.block<3, 3>(INDEX_STATE_POSI, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity();
F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ORI) = -pose_.block<3, 3>(0, 0) * BuildSkewSymmetricMatrix(ComputeUnbiasAccel(curr_imu_data.linear_accel));
F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_ORI) = -BuildSkewSymmetricMatrix(ComputeUnbiasGyro(curr_imu_data.angle_velocity));
F_.block<3, 3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = -pose_.block<3, 3>(0, 0);
F_.block<3, 3>(INDEX_STATE_ORI, INDEX_STATE_GYRO_BIAS) = -Eigen::Matrix3d::Identity();

B_.block<3, 3>(INDEX_STATE_VEL, 3) = Eigen::Matrix3d::Identity();
B_.block<3, 3>(INDEX_STATE_ORI, 0) = Eigen::Matrix3d::Identity();

【二】姿态角的EliminateError问题

同上，EliminateError()源代码相关的实现为：

Eigen::Matrix3d C_nn = SO3Exp(-X_.block<3, 1>(INDEX_STATE_ORI, 0));
pose_.block<3, 3>(0, 0) = C_nn * pose_.block<3, 3>(0, 0);

同样参考《Quaternion kinematics for the error-state Kalman filter》一文中的公式，实现应该修改如下：

pose_.block<3, 3>(0, 0) = pose_.block<3, 3>(0, 0) * SO3Exp(X_.block<3, 1>(INDEX_STATE_ORI, 0));

【三】疑问

1. 以上两处源代码的推导过程是如何来的？是否有错误？
2. 实验有趣地发现，问题一、二均使用保留源代码的实现，或者均使用修改后的实现，都能得到正确一样的结果。请教原因。

首先在eskf.cpp中updateOdomEstimate中，是由于Eigen库中的数据的截断误差，导致 在eskf.cpp中的R_nm_nm_1不完全正交，在这里我做了较多的尝试，使用svd分解等方法都不能得到一个较好的结果，因此我直接使用了inverse()，虽然运算速度上有损失，但是毕竟效果会更好一点。
第二个部分是eskf.cpp中关于角速度更新的部分，在ComputerNavigationFrameAngularVelocity中，对于纬度和高度直接使用了卫星的真值，这里当然是可以的，但是对于纯积分来说，没有GPS，因此最好加一个if的判断，没有GPS，直接使用上一时刻的imu的数据。（目前正在修改中）
第三个部分是在eskf.cpp中ComputePosition中，关于惯性比力方程，完整的公式需要扣除地球自转引起的加速度与对地向心加速度，但是在unbias_accel_0与unbias_accel_1求解的过程中，均采用了简化模型，这里可能有精度损失。我的公式和您的一样，就是您的博客“https://blog.csdn.net/u011341856/article/details/132948044#comments_31875260”中“2.速度更新a. 精确速度微分建模”下的模型。

GPS数据：
True GPS position/velocity, ['rad', 'rad', 'm', 'm/s', 'm/s', 'm/s'] for NED (LLA)

imu数据：
'ref_gyro' True angular velocity in the body frame, units: ['rad/s', 'rad/s', 'rad/s']
'ref_accel' True acceleration in the body frame, units: ['m/s^2', 'm/s^2', 'm/s^2']
The body coordinate system is fixed at the device. Its origin is located at the center of device. Its x axis points forward, lying in the symmetric plane of the device. Its y axis points to the right side of the device. Its z axis points downward to complete the right-hand coordinate system.

目标看起来是要都转换到ENU坐标系下。
但gps数据中的velocity 是NED 坐标系的，而imu数据Body frame的，为何他们可以共用一个TransformCoordinate转换函数呢？

eskf.cpp(当前主分支150行)，计算R_nm_nm_1时，用angle_axisd对应的旋转矩阵的transpose，210行在使用R_nm_nm_1时，又transpose了一下，这是误操作？还是姿态解算方程使然？

<>
这篇文章,我查到了很多版本,方便您分享一下这个文章的DOI号么?
另外想请教的是Observability是观测性分析还是观测度分析?如果是前者,后者的文章,您有推荐么?

如题，可以使用ros订阅发布话题的形式，使用这套开源代码吗？

当前主分支eskf.cpp第12行，g_初始化为(0，0，-9.78)，是不是在NED系下应该初始化为正的9.78，ENU下才是-9.78吧？第221、222行程序里用的是减去g_，反而结果是对的:)

[我按照您的运行一遍，没有报错，编译成功，然后./gps_imu_fusion后没有任何反应，就一直卡在那里一直在跑程序，没有数据输出，也不会结束。[

如题。

看代码中也对gps_data.velocity 执行转换到导航坐标系了。
TransformCoordinate(gps_data.velocity);

in file src\gps_flow.cpp

姿态初始化中，作者按照Z、Y、X的顺序对姿态进行初始化，这里是否应该把顺序改为X、Y、Z。因为代码中作者将代表该姿态的旋转矩阵用作从载体坐标系到导航坐标系的变换矩阵，如果顺序是Z、Y、X，该旋转矩阵则是从导航坐标系到载体坐标系（代码中由于初始姿态和北东地坐标系重合，不影响结果）。

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

ini lat (deg),ini lon (deg),ini alt (m),ini vx_body (m/s),ini vy_body (m/s),ini vz_body (m/s),ini yaw (deg),ini pitch (deg),ini roll (deg)
32,120,0,2,0,0,0,0,0
command type,yaw (deg),pitch (deg),roll (deg),vx_body (m/s),vy_body (m/s),vz_body (m/s),command duration (s),GPS visibility
1,0,5,0,0,0,0,10,1
1,0,0,0,0,0,0,10,1
1,0,-5,0,0,0,0,10,1
1,0,0,0,0,0,0,10,1
1,0,5,0,0,0,0,10,1
1,0,0,0,0,0,0,10,1
1,0,-5,0,0,0,0,10,1
1,0,0,0,0,0,0,10,1
1,0,0,0,0,0,0,10,1

您好，我从RTK上获取到相应的GPS数据和IMU加速度和角速度信息，RTK数据为真值，由此创建相应的文件信息。但是仿真结果不容乐观，请问该怎么修改呢？RKT [IMU的X轴朝车辆右方，Y轴朝车辆前方，Z轴朝上，输入时已经做了坐标转换。

如下图，
我把您的方法试着移植到了ROS上，实时播放制作的rosbag。
数据前半段轨迹很正常，突然就变得离谱了...
对照了一下过程基本都是照搬了，就想问下您调的时候有遇到过这种情况吗？