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.