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

8互補(bǔ)卡爾曼濾波

前言

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

互補(bǔ)的概念

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

從加速度計(jì)計(jì)算姿態(tài)角roll、pitch

加速度計(jì)（Accelerometer）可以輸出三個(gè)軸的加速度，在靜止的情況下，三個(gè)軸的合向量就是重力加速度。因此，我們可以利用三個(gè)軸加速度之間的關(guān)系計(jì)算靜止?fàn)顟B(tài)下的俯仰角（pitch）和翻滾角（roll）

關(guān)于如何推導(dǎo)從三個(gè)軸的加速度計(jì)算roll和pitch，可以看這篇文章

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

從磁力計(jì)計(jì)算姿態(tài)角yaw

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

具體的計(jì)算公式可以看這篇博客

從加速度計(jì)算的姿態(tài)彌補(bǔ)

從加速度計(jì)可以計(jì)算出roll角和pitch角，因此，可以將這個(gè)結(jié)果和角速度的積分結(jié)果結(jié)合起來，得到一個(gè)更好的估計(jì)姿態(tài)。不過要注意的是，從加速度計(jì)算的姿態(tài)彌補(bǔ)有兩個(gè)局限：

1. 加速度計(jì)只能計(jì)算出Roll角和Pitch角，因此yaw角無法得到互補(bǔ)信息
2. 當(dāng)車輛處于較大的加速度運(yùn)動狀態(tài)時(shí)，三軸加速度的合向量跟重力加速度會有偏差，因此互補(bǔ)結(jié)果應(yīng)該根據(jù)這個(gè)偏差的大小做改變。

互補(bǔ)濾波器

互補(bǔ)濾波器使用角速度的積分結(jié)果和加速度與磁力計(jì)的計(jì)算結(jié)果進(jìn)行插值，得到更好的結(jié)果

其中，是對角速度積分得出的姿態(tài)（用四元素表達(dá)）；是使用加速度計(jì)和磁力計(jì)計(jì)算出的姿態(tài)。

互補(bǔ)卡爾曼濾波器

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

1. 卡爾曼濾波器輸出姿態(tài)角的誤差和角速度的誤差
2. 將當(dāng)前時(shí)刻角速度的輸出加上角速度的誤差，并利用積分公式算出姿態(tài)角
3. 將步驟2算出的姿態(tài)角加上步驟1輸出的姿態(tài)角誤差

狀態(tài)向量和測量向量

狀態(tài)向量

其中是系統(tǒng)的角；是三軸角速度。

測量向量

其中，是使用加速度計(jì)和磁力計(jì)計(jì)算出的系統(tǒng)角

狀態(tài)方程

我們可以推導(dǎo)出姿態(tài)角對于時(shí)間的導(dǎo)數(shù)，通常這個(gè)導(dǎo)數(shù)是姿態(tài)角和角速度的函數(shù)，即

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

我們直接假設(shè)角速度的誤差是一個(gè)常量誤差，即

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

測量方程

預(yù)測&&更新過程

有了上面狀態(tài)方程和測量方程的推導(dǎo)，剩下的就是按照公式（4-3）到公式（4-10）的過程代入。這里唯一不同的就是卡爾曼濾波輸出的向量是角速度的誤差和姿態(tài)的誤差。在計(jì)算姿態(tài)的時(shí)候先將角速度的誤差應(yīng)用到角速度計(jì)的數(shù)據(jù)，對角度積分，將角度的誤差應(yīng)用到角度積分結(jié)果，得到最終的角度輸出。整個(gè)框架如下圖

9后話

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

10Reference

[1] Roll and Pitch Angles From Accelerometer Sensors

[2] 四元數(shù)、歐拉角、旋轉(zhuǎn)矩陣轉(zhuǎn)換

[3] 四元素乘積求導(dǎo)

[4] 一種基于互補(bǔ)卡爾曼濾波算法計(jì)算融合姿態(tài)角度的方法

[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] 卡爾曼濾波基礎(chǔ)知識及公式推導(dǎo)

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