相机IMU融合四部曲（二）：偏差状态四元数详细解读

相机IMU融合四部曲（二）：偏差状态四元数详细解读

极品巧克力

前言

上一篇文章，《D-LG-EKF详细解读》中，讲了理论上的SE3上相机和IMU融合的思想。可是，尚未涉及到实际的操做，以及实际操做中会遇到的一些问题。因此，本文开始讲实际操做，包括，在相机和IMU融合的过程当中，IMU速度的计算，加速度计和陀螺仪的使用，偏移的处理，重力的滤波等。

本文的主要参考文献为John sola的《Quaternion kinematics for the error state Kalman》，简称为偏差状态四元数。它的基本思想和D-LG-EKF是同样的，都是对均值状态和扰动状态的进行处理。可是，不一样的是，在偏差状态四元数里，是把偏移也放到状态里滤波的，而Google Cardboard里的偏移是经过低通滤波滤出来的。

并且John sola的相机和IMU融合的程序是开源的，项目名称为RT-SLAM，源代码地址为https://www.openrobots.org/wiki/rtslam/。

其运行效果如视频（https://vimeo.com/114879173）所示。

本文目标读者：传感器融合算法工程师。

1.预测

首先，列出运动方程。

其中，下标t表示的是true的意思。

又由于实际测量值与真实值的关系为，

把实际测量值代入到上式中，

因此，用D-LG-EKF里面的均值+扰动的思想，表示两个时刻的均值以及扰动状态之间要知足的关系，

由于，，因此，代入上式，获得，

里面，实际上是，而不是，是中间的一个转换。

其中，上式左边的等都为均值，就是由时刻的均值变换过来的，新的均值的计算过程以下，

再代回到以前的公式中，

从而获得，新的扰动与旧的扰动的关系，

把的平方项都忽略掉，忽略二阶的极小值，进一步推导。

参考《从角轴到四元数微分方程》，角轴扰动与四元数扰动的关系为，

原先的表达式能够转换为，

在上面的推导中，由于是二阶极小值，因此忽略掉。由于是各向相同的噪声，因此，参考论文里面的推导，并不会影响协方差的计算。

对下一个表达式进行转换，

把上面的四元数乘法，所有都解开，应该就能算出来。可是太麻烦了。

利用，

上式转换为，

在上式中，由于，认为是一个微小值，因此直接。

或者，另一种方法，参考论文上的方法，对表达式两边求导，导数也应该是相同的。

由于，，因此，

在上面，进行了两处简化，首先，忽略掉了项，而后，。积分以后，就获得，

与前面那种方法的结果相同。可是，论文上的这种方法难想到，为了思考上的方便，之后仍然仍是用前面的那种方法。

启发：这里用旋转矩阵，而不是叉乘，这么操做的目的应该是，叉乘只是对旋转矩阵的近似，而角轴转旋转矩阵，用罗德里格斯变换，获得的结果最准确。因此，最终结果里，仍是尽可能少用叉乘，能组合成旋转矩阵就组合成旋转矩阵，而角轴到旋转矩阵的方法用罗德里格斯变换。

而后，算后面的扰动。

新的扰动与旧的扰动的关系，总结起来就是，

因此，都转换成了线性的关系，能够表示为，

其中，

由于，，因此，

其中，的计算，参考论文上的公式（270）。

2.更新

而后，使用贝叶斯公式，用表示，

上面的表示的是一种特殊的运算，意思是距离。

为了能像《D-LG-EKF》里面那样转换成卡尔曼滤波的形式，上式的右边内容，须要进行线性化。在扰动的均值处进行线性化，在这里，即为处进行线性化。

固然，，也能够进一步变换，好比，MSF里，把四元数残差转换成角轴残差。但这些残差都要有相对应的。

而在偏差状态四元数论文里，是经过级联求导的方法。

其中，要根据具体的残差方程来计算，而则是固定的。不管是四元数残差仍是角轴残差，仍是其它的残差，不一样的仅仅只是，而和都是同样的。因此，上面的表达式，是一个通用模型，适用于全部的残差，也能够说是，适用于全部的观测。

因此，能够提早计算好，

其中，其它项都是单位阵，除了，则计算以下，

因此，

这是能够提早计算好的，在实际计算时，只须要把代进来就行。

用，则原式能够转换为，

而后，就能够转换为卡尔曼滤波公式，这些对应的是扰动的均值和协方差，

而后，就是把卡尔曼滤波算出来的最大后验值，加入到原先的状态中，。

也就是论文里面的reset部分，就是让旧的状态吸取进卡尔曼滤波出来的扰动的均值，让新扰动的均值变为0。

则新的扰动，与旧的扰动的关系为，

其中，

代回到以前的公式，获得，

从而获得，

因此，新的扰动的均值为，

新的扰动的协方差为，

因此，也就获得的新的协方差，。

或者，也不必这么麻烦，直接根据前面新旧扰动的关系，算，而后。其实根据协方差计算公式，本质上是同样的。

3.全局扰动

以前的扰动都是加在右边的，全局扰动就是加在左边的扰动。

全局扰动与本地扰动的区别，如论文中的表格4所示。差异不大，主要是角度上的扰动的雅克比。这里也计算一下。

4.微分方程的积分

论文的附录部分，就是讨论用龙格库塔的方法，或者泰勒多阶展开的方法，对状态转移矩阵进行积分。

5.总结

虽然论文中有说这么一句话，全局扰动的方法比局部扰动的方法要好，好比李名杨的MSCKF中的方法，可是没有具体举例说明好在哪里。

用四元数来表示状态，四元数扰动与角轴扰动的转换太麻烦了，能够改为用李代数来表示旋转，可是李代数里面的BCH近似的又很差算。

6.参考文献

1. Solà J. Quaternion kinematics for the error-state Kalman filter[J]. 2017.