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

本文轉載自查看原文 2018-07-12 23:48 3406

相機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時刻位姿的真實值為。