无人驾驶算法学习（六）：多传感器融合MSF算法

1.引言

本文的多传感器融合是建立在读懂《Quaternion kinematics for the error-state Kalman filter》基础上的 ，是一种相机和IMU融合的理论，里面讲解了IMU的误差状态运动方程构建。误差状态四元数，是有开源的程序的，但是它是集成在rtslam里面的，不方便提取出来使用。
但还有另外一个开源的程序，ETH的MSF，可以比较方便地用在自己的工程里面，并且它的理论与误差状态四元数很接近，稍微有点不同，所以MSF开源程序就成了一个不错的选择。
所以本人研究了ETH的两篇文章：《Vision Based Navigation for Micro Helicopters》和《A Robust and Modular Multi-Sensor Fusion Approach Applied to MAV Navigation》
其核心就是扩展卡尔曼滤波EKF，本人把MSF的核心代码阅读了一遍，推导了论文中的算法，并最后做了实验。

2. 算法理论

MSF关键点：

• 我们使用IMU含噪声的测量值和bias去计算估计状态量X。并且没有考虑测量值的噪声项和扰动，从而产生了误差。我们把所有不确定性引来的误差放在误差变量δx中考虑，并推导了误差状态运动模型，以此求解Fd状态转移矩阵和P过程协方差矩阵
• 利用其它来源的数据如GPS,视觉等来作为矫正（使误差可观）。通过估计状态量X计算Hj，利用观测量和估计量计算残差，最后计算K，来更新P和误差变量δx，最后更新状态量X。

2.1 MSF基本模型

基本模型如下图所示。MSF的可扩展性很好，程序里可以接入新的传感器，比如GPS，激光雷达，码盘。

下图是msf的原理示意图：

下图是eskf中所有变量解释表

2.2 预测

状态表示：

理想运动模型：

实际运动模型：

误差状态表示：

误差运动状态模型：

注意：在eskf中有详细推导和解释

误差状态导数的转移矩阵：

二阶泰勒展开（忽略二阶以上微小量）：

误差状态的转移矩阵：

预测误差状态和预测误差差协方差矩阵：

2.3 测量与更新

p测量模型：

q测量模型：

测量更新：

3. 核心代码分析

核心算法的代码分析如下图所示：

预测与更新算法调用关系图：

4. 代码实战

msf-ethsal编译及使用
相机IMU融合四部曲（三）:MSF详细解读与使用
基本操作

roslaunch msf_updates position_pose_sensor.launch 
roslaunch msf_updates viconpos_sensor.launch
rosrun rqt_reconfigure rqt_reconfigure#初始化卡尔曼滤波器,很重要!!
如下界面,需要点击filter