相機IMU融合四部曲（一）：D-LG-EKF詳細解讀

相機IMU融合四部曲（一）：D-LG-EKF詳細解讀

極品巧克力

前言

前兩篇文章《Google Cardbord的九軸融合算法》，《Madgwick算法詳細解讀》，討論的都是在SO3上的傳感器融合，即，輸出的只是純旋轉的姿態。只有旋轉，而沒有位移，也就是目前的一些普通的VR盒子的效果。

而《相機IMU融合四部曲》要討論的是，在SE3上面的傳感器融合，在既有旋轉又有位移的狀況下，該如何對多傳感器進行融合。也就是，工程實踐中的，如何把基於相機算出來的位姿，與IMU的位姿融合在一塊兒。既有旋轉又有位移，能夠反映玩家在三維空間中的運動，也就是目前的高端VR的效果，好比HTC Vive，Oculus以及微軟的Hololens, MR頭盔。

本系列文章分爲四篇，分別從理論和實踐層面進行詳細闡述。

理論部分主要介紹相機和IMU在SE3上的融合，基於偏差狀態的卡爾曼濾波，IMU的在線標定，鬆耦合方法，緊耦合方法等。實踐部分，則是這些理論的具體實現，在VR中的實際融合效果，以及針對實驗結果的改進方案，實踐中總結的經驗等。

如下爲第一篇，我結合參考文獻《Discrete extended Kalman filter on lie groups》，對SE3的融合理論進行詳細推導，總結成本文，與各位分享。

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

一.基礎理論

首先，有SE3上的伴隨性質，這與《視覺SLAM十四講》裏面的公式4.48同樣。伴隨性質與BCH近似的目的同樣，都是要讓相乘的李代數融成一個李代數。

還能夠轉換成其它的形式。

而後是BCH公式。

另一個公式。

關於的計算，參考《機器人狀態估計》。

不過，我以爲，上面這個公式，做者可能寫錯了，好比，當很大，而時，代入上式，獲得，這樣就獲得了，這樣就能夠進一步得出，因此上面的公式是有錯誤的。

因此，做者想表達的應該是，BCH近似公式。應該寫成以下形式，當和都爲小量時，

而後，假設在均值0附近的李代數知足高斯分佈，均值處的李羣爲。

又由於，參考《on-manifold詳細解讀》，把的中的表示旋轉的的範圍限制住，能夠得出和是一一對應的關係，，因此，

而，若是是在位姿的附近進行一樣的偏差分佈呢，則能夠表示爲，用公式表示以下，

還能夠推導出，，

但不能認爲，是繞着進行高斯分佈的。由於李代數也都是相對的，根據BCH公式，

由於不必定是個微小值，因此不能用BCH近似。因此，經過上面公式能夠看出，若是沒有的話，則服從的分佈和同樣，會是高斯分佈。可是，在的狀況下，會隨着值的改變而改變。在不一樣位置對高斯曲線會有不一樣的改變，因此，也就不會服從高斯分佈，而且對應的協方差也會隨之改變。參考《李代數及其協方差都是相對的》。

二.D-LG-EKF

2.1系統模型

是第時刻的狀態，是外界輸入，是噪聲，則對下一個狀態的預測爲，

其中，表示的是，這個輸入所形成的位姿變換的李代數。要注意的是，並不直接表明上的噪聲，雖然它的來源是上的噪聲，它須要根據上的噪聲，經過的實際表達式，轉換出來。

由於和都是已知的，假設只在的做用下，狀態變成了。則，

而所表示的就是，

由於和都是已知的，因此能夠經過這個狀態變換，計算獲得。參考《視覺SLAM十四講》的圖4-1，能夠將轉換爲。因此，也就能夠獲得了。

測量方程。

2.2傳播

假設在時刻的狀態的後驗機率分佈服從，。即，

若是沒有噪聲的話，則對下一個狀態均值的預測爲，

而根據以前的公式，對預測的分佈知足，

因此，設下一個狀態的偏差分佈爲，則它要知足，

能夠得出，

參考《李代數擾動的理解》，當時，，因此，上式能夠轉換爲，

要注意的是，，由於這裏的其實表示的是一種分佈，，因此，，因此，原式還能夠轉換成，

