8互補卡爾曼濾波

前言

正如前文所說，卡爾曼濾波器的建模形式多種多樣，而且很多研究也是在上世紀，對于誤差狀態卡爾曼濾波（Error-state Kalman Filter)和互補卡爾曼濾波（Comlimentary Kalman Filter)其實沒有嚴格的定義。這里的互補卡爾曼濾波其實也可以看成上文ErKF的另一種形式。主要采用自論文Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter?？柭鼮V波的工作太多，博客和論文也五花八門，看起來十分不易。這篇論文從引用、論文敘述、符號標識看起來都很不錯，很適合想要將卡爾曼濾波應用到姿態估計的工程師閱讀。甚至有一些工作，直接使用普通卡爾曼濾波輸出，然后利用互補濾波器的概念，在多種姿態輸出之間做加權平均，也叫互補卡爾曼濾波器，比如這篇專利：一種基于互補卡爾曼濾波算法計算融合姿態角度的方法

互補的概念

其實只要有了角速度計（Gyroscope）我們就可以根據其輸出，并在時間上做積分解算出車輛的姿態角。但是由于任何傳感器都是帶噪聲的，同時，直接用積分解算，誤差會隨機時間推移而累積，最終的姿態解算精度就非常差。除了使用角速度計進行積分可以得到姿態角之外，用加速度計（Accelerometer）和磁力計（Magnatometer）也可以計算出動態系統的姿態。其中，加速度計使用重力加速度作為錨定量，測量靜止狀態下三軸加速度之間的夾角，可以計算出角；磁力計（或叫地磁計）利用地球磁場北極作為錨定量，可以計算出角。使用這兩者對角速度計進行補充，可以得出更加準確的姿態估計。

從加速度計計算姿態角roll、pitch

加速度計（Accelerometer）可以輸出三個軸的加速度，在靜止的情況下，三個軸的合向量就是重力加速度。因此，我們可以利用三個軸加速度之間的關系計算靜止狀態下的俯仰角（pitch）和翻滾角（roll）

關于如何推導從三個軸的加速度計算roll和pitch，可以看這篇文章

最后得出的形式也非常簡單：

從磁力計計算姿態角yaw

磁力計的三軸合向量會指向地磁北極，利用這一特性，可以從磁力計的輸出獲得與地磁北極的偏轉角。利用這一點，可以計算出相對于起始位置的角

具體的計算公式可以看這篇博客

從加速度計算的姿態彌補

從加速度計可以計算出roll角和pitch角，因此，可以將這個結果和角速度的積分結果結合起來，得到一個更好的估計姿態。不過要注意的是，從加速度計算的姿態彌補有兩個局限：

1. 加速度計只能計算出Roll角和Pitch角，因此yaw角無法得到互補信息
2. 當車輛處于較大的加速度運動狀態時，三軸加速度的合向量跟重力加速度會有偏差，因此互補結果應該根據這個偏差的大小做改變。

互補濾波器

互補濾波器使用角速度的積分結果和加速度與磁力計的計算結果進行插值，得到更好的結果

其中，是對角速度積分得出的姿態（用四元素表達）；是使用加速度計和磁力計計算出的姿態。

互補卡爾曼濾波器

互補卡爾曼濾波器將姿態角的誤差、角速度的誤差當作狀態向量；并利用其余傳感器如加速度計和磁力計計算出的姿態角與估計的姿態角之間的差作為測量量。通過以下步驟得到系統的姿態角：

1. 卡爾曼濾波器輸出姿態角的誤差和角速度的誤差
2. 將當前時刻角速度的輸出加上角速度的誤差，并利用積分公式算出姿態角
3. 將步驟2算出的姿態角加上步驟1輸出的姿態角誤差

狀態向量和測量向量

狀態向量

其中是系統的角；是三軸角速度。

測量向量

其中，是使用加速度計和磁力計計算出的系統角

狀態方程

我們可以推導出姿態角對于時間的導數，通常這個導數是姿態角和角速度的函數，即

從公式（4-1）的推導過程，以及泰勒展開，我們可以得到的遞推公式

我們直接假設角速度的誤差是一個常量誤差，即

則我們可以將上面兩式寫成矩陣形式

測量方程

預測&&更新過程

有了上面狀態方程和測量方程的推導，剩下的就是按照公式（4-3）到公式（4-10）的過程代入。這里唯一不同的就是卡爾曼濾波輸出的向量是角速度的誤差和姿態的誤差。在計算姿態的時候先將角速度的誤差應用到角速度計的數據，對角度積分，將角度的誤差應用到角度積分結果，得到最終的角度輸出。整個框架如下圖

9后話

1. 卡爾曼濾波是一個很古老的算法，但同時又是被廣泛應用的算法。即使在今天姿態解算中很多用了因子圖，但是對IMU的預積分依舊要使用卡爾曼濾波。但是卡爾曼濾波算法只是一個工具，不同的系統建模方式會產生不同形式的卡爾曼濾波器，這也導致了初學者不知道從哪里入手。
2. 在查資料的過程中發現，卡爾曼濾波的一些變種如Error-State Kalman Filter和Complimentary Kalman Filter其實并不是嚴格定義的。
3. 筆者對卡爾曼濾波并沒有很豐富的應用經驗，本身也不是專門做運動控制和姿態解算的。本文的敘述在追求通俗易懂之外盡力保證公式和符號定義的準確。但無法保證沒有錯誤。
4. 對于卡爾曼濾波器中各個變量的初始值設置是門學問，論文中都會有獨立的章節講述初始值如何設置。這方面可能得結合實際應用和效果得出最優的方案。

10Reference

[1] Roll and Pitch Angles From Accelerometer Sensors

[2] 四元數、歐拉角、旋轉矩陣轉換

[3] 四元素乘積求導

[4] 一種基于互補卡爾曼濾波算法計算融合姿態角度的方法

[5] Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter

[6] Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation

[7] Kalman Filter的原始論文

[8] 卡爾曼濾波基礎知識及公式推導

卡爾曼濾波的變種有哪些？1