111、多传感器融合定位：IMU视觉激光的紧耦合融合
昨晚在实验室调了一整天的紧耦合融合，最后发现问题出在IMU的加速度计零偏初始值上——差了0.02 m/s²，跑完一圈走廊，位置漂了快三米。这种问题教科书上不会告诉你，但实际工程里天天见。今天这篇笔记，就把我踩过的坑和验证过的路子，掰开揉碎讲清楚。
先说清楚什么叫紧耦合，什么叫松耦合。松耦合就是各传感器先各自算位姿，然后拿卡尔曼滤波或者因子图去融合结果，相当于三个实习生各写一份报告，领导再综合一下。紧耦合不一样，它把IMU的原始测量（加速度和角速度）直接塞进优化或者滤波的预测模型里，视觉和激光的特征作为观测约束，相当于三个实习生坐在一起，边讨论边写一份报告。紧耦合精度高，但计算量大，调试难度也指数级上升。
我这边实际用的方案是：IMU预积分 + 视觉特征重投影 + 激光点云配准，三者统一在一个因子图框架里，用GTSAM做增量平滑。这个组合在室内走廊、室外园区、甚至地下车库都跑过，鲁棒性比任何单一传感器都好。
先看IMU预积分。这里有个关键点：不要直接对IMU原始数据做积分去更新位姿，那样每次优化位姿变化后，所有IMU测量都得重新积分，计算量爆炸。正确做法是预积分——把两个关键帧之间的IMU测量转成相对旋转、相对速度和相对位移，以及对应的协方差矩阵。这样优化时只需要更新预积分量，不用重新过一遍原始数据。
// IMU预积分——这里踩过坑：预积分量必须包含零偏的雅可比// 否则优化时零偏更新后，预积分量
