相機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.參考文獻
- Solà J. Quaternion kinematics for the error-state Kalman filter[J]. 2017.
相機IMU融合四部曲（二）：誤差狀態四元數詳細解讀