上式的右邊都是已知的，因此就是表明了從到的變換關係。首先，根據伴隨性質，獲得，

再根據BCH近似公式，上式能夠轉換爲，

令 ，則直接使用BCH公式，上式能夠轉換爲，

又由於和都是小量，因此能夠忽略掉，上式能夠轉換爲，

因此， 與的關係又能夠表示爲，

對上面的變換關係，在處進行線性化，也就是進行一階泰勒展開，

則，

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

因此，最終獲得，

而另一個，

因此，

而後，根據實際的運動方程，的表達式，算出的解析式，再求出。（或者，也能夠用數值擾動的方法，求。），而後獲得。

繼續以前的變換關係進行一階泰勒展開，

其中，是一個微小量，能夠忽略掉。因此，就獲得了與之間的線性轉換關係。

用表示，獲得，

再計算擾動的均值，

而後，再計算的協方差。協方差的計算，參考《機器人狀態估計》的第二章。

因此，最後獲得，

2.3更新

對預測出來的擾動和實際測量出來的擾動，進行融合。

對測量值的處理，能夠直接是傳感器的測量值，好比加速度計的測量值，而後再考慮這個測量值上的高斯噪聲。將預測出來的測量值與實際的測量值進行融合，而後再反饋給狀態。

對測量值的處理，也能夠轉換成李代數的形式。就像是madgwick算法同樣，以以前的姿態爲初值，優化姿態，使得經過姿態計算出來的測量值與實際測量值最接近。而在本文中，採用的就是這種方法。（若是測量值只是加速度計或磁場計的測量向量的話，就更簡單了，由於madgwick要優化出四元數，而本文只要李代數就能夠了。以以前的姿態爲初值，優化姿態，使得經過姿態計算出來的加速度計向量與實際加速度計向量最接近，而這兩個向量之間的相對位姿變化，只須要叉乘一下就能夠了，不須要經過優化。若是測量值是其它的，好比圖像上的特徵點位置，那就只能經過優化的方法，優化出測量位姿，或者採用上一種測量值融合的方法。或者，也能夠與視覺SLAM結合起來，直接以圖像計算出來的姿態或位姿爲測量值。）

設第k時刻位姿的真實值爲。

首先，有個預測的位姿，它的協方差爲。則意味着機率，

。

基於這個預測的位姿，預測出來的傳感器的測量值，而後有傳感器的實際測量值。用做爲位姿初值，優化（或叉乘）出新的位姿，使得預測測量值與實際測量值最接近。用，表示優化出來的位姿相對於預測位姿的位姿。因此，實際測量值位姿能夠表示爲，

其中，是傳感器的測量值的噪聲，傳遞到以後，再分離到右邊去。能夠經過這個過程當中的變換，從實際傳感器的測量值協方差，轉換過來。

因此，這就意味着，

因此，綜合目前的信息，能夠獲得，，就是要求一個，使得最大。

其中，是個未知數，用，轉換成用未知數來表示。而後，上式就能夠轉換爲，

但這樣子也解不出來。參考《李代數高斯分佈的求導》，對上式中的部分，在處進行線性化，一階泰勒展開。則能夠轉換爲，

同理，

其中，的計算，用數值擾動的方法。固然，也能夠用解析的方法，把公式都展開來推導。（或者，參考《MSF詳細解讀》裏面的方法，爲了算H矩陣，直接就認爲，這樣子算H矩陣很方便，其他的與原來方法同樣。若是是上式的話，則）。

 

接下來，爲了轉換成卡爾曼濾波的形式，用來表示。

因此，原式就能夠表示爲，

這樣子，參考《從貝葉斯到卡爾曼濾波》，就能夠轉換成卡爾曼濾波的形式了。

因此，獲得了融合後的擾動，

同時，知足，

因此，預測出來的位姿，乘以這個融合後的擾動均值，就獲得了融合後的位姿，

因此，新的位姿的李代數爲，

則新位姿附近的李代數擾動要知足，

因此，新的擾動的均值和協方差，

協方差，

因此，

三.總結

總結起來，流程就是，

四.參考文獻

1. Bourmaud G, Megret R, Giremus A, et al. Discrete Extended Kalman Filter on Lie groups[C]// Signal Processing Conference. EURASIP, 2013:1-5.