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.