相机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.