关于SOPExp函数和偏差消除

[nimendafeige]

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

[zm0612]

你好

1. velocity = velocity + X_.block<3, 1>(INDEX_STATE_VEL, 0);这个是因为建模方式导致的，建模的时候认为 真值 = 有误差的值 + 预测的误差，所以在修整的时候，也是按照加法去修正。
2. SO3Exp函数，确实是将等效旋转矢量转换成为旋转矩阵，它内部的实现就是罗德里格斯公式。可以拿代码和公式对一下，是一模一样的。

[nimendafeige]

感谢博主的耐心回答，我还有个问题想请教下：在消除速度偏差和消除重力时，操作为R（accel - accelbias - g_），上述步骤是如何考虑的。我的理解是先消除速度偏差，然后再进行坐标系变换，最后再减去重力，即R (accel - accelbias )-g_。

[zm0612]

看了一下，我这里好像确实写错了，非常抱歉，R（accel - accelbias - g_）这个是不合理，accel是body坐标系下的，只有先转到导航系才能去和重力加速度g_进行运算，你这个方式R (accel - accelbias )-g_是对的。由于我的仿真运动是在水平面上的，所以这个问题并没有在我的数据上显示出问题。

代码目前已经修复，尝试拉取新的代码试一下。 @nimendafeige

[nimendafeige]

感谢博主的及时回复，当我使用mems的IMU做实车试验时，如果不用GPS的数据对INS的结果进行修正，INS的结果反而是使用公式R（accel - accelbias - g_）时效果更好，使用R (accel - accelbias )-g_时效果较差，我可以将我的原始数据发给您，您帮我看一看这样的INS是否正常嘛

[nimendafeige]

raw_data_real_20231008_3.zip raw_data_real_20231008_3_withbias.zip 上述数据均已转换为博主所用的数据格式，raw_data_real_20231008_3是指我平衡掉车辆静止时IMUx、y轴零偏之后的数据，raw_data_real_20231008_3是指原始数据

[zm0612]

hello @nimendafeige，发过来的数据，我已经测试和复现了，确实存在异常，主要原因是你的数据文件不够全，导致gps的参考点读取的时候出现了问题。

但是不需要担心，为了避免这个问题，使你能正常使用，我已经在代码中修改了一部分逻辑，你拉取代码就可以正常使用了。

我大致根据你的数据稍微调整了一些卡尔曼的参数，得到了如下的效果：

我的参数大致调整如下，你可以进一步去尝试细致调整，观看效果。

# imu sensor noise
gyro_noise_std: 1.0e-1
accelerometer_noise_std: 1.0e-1

# kalman measurement process std (uint: m)
gps_position_x_std: 0.5
gps_position_y_std: 0.5
gps_position_z_std: 1.0

进一步去尝试调整config中的参数，应该可以得到更好的结果！ https://github.com/zm0612/eskf-gps-imu-fusion/blob/main/config/config.yaml

如有问题 欢迎继续讨论。

[nimendafeige]

非常感谢您一直以来的耐心帮助和指导，我想请问您是否方便添加我的微信nimenfeifeige，使得我能够更方便地请教您一些问题，如果真能如愿，我将不胜感激。