相机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矩阵很方便，其他的与原来方法同样。若是是上式的话，则）。

 

接下来，为了转换成卡尔曼滤波的形式，用来表示。

因此，原式就能够表示为，

这样子，参考《从贝叶斯到卡尔曼滤波》，就能够转换成卡尔曼滤波的形式了。

因此，获得了融合后的扰动，

同时，知足，

因此，预测出来的位姿，乘以这个融合后的扰动均值，就获得了融合后的位姿，

因此，新的位姿的李代数为，

则新位姿附近的李代数扰动要知足，

因此，新的扰动的均值和协方差，

协方差，

因此，

三.总结

总结起来，流程就是，

四.参考文献

1. Bourmaud G, Megret R, Giremus A, et al. Discrete Extended Kalman Filter on Lie groups[C]// Signal Processing Conference. EURASIP, 2013:1-5.