相機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融合四部曲(二):誤差狀態四元數詳細解讀