Google Cardboard的九轴融合算法——基于李群的扩展卡尔曼滤波

Google Cardboard的九轴融合算法

——基于李群的扩展卡尔曼滤波

极品巧克力

 

前言

九轴融合算法是指经过融合IMU中的加速度计（三轴）、陀螺仪（三轴）、磁场计（三轴），来获取物体姿态的方法。它是开发VR头显中的一个相当重要的部分。VR头显必需要实时准确地获取用户头部的姿态，而后在屏幕上渲染出在对应的姿态所应该要看到的画面，才能让用户在VR世界里得到沉浸感。

由于人眼是很是精密的器官，若是渲染出来的画面稍微有一点点的延时或者误差，人眼都能察觉出来，致使用户头晕想吐，不再相信VR了。因此，VR头显对九轴融合算法的实时性和精度提出了很是高的要求。

而另外一方面，公开的九轴融合方法又少之又少，常见的就是互补滤波算法和Madgwick算法，可是这两个方法的精度都不能达到VR头显的要求。而精度高的九轴融合算法都掌握在一些算法公司手里，须要向他们支付高昂的算法使用费，源码的价格更是天价。

Cardboard是谷歌在2014年发布的VR盒子，虽然它不是开源的，可是在GitHub上有不少Cardboard的反编译工程，好比https://github.com/rsanchezsaez/cardboard-java。Cardboard的VR体验，能够在必定程度上，证实它的九轴融合算法是知足VR要求的。因此，我对Cardboard反编译工程中的九轴融合部分的程序进行了研读，这部分的程序大概有5000行左右。我在通读完程序以后，结合文献[1],把程序背后的算法理论公式所有都反推出来，总结成了本文，与各位分享。

虽然早在2014年，Cardboard就已经在GitHub上被反编译了，可是这么多年过去了，有关它的代码原理分析的文章倒是几乎没有。能结合源代码，把它背后的算法理论基础详细推导出来的，本文应该算是第一篇。若有推导错误的地方，还请各位不吝赐教。

本文目标读者：传感器融合算法工程师。

一.预测

基于陀螺仪积分来预测出下一个姿态。

假设在时刻的状态的SO3形式的几率知足高斯分布，

其中为归一化常数。为方便起见，把知足上面条件的表示成，。

在时刻，陀螺仪的测量值为，若是没有噪声的话，则对下一个时刻的状态均值的预测为，

其中，为时刻到时刻的时间间隔，。

而若是考虑噪声的影响的话，则对时刻的预测的状态分布要知足，

其中，表示陀螺仪数据的噪声，协方差能够经过采集一段时间的数据，计算获得。

因此，新的均值附近的扰动要知足这样的分布，

又由于有SO3上的性质，，因此，上式中的。因此，原式能够转换以下，

这时候，又由于有SO3上的伴随性质，

原式就能够转换为，

因此，就能够获得，

因此，新的扰动的均值，

新的扰动的协方差，，

因此，最终获得，

二.更新

设在世界坐标系下，加速度计所测的重力向量为，磁场计所测的磁场向量为。则在时刻时，加速度计所测的重力向量为，磁场计所测的磁场向量为。加速度计上面的测量噪声知足。磁场计上面的测量噪声。

2.1加速度计测量更新

把第一部分预测出来的姿态，做为预测的测量姿态，能够预测出当前加速度计的测量值，其计算过程以下，

而根据实际测量值，能够反过来计算出姿态，做为实际的测量姿态。以以前的预测姿态为初值，则把二者的关系表示为，

能够把优化出来，或者直接叉乘出来。

根据李代数与向量叉乘的转换关系。不考虑测量噪声，则能够获得的均值。

设上的噪声为，则关系知足以下，

进一步获得，

要得到与之间的关系，

这二者间的关系不是线性化的，那么就只能进行线性化，一阶泰勒展开，

其中，的计算，采用数值扰动的方法。

从而，能够获得。

最终获得，的分布，

再进行转换，用跟第一部分一样的方法，转换出扰动。

用来表示。

因此，根据第一部分，能够获得，如今又获得了。综合这二者的信息，能够获得，。就是要求一个，使得最大，用公式表达以下。

其中，是个未知数，用，转换成用未知数来表示。而后，上式就能够转换为，

但这样子也解不出来。对上式中的部分，在处进行线性化，一阶泰勒展开。则能够转换为，

其中，的计算，程序里面是用数值扰动的方法。这里应该也能够用解析的方法，把公式都展开来推导。

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

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

参考《State Estimation for Robotics》的3.1.2和3.3.2，求，则上式最终能够转换出卡尔曼滤波的形式了。

因此，

同时，

则融合后的姿态的均值为，

设相对于姿态的李代数扰动。则与的关系要知足，

因此，获得扰动的均值，

获得扰动的协方差，

因此，的分布知足，

2.2公式总结

2.1中的公式总结出来就是，

上面的方法跟《State Estimation for Robotics》的7.3.4和8.2.4很像，可是上面的方法，对协方差的处理更加精细。

要融合磁场计，也是一样的方法。

要融合视觉SLAM中送过来的姿态，也是一样的方法。

3.实际程序

在cardboard的实际程序中，还有不少细节的处理。好比，

增长了不少加权滤波的方法。

把加速度计的模的变化滤波出来，实时更新加速度计的协方差。这一步，至关因而madgwick里面的动态调整权重，但这一步更好，由于是直接算加速度计的协方差来调整权重，而不是经过陀螺仪的测量值来间接表示运动过快而调整权重。

在静止的时候，把陀螺仪的偏移滤波出来。

还有时间差平滑滤波的方法。

在融合磁场计的时候，把磁场计向量映射到水平面上，至关于只优化水平面上的旋转误差。这个，在空间想象时，应该保持重力竖直方向（0,0,1）不变，以此做为参考，再看原来的模型，就容易理解了。

 

可是没有对磁场计进行修正。若是要对磁场计进行修正，简单的方法能够参考madgwick里面的方法。全面的方法，则要参考那些专门搞磁场计标定的论文了。

4.总结

Cardboard里面的九轴融合算法，效果比Madgwick方法和互补滤波方法都要好，对细节的处理也很是棒。之后再写一篇文章，详细比较基于李群的扩展卡尔曼滤波方法，Madgwick算法，互补滤波的异同。

根据参考文献[1]，这套理论也一样可使用在六自由度（位移+旋转）融合上面，只须要把SO3改为SE3就能够了。能够用同一套理论，把视觉SLAM的位姿与IMU位姿融合在一块儿，获得融合后的六自由度数据，应用在VR头显中。

但愿有一天，VR头显的体验能作到像电影《头号玩家》里面那样。与仍然还在作VR的各位同行共勉。

5.求赞扬

您以为，本文值多少？ 

6.有奖问答

给各位出一道思考题。

已知，一个IMU水平地放在桌面上不动。重力大小为。陀螺仪和加速度计以相同的频率同时输出，输出的时间间隔为。它的初始状态为。陀螺仪数据的噪声为，加速度计数据的噪声为。

其中，，，都为对角矩阵。则随着时间的增加，请问，

（1）这个IMU的后验状态协方差是否会收敛？

（2）若是收敛的话，会收敛到什么值？

请在下面评论区做答。第一名正确回答的，将能够得到哈士企公仔一只。

 

7.参考文献

 

1. Bourmaud G, Megret R, Giremus A, et al. Discrete Extended Kalman Filter on Lie groups[C]// Signal Processing Conference. EURASIP, 2013:1-5.
2. Timothy D. Barfoot. State Estimation for Robotics [M].Cambridge University Press, 2017.