相机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.参考文献
- Solà J. Quaternion kinematics for the error-state Kalman filter[J]. 2017.