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.