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.参考文献
- Bourmaud G, Megret R, Giremus A, et al. Discrete Extended Kalman Filter on Lie groups[C]// Signal Processing Conference. EURASIP, 2013:1-5.
- Timothy D. Barfoot. State Estimation for Robotics [M].Cambridge University Press, 2017.