相機IMU融合四部曲（二）：偏差狀態四元數詳細解讀

相機IMU融合四部曲（二）：偏差狀態四元數詳細解讀

極品巧克力

前言

上一篇文章，《D-LG-EKF詳細解讀》中，講了理論上的SE3上相機和IMU融合的思想。可是，尚未涉及到實際的操做，以及實際操做中會遇到的一些問題。因此，本文開始講實際操做，包括，在相機和IMU融合的過程當中，IMU速度的計算，加速度計和陀螺儀的使用，偏移的處理，重力的濾波等。

本文的主要參考文獻爲John sola的《Quaternion kinematics for the error state Kalman》，簡稱爲偏差狀態四元數。它的基本思想和D-LG-EKF是同樣的，都是對均值狀態和擾動狀態的進行處理。可是，不一樣的是，在偏差狀態四元數裏，是把偏移也放到狀態裏濾波的，而Google Cardboard裏的偏移是經過低通濾波濾出來的。

並且John sola的相機和IMU融合的程序是開源的，項目名稱爲RT-SLAM，源代碼地址爲https://www.openrobots.org/wiki/rtslam/。

其運行效果如視頻（https://vimeo.com/114879173）所示。

本文目標讀者：傳感器融合算法工程師。

1.預測

首先，列出運動方程。

其中，下標t表示的是true的意思。

又由於實際測量值與真實值的關係爲，

把實際測量值代入到上式中，

因此，用D-LG-EKF裏面的均值+擾動的思想，表示兩個時刻的均值以及擾動狀態之間要知足的關係，

由於，，因此，代入上式，獲得，

裏面，實際上是，而不是，是中間的一個轉換。

其中，上式左邊的等都爲均值，就是由時刻的均值變換過來的，新的均值的計算過程以下，

再代回到以前的公式中，

從而獲得，新的擾動與舊的擾動的關係，

把的平方項都忽略掉，忽略二階的極小值，進一步推導。

參考《從角軸到四元數微分方程》，角軸擾動與四元數擾動的關係爲，

原先的表達式能夠轉換爲，

在上面的推導中，由於是二階極小值，因此忽略掉。由於是各向相同的噪聲，因此，參考論文裏面的推導，並不會影響協方差的計算。

對下一個表達式進行轉換，

把上面的四元數乘法，所有都解開，應該就能算出來。可是太麻煩了。

利用，

上式轉換爲，

在上式中，由於，認爲是一個微小值，因此直接。

或者，另一種方法，參考論文上的方法，對錶達式兩邊求導，導數也應該是相同的。

由於，，因此，

在上面，進行了兩處簡化，首先，忽略掉了項，而後，。積分以後，就獲得，

與前面那種方法的結果相同。可是，論文上的這種方法難想到，爲了思考上的方便，之後仍然仍是用前面的那種方法。

啓發：這裏用旋轉矩陣，而不是叉乘，這麼操做的目的應該是，叉乘只是對旋轉矩陣的近似，而角軸轉旋轉矩陣，用羅德里格斯變換，獲得的結果最準確。因此，最終結果裏，仍是儘可能少用叉乘，能組合成旋轉矩陣就組合成旋轉矩陣，而角軸到旋轉矩陣的方法用羅德里格斯變換。

而後，算後面的擾動。

新的擾動與舊的擾動的關係，總結起來就是，

因此，都轉換成了線性的關係，能夠表示爲，

其中，

由於，，因此，

其中，的計算，參考論文上的公式（270）。

2.更新

而後，使用貝葉斯公式，用表示，

上面的表示的是一種特殊的運算，意思是距離。

爲了能像《D-LG-EKF》裏面那樣轉換成卡爾曼濾波的形式，上式的右邊內容，須要進行線性化。在擾動的均值處進行線性化，在這裏，即爲處進行線性化。

固然，，也能夠進一步變換，好比，MSF裏，把四元數殘差轉換成角軸殘差。但這些殘差都要有相對應的。

而在偏差狀態四元數論文裏，是經過級聯求導的方法。

其中，要根據具體的殘差方程來計算，而則是固定的。不管是四元數殘差仍是角軸殘差，仍是其它的殘差，不一樣的僅僅只是，而和都是同樣的。因此，上面的表達式，是一個通用模型，適用於全部的殘差，也能夠說是，適用於全部的觀測。

因此，能夠提早計算好，

其中，其它項都是單位陣，除了，則計算以下，

因此，

這是能夠提早計算好的，在實際計算時，只須要把代進來就行。

用，則原式能夠轉換爲，

而後，就能夠轉換爲卡爾曼濾波公式，這些對應的是擾動的均值和協方差，

而後，就是把卡爾曼濾波算出來的最大後驗值，加入到原先的狀態中，。

也就是論文裏面的reset部分，就是讓舊的狀態吸取進卡爾曼濾波出來的擾動的均值，讓新擾動的均值變爲0。

則新的擾動，與舊的擾動的關係爲，

其中，

代回到以前的公式，獲得，

從而獲得，

因此，新的擾動的均值爲，

新的擾動的協方差爲，

因此，也就獲得的新的協方差，。

或者，也不必這麼麻煩，直接根據前面新舊擾動的關係，算，而後。其實根據協方差計算公式，本質上是同樣的。

3.全局擾動

以前的擾動都是加在右邊的，全局擾動就是加在左邊的擾動。

全局擾動與本地擾動的區別，如論文中的表格4所示。差異不大，主要是角度上的擾動的雅克比。這裏也計算一下。

4.微分方程的積分

論文的附錄部分，就是討論用龍格庫塔的方法，或者泰勒多階展開的方法，對狀態轉移矩陣進行積分。

5.總結

雖然論文中有說這麼一句話，全局擾動的方法比局部擾動的方法要好，好比李名楊的MSCKF中的方法，可是沒有具體舉例說明好在哪裏。

用四元數來表示狀態，四元數擾動與角軸擾動的轉換太麻煩了，能夠改爲用李代數來表示旋轉，可是李代數裏面的BCH近似的又很差算。

6.參考文獻

1. Solà J. Quaternion kinematics for the error-state Kalman filter[J]. 2017.