作者 | 半闲居士 编辑 | 汽车人
原文链接：https://www.zhihu.com/answer/3127818634
点击下方卡片，关注“自动驾驶之心”公众号
ADAS巨卷干货，即可获取
点击进入→自动驾驶之心【SLAM】技术交流群
本文只做学术分享，如有侵权，联系删文
两个月过去了，不知道你的问题是不是已经解决 :D
如果你从头开始写VIO或者LIO，那么跑飞是正常情况，不跑飞说明你天赋异禀，能够把所有咒语一口气念对。实际系统中相差一点点细节都可能导致IMU系统跑飞，比如某家IMU帮你去掉了重力，某家IMU的陀螺测量单位用的角度，而你假设的是弧度，等等。
而且优化系统并不比滤波器简单。优化也要调各因子的平衡性，而且雅可比矩阵、信息矩阵很容易把矩阵块的索引写错，除了自己查之外别人也帮不上什么忙 :D 我还碰到过把优化器搞出内存泄露调了好几周调不出来的。
如果你执意想从头写的话，我建议按照以下步骤来调：
先把你的VO或LO搞对。大部分系统在没有IMU的时候也能跑起来，保证你的纯视觉、纯激光系统大致是没错的。
检查IMU的单位是否和你预期的一致，比如陀螺和加计的单位，加计有没有去重力。检查IMU的三个轴方向是否和你预计的一样（视觉坐标系和雷达坐标系习惯通常是不一样的，雷达Z朝上，相机Z朝前）。
检查滤波器的F阵、H阵的元素和符号有没有写反。如果你用的开源框架，通常是对的，自己写的就不一定。
如果上述都是对的，那么轨迹“飞”通常是加计导致的（陀螺直接积分都不会飞，但加计会）。各数据集的加计性能相差很多，不能一概而论。实际上你可以先把加计部分去掉，直接用VO或LO估计的平移量。然后检查这个平移量和你用速度+加计估计出来的是否一致。如果不一致，通常就是坐标系出现了问题。
最后才是调参。在VO、LO能正常运行的情况下，把IMU的噪声设大一些，那么EKF会由视觉或激光部分来主导。如果都正常，再慢慢减小IMU的噪声值。不要太相信IMU的标定，我见过理论上推十几页公式，实践动不动把噪声参数乘10，乘100的，都正常。
至于边缘化、FEJ、能观性、P阵切空间投影这些，不会有很大影响，主要是让论文变长一些，至少不会让一个原本飞走的系统变得不飞走。在前期调试的时候甚至都不用做，甚至你可以在每次迭代之后把P阵设成一个单位阵。只要你纯VO或LO不飞，EKF也不会飞。
MSCKF不建议你直接写，我的某个实习生鸽了我两年半的MSCKF-LIO，直到毕业了也没写出来 @白徐行 。你可以在EKF基础上添加multi state部分，做视觉的话参考黄老师他们的方案即可。
然后论文和代码不一致是正常现象。完全一致通常说明作者毕业了或者懒得更新了。代码是在不断发展的，论文多灌一篇是一篇，谁还能不断更新论文不成……
EKF的dx有若干种方式来算，一般就是K乘innovation，其他的无非就是把K变一变形。本身K就是个中间变量，有很多种不同写法。我一般就是：
关键在于H阵和innvation都不要错符号（当然有时候两个都错等于没错就是了）。
还有什么疑问的，欢迎讨论。
① 全网独家视频课程
BEV感知、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、协同感知、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码学习）
视频官网：www.zdjszx.com
② 国内首个自动驾驶学习社区
近2000人的交流社区，涉及30+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（2D检测、分割、2D/3D车道线、BEV感知、3D目标检测、Occupancy、多传感器融合、多传感器标定、目标跟踪、光流估计）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！
③【自动驾驶之心】技术交流群
自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、点云处理、端到端自动驾驶、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）
④【自动驾驶之心】平台矩阵，欢迎联系我们！
