MSF多传感器融合流程

目录

MSF多传感器融合流程

1. 添加重力约束观测

2.添加INS数据观测

3.添加语义点云匹配观测

4. ESKF过程

一、更新前的准备工作（StateUpdate）

二、进行1)零速更新，或2)ESKF滤波

三、完成观测更新后

---

• MSF多传感器融合流程

多传感融合包含：IMU, CAN，视觉或激光匹配的结果，INS，GNSS。以IMU为核心递推（包含can速度）

观测的约束如下：

1. 添加重力约束观测

重力约束是在在车体系的方向是（0，0，1），添加时1）如果已知车辆在水平方向运动，添加平面约束；2）如果已知车辆静止（判断IMU加速度计和陀螺仪的norm都很小）则按照IMU的加速度计当观测，协方差用加速度计的模-重力常亮的模。

注意重力约束只能在车辆为静止状态下使用，因为此时车辆没有顺时的加速度影响结果。此时不需要考虑bias的问题，因为相比较于9.8的数值bias的影响微乎其微。

double gravity_diff = fabs(imu_data.acc.norm() - GetIMUConfig().gravity_norm); 
double err = std::acos(GetIMUConfig().gravity_norm /(GetIMUConfig().gravity_norm +gravity_diff));
err = (err < 0.1 / 180.0 * 3.141592653)