极品巧克力
前言
前两篇文章《Google Cardbord的九轴融合算法》，《Madgwick算法详细解读》，讨论的都是在SO3上的传感器融合，即，输出的只是纯旋转的姿态。只有旋转，而没有位移，也就是目前的一些普通的VR盒子的效果。
而《相机IMU融合四部曲》要讨论的是，在SE3上面的传感器融合，在既有旋转又有位移的情况下，该如何对多传感器进行融合。也就是，工程实践中的，如何把基于相机算出来的位姿，与IMU的位姿融合在一起。既有旋转又有位移，可以反映玩家在三维空间中的运动，也就是目前的高端VR的效果，比如HTC Vive，Oculus以及微软的Hololens, MR头盔。
本系列文章分为四篇，分别从理论和实践层面进行详细阐述。
理论部分主要介绍相机和IMU在SE3上的融合，基于误差状态的卡尔曼滤波，IMU的在线标定，松耦合方法，紧耦合方法等。实践部分，则是这些理论的具体实现，在VR中的实际融合效果，以及针对实验结果的改进方案，实践中总结的经验等。
以下为第一篇，我结合参考文献《Discrete extended Kalman filter on lie groups》，对SE3的融合理论进行详细推导，总结成本文，与各位分享。
本文目标读者：传感器融合算法工程师。
一.基础理论
首先，有SE3上的伴随性质，这与《视觉SLAM十四讲》里面的公式4.48一样。伴随性质与BCH近似的目的一样，都是要让相乘的李代数融成一个李代数。
还可以转换成其它的形式。
然后是BCH公式。
另外一个公式。
关于
的计算，参考《机器人状态估计》。
不过，我觉得，上面这个公式，作者可能写错了，比如，当
很大，而
时，代入上式，得到
，这样就得到了
，这样就可以进一步得出
，所以上面的公式是有错误的。
所以，作者想表达的应该是，BCH近似公式。应该写成如下形式，当
和
都为小量时，
然后，假设在均值0附近的李代数满足高斯分布，均值处的李群为
。
又因为
，参考《on-manifold详细解读》，把
的中的表示旋转的
的范围限制住，可以得出
和
是一一对应的关系，
，所以，
而，如果是在位姿
的附近进行同样的误差分布呢，则可以表示为
，用公式表示如下，
还可以推导出，
，
但不能认为，
是绕着
进行高斯分布的。因为李代数也都是相对的，根据BCH公式，
因为不一定是个微小值，所以不能用BCH近似。所以，通过上面公式可以看出，如果没有
的话，则
服从的分布和
一样，会是高斯分布。但是，在
的情况下，
会随着
值的改变而改变。在不同位置对高斯曲线会有不同的改变，所以，
也就不会服从高斯分布，并且对应的协方差也会随之改变。参考《李代数及其协方差都是相对的》。
二.D-LG-EKF
2.1系统模型
是第
时刻的状态，
是外界输入，
是噪声，则对下一个状态的预测为，
其中，
表示的是，这个输入所造成的位姿变换的李代数。要注意的是，
并不直接代表
上的噪声，虽然它的来源是
上的噪声，它需要根据
上的噪声，通过
的实际表达式，转换出来。
因为
和
都是已知的，假设只在
的作用下，状态
变成了
。则，
而
所表示的就是，
因为
和
都是已知的，所以可以通过这个状态变换，计算得到
。参考《视觉SLAM十四讲》的图4-1，可以将
转换为
。所以，
也就可以得到了。
测量方程。
2.2传播
假设在
时刻的状态的后验概率分布服从，
。即，
如果没有噪声的话，则对下一个状态均值的预测
为，
而根据之前的公式，对
预测的分布满足，
所以，设下一个状态的误差分布为
，则它要满足，
可以得出，
参考《李代数扰动的理解》，当
时，
，所以，上式可以转换为，
要注意的是，
，因为这里的
其实表示的是一种分布，
，所以，
，所以，原式还可以转换成，
上式的右边都是已知的，所以就是代表了从
到
的变换关系。首先，根据伴随性质，得到，
再根据BCH近似公式，上式可以转换为，
令
，则直接使用BCH公式，上式可以转换为，
又因为
和
都是小量，所以
可以忽略掉，上式可以转换为，
所以， 
与
的关系又可以表示为，
对上面的变换关系，在
处进行线性化，也就是进行一阶泰勒展开，
则，
因为
，所以
，代入上式，得到，
所以，最终得到，
而另外一个，
所以，
然后，根据实际的运动方程，
的表达式，算出
的解析式，再求出
。（或者，也可以用数值扰动的方法，求
。），然后得到
。
继续之前的变换关系进行一阶泰勒展开，
其中，
是一个微小量，可以忽略掉。所以，就得到了
与
之间的线性转换关系。
用
表示
，得到，
再计算扰动的均值
，
然后，再计算
的协方差
。协方差的计算，参考《机器人状态估计》的第二章。
所以，最后得到，
2.3更新
对预测出来的扰动和实际测量出来的扰动，进行融合。
对测量值的处理，可以直接是传感器的测量值，比如加速度计的测量值，然后再考虑这个测量值上的高斯噪声。将预测出来的测量值与实际的测量值进行融合，然后再反馈给状态。
对测量值的处理，也可以转换成李代数的形式。就像是madgwick算法一样，以之前的姿态为初值，优化姿态，使得通过姿态计算出来的测量值与实际测量值最接近。而在本文中，采用的就是这种方法。（如果测量值只是加速度计或磁场计的测量向量的话，就更简单了，因为madgwick要优化出四元数，而本文只要李代数就可以了。以之前的姿态为初值，优化姿态，使得通过姿态计算出来的加速度计向量与实际加速度计向量最接近，而这两个向量之间的相对位姿变化，只需要叉乘一下就可以了，不需要通过优化。如果测量值是其它的，比如图像上的特征点位置，那就只能通过优化的方法，优化出测量位姿，或者采用上一种测量值融合的方法。或者，也可以与视觉SLAM结合起来，直接以图像计算出来的姿态或位姿为测量值。）
设第k时刻位姿的真实值为
。
首先，有个预测的位姿
，它的协方差为
。则意味着概率，
。
基于这个预测的位姿，预测出来的传感器的测量值
，然后有传感器的实际测量值
。用
作为位姿初值，优化（或叉乘）出新的位姿，使得预测测量值与实际测量值
最接近。用
，表示优化出来的位姿相对于预测位姿的位姿。所以，实际测量值位姿可以表示为，
其中，
是传感器的测量值
的噪声，传递到
之后，再分离到右边去。
可以通过这个过程中的变换，从实际传感器的测量值协方差，转换过来。
所以，这就意味着，
所以，综合目前的信息，可以得到，
，就是要求一个
，使得
最大。
其中，
是个未知数，用
，转换成用未知数
来表示。然后，上式就可以转换为，
但这样子也解不出来。参考《李代数高斯分布的求导》，对上式中的部分，在
处进行线性化，一阶泰勒展开。则可以转换为，
同理，
其中，
的计算，用数值扰动的方法。当然，也可以用解析的方法，把公式都展开来推导。（或者，参考《MSF详细解读》里面的方法，为了算H矩阵，直接就认为
，这样子算H矩阵很方便，其余的与原来方法一样。如果是上式的话，则
）。
接下来，为了转换成卡尔曼滤波的形式，用
来表示。
所以，原式就可以表示为，
这样子，参考《从贝叶斯到卡尔曼滤波》，就可以转换成卡尔曼滤波的形式了。
所以，得到了融合后的扰动
，
同时，
满足，
所以，预测出来的位姿
，乘以这个融合后的扰动均值
，就得到了融合后的位姿
，
所以，新的位姿的李代数为
，
则新位姿附近的李代数扰动
要满足，
所以，新的扰动
的均值
和协方差
，
协方差，
所以，
三.总结
总结起来，流程就是，
四.参考文献
-
Bourmaud G, Megret R, Giremus A, et al. Discrete Extended Kalman Filter on Lie groups[C]// Signal Processing Conference. EURASIP, 2013:1-5.