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

目录

  • 1. 公式推导
  • 2. GNSS+IMU+Odom融合定位
  • 3. 利用数值求导工具,验证本书实验中的雅可比矩阵的正确性
  • 4. 也欢迎大家来我公众号读书--“过千帆”

1. 公式推导

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

2. GNSS+IMU+Odom融合定位

3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第2张图片
程序实现以及运行效果:
①首先是在预积分程序中记录了预积分积累的IMU数据个数(用来保证相邻两关键帧之间要有IMU预积分量来做约束)
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第3张图片
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第4张图片
②然后,参考addGnss部分代码修改addOdom部分代码:
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第5张图片
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第6张图片
运行效果:
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第7张图片
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第8张图片
上图显示开始一段轨迹平滑性不好,应该是odom与GNSS之间的IMU数据限制较少导致的,下面是修改了一下阈值,平滑性明显转好。
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第9张图片
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第10张图片

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

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

3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第11张图片
3. 【自动驾驶和机器人中的SLAM技术】实现基于预积分和图优化的GNSS+IMU+Odom的融合定位系统_第12张图片
运行终端打印结果:

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

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

你可能感兴趣的:(SLAM学习,自动驾驶,机器人,人工智能)