3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统

1. 公式推导

2. GNSS+IMU+Odom融合定位

程序实现以及运行效果：
①首先是在预积分程序中记录了预积分积累的IMU数据个数（用来保证相邻两关键帧之间要有IMU预积分量来做约束）


②然后，参考addGnss部分代码修改addOdom部分代码：


运行效果：


上图显示开始一段轨迹平滑性不好，应该是odom与GNSS之间的IMU数据限制较少导致的，下面是修改了一下阈值，平滑性明显转好。

3. 利用数值求导工具，验证本书实验中的雅可比矩阵的正确性

利用g2o的自动求导功能来验证解析解对不对。

运行终端打印结果：

4. 也欢迎大家来我公众号读书–“过千帆”