相机IMU融合四部曲（一）：D-LG-EKF详细解读

相机IMU融合四部曲（一）：D-LG-EKF详细解读

极品巧克力

前言

前两篇文章《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.