1從基礎(chǔ)卡爾曼濾波到互補(bǔ)卡爾曼濾波

卡爾曼濾波自從1960被Kalman發(fā)明并應(yīng)用到阿波羅登月計(jì)劃之后一直經(jīng)久不衰，直到現(xiàn)在也被機(jī)器人、自動(dòng)駕駛、飛行控制等領(lǐng)域應(yīng)用。基礎(chǔ)卡爾曼濾波只能對(duì)線性系統(tǒng)建模；擴(kuò)展卡爾曼濾波對(duì)非線性方程做線性近似以便將卡爾曼濾波應(yīng)用到非線性系統(tǒng)。后來(lái)研究者發(fā)現(xiàn)將系統(tǒng)狀態(tài)分成主要成分和誤差，并將卡爾曼濾波用來(lái)預(yù)測(cè)誤差，會(huì)使得系統(tǒng)的近似程度更高，效果更好。在姿態(tài)解算任務(wù)中，研究者們用輔助傳感器如加速度計(jì)和磁力計(jì)來(lái)修正角速度計(jì)的積分結(jié)果，得到互補(bǔ)卡爾曼濾波的形式。 卡爾曼濾波是一種工具，對(duì)實(shí)際問(wèn)題的不同建模方式會(huì)得到不同形式的卡爾曼濾波器。這導(dǎo)致了對(duì)于初學(xué)者不知道從何看起是好。另外也似乎很少有文章對(duì)基礎(chǔ)卡爾曼濾波到各種形式的濾波形式做總結(jié)說(shuō)明，于是便有了這篇文章。本文會(huì)從以下幾個(gè)方面分析和講解多種卡爾曼濾波器形式：

基礎(chǔ)卡爾曼濾波——對(duì)線性系統(tǒng)的預(yù)測(cè)

擴(kuò)展卡爾曼濾波——基礎(chǔ)卡爾曼濾波在非線性系統(tǒng)的擴(kuò)展

基于四元素的卡爾曼濾波器——基于實(shí)際問(wèn)題的講解

狀態(tài)誤差卡爾曼濾波——將狀態(tài)誤差引入狀態(tài)向量

互補(bǔ)卡爾曼濾波——一種只使用角度誤差和角速度誤差作為狀態(tài)向量和測(cè)量向量的濾波器形式

2符號(hào)定義

小寫(xiě)字母為變量；加粗小寫(xiě)字母為向量；大寫(xiě)和加粗大寫(xiě)為矩陣

3基礎(chǔ)卡爾曼濾波器

宏觀認(rèn)識(shí)

卡爾曼濾波包含兩個(gè)步驟

預(yù)測(cè)（prediction）—— Dynamic model

更新（correction/measurment update）—— Observation model

所謂預(yù)測(cè)，就是用一個(gè)數(shù)學(xué)模型，根據(jù)當(dāng)前的傳感器輸入，直接計(jì)算此時(shí)系統(tǒng)的狀態(tài)。可以理解為一個(gè)方程的計(jì)算就行。 所謂的更新，就是在某些時(shí)刻或者每一時(shí)刻，獲取一些系統(tǒng)的其他狀態(tài)輸入（我們將這個(gè)值叫做測(cè)量值），比較此刻預(yù)測(cè)的系統(tǒng)狀態(tài)和測(cè)量的系統(tǒng)狀態(tài)，對(duì)預(yù)測(cè)出的系統(tǒng)狀態(tài)進(jìn)行修正，因此也叫測(cè)量更新（measurment update）。 整體框架如下圖所示

狀態(tài)方程及測(cè)量方程

其中是系統(tǒng)狀態(tài)向量，是測(cè)量向量。分別是過(guò)程噪聲和觀測(cè)噪聲，且滿足高斯分布 ? ?

預(yù)測(cè)過(guò)程

其中，是先驗(yàn)狀態(tài)的誤差協(xié)方差矩陣 ?

更新過(guò)程

詳細(xì)公式推導(dǎo)

本文作為一篇概述性文章，為了不使篇幅過(guò)于冗長(zhǎng)，不進(jìn)行基礎(chǔ)卡爾曼濾波器公式的推導(dǎo)。想完全理解基礎(chǔ)卡爾曼濾波器可以參考下面這幾篇資料：

卡爾曼濾波基礎(chǔ)知識(shí)及公式推導(dǎo)——較為形象化地講解預(yù)測(cè)和更新這兩個(gè)過(guò)程之間地概率分布關(guān)系

wiki Kalman Filter——準(zhǔn)確的公式化推導(dǎo)

如何理解卡爾曼濾波器？

從概率分布的角度

卡爾曼濾波器將系統(tǒng)狀態(tài)的變化中的過(guò)程噪聲假設(shè)為均值為0的高斯噪聲，使得狀態(tài)向量也變?yōu)橐粋€(gè)符合高斯分布的隨機(jī)向量。同時(shí)對(duì)觀測(cè)過(guò)程的噪聲也假設(shè)為均值為0的高斯噪聲。通過(guò)測(cè)量方程，即公式（1-2）得到將狀態(tài)向量映射到測(cè)量向量的函數(shù)。于是，當(dāng)?shù)玫綔y(cè)量值的時(shí)候，可以利用測(cè)量值與狀態(tài)向量之間的關(guān)系得出另外一個(gè)對(duì)狀態(tài)向量的估計(jì)。利用測(cè)量值得出的狀態(tài)估計(jì)和狀態(tài)方程計(jì)算的狀態(tài)均符合高斯分布，兩個(gè)高斯分布的聯(lián)合概率分布依舊保持高斯特性。進(jìn)一步推導(dǎo)可以得到公式（1-5）到公式（1-7）。關(guān)于這個(gè)角度的理解可以閱讀上面推薦的第一篇文章。

從最小化誤差的角度

卡爾曼濾波的最終輸出是，真實(shí)的狀態(tài)為，令 ? 對(duì)誤差的平方求最小值，同樣可以推導(dǎo)出公式（1-5）到公式（1-7）。因此卡爾曼濾波器也是系統(tǒng)狀態(tài)的最優(yōu)估計(jì)。關(guān)于這個(gè)角度的理解和推導(dǎo)可以參考上面推薦的第二篇文章。

4擴(kuò)展卡爾曼濾波

非線性方程的線性近似

卡爾曼濾波器建立在線性的狀態(tài)方程和測(cè)量方程也就是公式（1-1）和公式（1-2）。但是在實(shí)際應(yīng)用中，更多的關(guān)系是非線形關(guān)系，比如如何從連續(xù)的角速度計(jì)算出車輛當(dāng)前的姿態(tài)角等。為了能夠利用基本卡爾曼濾波器的預(yù)測(cè)和更新過(guò)程，對(duì)于非線性的狀態(tài)方程和觀測(cè)方程，我們利用一階的泰勒展開(kāi)，將非線性公式近似為線性公式。

狀態(tài)方程及測(cè)量方程

公式（2-1，2-2）可以類比基礎(chǔ)卡爾曼濾波器中的公式（1-1，1-2）

一階泰勒展開(kāi)

我們先假設(shè)已知時(shí)刻濾波器的輸出，也就是時(shí)刻的狀態(tài)后驗(yàn)，以及對(duì)應(yīng)的協(xié)方差矩陣為 ? 同時(shí)，我們令的先驗(yàn)為 ? 對(duì)公式（2-1）在這一點(diǎn)做展開(kāi)，并只保留一次項(xiàng) ? 同時(shí)，對(duì)公式（2-2）在這一點(diǎn)做泰勒展開(kāi)，并只保留一次項(xiàng) ? 在公式（2-4）和公式（2-5）中：

預(yù)測(cè)方程及狀態(tài)協(xié)方差矩陣

其中，公式（2-7）中的第二項(xiàng)，因?yàn)樵诰€性近似方程（2-4）中，噪聲項(xiàng)滿足分布 ?

更新方程及卡爾曼增益

5基礎(chǔ)卡爾曼濾波器 && 擴(kuò)展卡爾曼濾波器總結(jié)

?

6基于四元數(shù)的拓展卡爾曼濾波器

從實(shí)際問(wèn)題講起

在運(yùn)動(dòng)物體的姿態(tài)估計(jì)，比如車輛的姿態(tài)計(jì)算中，常常利用IMU(Inertial Meseasurment Unit)慣性測(cè)量單元計(jì)算物體的姿態(tài)。為了方便敘述，這里的姿態(tài)估計(jì)意味著我們希望解算車輛在每一時(shí)刻與起始坐標(biāo)系之間三個(gè)軸的偏轉(zhuǎn)角，通常用roll、pitch、yaw表示。

?

IMU(慣性測(cè)量單元)

現(xiàn)在的IMU一般是六軸或者九軸。六軸IMU可以輸出三個(gè)軸的加速度（acc）和角速度（gyro）；九軸則在六軸的基礎(chǔ)上增加了磁力計(jì)（Magnetometer），測(cè)量三軸的磁場(chǎng)方向。為了簡(jiǎn)化問(wèn)題，我們用六軸IMU作為示例。

相關(guān)定義

在姿態(tài)估計(jì)的各個(gè)領(lǐng)域中，通常使用四元數(shù)來(lái)表示一個(gè)旋轉(zhuǎn)。四元數(shù)比歐拉角表達(dá)擁有更好的特性，同時(shí)相比于旋轉(zhuǎn)矩陣又更加緊湊。定義四元數(shù)如下 ? 為了估計(jì)系統(tǒng)的姿態(tài)，通常的做法是使用慣性測(cè)量單元IMU跟蹤角速度，我們另每一時(shí)刻的角速度為 ? 另外，我們需要知道IMU的輸出頻率。假設(shè)IMU的輸出時(shí)間間隔為

四元素乘積的導(dǎo)數(shù)

這部分是為了后面推導(dǎo)擴(kuò)展卡爾曼濾波的狀態(tài)方程中的雅各比矩陣準(zhǔn)備 我們令時(shí)刻的旋轉(zhuǎn)為.時(shí)刻的旋轉(zhuǎn)為。則的旋轉(zhuǎn)是時(shí)刻的旋轉(zhuǎn)經(jīng)過(guò)的變化得到的，即 ? 其中等于在時(shí)間間隔內(nèi)的角度變化量。在極短的時(shí)間間隔內(nèi)，我們認(rèn)為角度的變化是勻速的，也就是。我們令在這個(gè)時(shí)間間隔內(nèi)，角度的旋轉(zhuǎn)軸為，旋轉(zhuǎn)角度為,則 ? ? 根據(jù)四元數(shù)的基礎(chǔ)知識(shí)，我們可以得到 ? 擴(kuò)展卡爾曼濾波的重點(diǎn)之一在于求狀態(tài)方程相對(duì)于狀態(tài)的偏導(dǎo)；我們雖然可以從三軸角速度的輸出得出角度積分的離散形式公式（1），但是我們其實(shí)不能對(duì)其直接對(duì)求偏導(dǎo)。至于為什么不能直接求導(dǎo)，下面說(shuō)得挺好:

求導(dǎo)的定義是函數(shù)值的微增量關(guān)于自變量的微增量的極限。表示旋轉(zhuǎn)的單位四元數(shù)作差后，其不再是單位四元數(shù)，也就不是旋轉(zhuǎn)四元數(shù)了

為了能夠?qū)剑?）求出偏導(dǎo)數(shù)，我們先求旋轉(zhuǎn)相對(duì)時(shí)間的導(dǎo)數(shù)。然后利用泰勒展開(kāi)，將公式（1）展開(kāi)為線性方程，并只取一次項(xiàng)，這樣就可以得到 ? 我們令 則 ? 有了角度相對(duì)于時(shí)間的導(dǎo)數(shù)之后，我們可以將公式（1）用泰勒展開(kāi) ? 只保留到一次項(xiàng)，則我們可以得到 ?

狀態(tài)向量及控制輸入

我們直接將車輛的姿態(tài)角以四元數(shù)形式表達(dá)作為系統(tǒng)狀態(tài)向量 同時(shí)，將每一時(shí)刻IMU的三軸角速度作為控制輸入

狀態(tài)方程及其雅各比矩陣

有了上一部分關(guān)于四元素導(dǎo)數(shù)的推導(dǎo)，我們可以直接寫(xiě)出狀態(tài)方程如下 ? 其中是隨機(jī)噪聲，其協(xié)方差矩陣為

預(yù)測(cè)方程

由于在上面的推導(dǎo)中已經(jīng)求出雅各比矩陣，所以預(yù)測(cè)方程可以直接根據(jù)拓展卡爾曼濾波公式寫(xiě)出來(lái) ? ?

測(cè)量更新

前文我們使用了角速度計(jì)的輸出作為控制輸入，并構(gòu)建了狀態(tài)方程和預(yù)測(cè)方程。IMU通常都會(huì)有加速度計(jì)輸出，這部分輸出可以用來(lái)作為測(cè)量量，并對(duì)預(yù)測(cè)的狀態(tài)進(jìn)行測(cè)量更新。我們先回顧測(cè)量更新中狀態(tài)向量的更新過(guò)程。 ? 令加速度計(jì)的輸出為測(cè)量向量：

測(cè)量模型

現(xiàn)在我們已經(jīng)確定好了狀態(tài)向量為姿態(tài)角的四元數(shù)表達(dá)，測(cè)量向量為加速度計(jì)三個(gè)軸的輸出，那么函數(shù)要采用什么形式可以將姿態(tài)角轉(zhuǎn)成三軸加速度呢？ 其中的關(guān)鍵聯(lián)系就是，當(dāng)車輛靜止時(shí)，加速度的合向量是重力加速度，垂直向下！

上圖中，假設(shè)坐標(biāo)系是起始坐標(biāo)系，是小車移動(dòng)后的坐標(biāo)系。在起始時(shí)，小車靜止，小車的重力加速度的輸出是垂直向下的，即 ? 我們采用歸一化的形式，也就是將重力加速度當(dāng)作一個(gè)單位。 則當(dāng)小車運(yùn)動(dòng)后，重力加速度在坐標(biāo)系下的表達(dá)為 ? 和是重力加速度在兩個(gè)不同坐標(biāo)系下的表達(dá)，同時(shí)，這兩個(gè)坐標(biāo)系之間的旋轉(zhuǎn)是此時(shí)的狀態(tài)向量，因此 ? 其中， ? ? 是將四元數(shù)轉(zhuǎn)成旋轉(zhuǎn)矩陣的表達(dá)，旋轉(zhuǎn)矩陣左乘一個(gè)三位的列向量表示將該向量進(jìn)行三維旋轉(zhuǎn)。即下面的形式（是我們之前定義過(guò)的）

雅各比矩陣

從公式（3-7）我們可以得到測(cè)量模型中的轉(zhuǎn)換函數(shù)的雅各比矩陣

更新方程

7狀態(tài)誤差卡爾曼濾波器（ErKF : Error-state Kalman Filter)

概述

在使用卡爾曼濾波器做姿態(tài)估計(jì)（Attitude Estimation）中，很大一部分都采用不是直接將系統(tǒng)姿態(tài)角作為卡爾曼濾波的狀態(tài)，而是將姿態(tài)角的積分誤差和角速度計(jì)的誤差作為系統(tǒng)狀態(tài)。將角速度計(jì)的輸出彌補(bǔ)上估計(jì)出的角速度計(jì)誤差，然后對(duì)其積分，得到姿態(tài)角的估計(jì)，再?gòu)浹a(bǔ)上姿態(tài)角的誤差估計(jì)。整個(gè)的流程圖大概如下面的圖，引用自Intertial Head-Tracker Sensor Fusion by a Complementary Separate-Bias Kalman Filter

PS: 要強(qiáng)調(diào)的是，各種卡爾曼濾波的形式多種多樣，同時(shí)各種符號(hào)的定義也都并不完全一致，這也是入門(mén)卡爾曼濾波比較難的地方，有時(shí)候找資料都不知道怎么找。這也是寫(xiě)這篇文章的目的，提供一個(gè)基礎(chǔ)的脈絡(luò)給卡爾曼濾波的初學(xué)者。因此這里給出的ErKF只是形式之一，主要是引用自論文Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation

狀態(tài)誤差的遞推公式

首先，我們令表示時(shí)間連續(xù)形式的狀態(tài)向量。其相對(duì)于時(shí)間的導(dǎo)數(shù)為 ? 在等號(hào)的右邊輸入里加入微小擾動(dòng)，同時(shí)，根據(jù)泰勒展開(kāi)將函數(shù)展開(kāi) ? 則 ? 將上式使用離散形式表達(dá) ? 即 ? 于是，有了狀態(tài)誤差的遞推公式，我們就可以像卡爾曼濾波一樣推導(dǎo)預(yù)測(cè)和更新過(guò)程

預(yù)測(cè)過(guò)程

與直接對(duì)系統(tǒng)狀態(tài)做卡爾曼濾波稍有不同，使用誤差狀態(tài)的卡爾曼濾波在計(jì)算姿態(tài)角的時(shí)候可以看成三步：

在卡爾曼濾波系統(tǒng)外使用積分算出此時(shí)的系統(tǒng)狀態(tài)

使用卡爾曼濾波算出此時(shí)系統(tǒng)狀態(tài)的誤差

將積分出來(lái)的系統(tǒng)狀態(tài)彌補(bǔ)上卡爾曼濾波計(jì)算出誤差

系統(tǒng)狀態(tài)計(jì)算

上式只是一個(gè)公式化表達(dá)，其實(shí)就是將上一時(shí)刻的狀態(tài)，在加上這一時(shí)間段狀態(tài)的變化量。姿態(tài)估計(jì)中，狀態(tài)的變化量通常是角速度計(jì)的輸出乘以時(shí)間間隔。

狀態(tài)誤差方程及預(yù)測(cè)方程

狀態(tài)誤差方程 由公式（4-1）可以得到狀態(tài)誤差的方程為 ? 其中，是過(guò)程噪聲，協(xié)方差矩陣為 預(yù)測(cè)方程 類似于普通卡爾曼濾波，預(yù)測(cè)方程為 ? ? ? ?

測(cè)量方程及更新方程

這里我們直接將使用其余傳感器如加速度計(jì)、磁力計(jì)計(jì)算出的姿態(tài)作為系統(tǒng)的測(cè)量量。在這里先記得，加速度計(jì)根據(jù)輸出的夾角可以計(jì)算出角，由磁力計(jì)可以計(jì)算出角即可。當(dāng)然，也可以采用其他能夠直接輸出系統(tǒng)姿態(tài)角的傳感器作為測(cè)量值。 測(cè)量方程 ? 其中，是測(cè)量噪聲，協(xié)方差矩陣為 更新方程 這里要執(zhí)行兩步更新

先更新對(duì)狀態(tài)誤差的估計(jì)

更新?tīng)顟B(tài)的估計(jì)（即把狀態(tài)誤差彌補(bǔ)到)

補(bǔ)充

在上面的推導(dǎo)中，將我們的目標(biāo)變量，即系統(tǒng)的姿態(tài)角作為外部一個(gè)單獨(dú)的積分計(jì)算，但是實(shí)際上更多的做法是將姿態(tài)角和角速度的偏差直接放在狀態(tài)向量中進(jìn)行計(jì)算。然后對(duì)每一時(shí)刻的角速度偏差應(yīng)用到角速度計(jì)的輸出，再將其作為系統(tǒng)輸入應(yīng)用到狀態(tài)方程。也就是像概述中的圖示那樣。但是其實(shí)各種卡爾曼濾波的建模方式都不一樣，Error-state Kalman Filter和Complimentarty Kalman Filter也沒(méi)有嚴(yán)格的定義。所以索性這一章節(jié)當(dāng)作對(duì)狀態(tài)誤差的理解和推導(dǎo)，在下面的互補(bǔ)卡爾曼濾波給出一種似乎是應(yīng)用更廣泛的卡爾曼濾波器。

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

前言

正如前文所說(shuō)，卡爾曼濾波器的建模形式多種多樣，而且很多研究也是在上世紀(jì)，對(duì)于誤差狀態(tài)卡爾曼濾波（Error-state Kalman Filter)和互補(bǔ)卡爾曼濾波（Comlimentary Kalman Filter)其實(shí)沒(méi)有嚴(yán)格的定義。這里的互補(bǔ)卡爾曼濾波其實(shí)也可以看成上文ErKF的另一種形式。主要采用自論文Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filter。卡爾曼濾波的工作太多，博客和論文也五花八門(mén)，看起來(lái)十分不易。這篇論文從引用、論文敘述、符號(hào)標(biāo)識(shí)看起來(lái)都很不錯(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í)，直接用積分解算，誤差會(huì)隨機(jī)時(shí)間推移而累積，最終的姿態(tài)解算精度就非常差。除了使用角速度計(jì)進(jìn)行積分可以得到姿態(tài)角之外，用加速度計(jì)（Accelerometer）和磁力計(jì)（Magnatometer）也可以計(jì)算出動(dòng)態(tài)系統(tǒng)的姿態(tài)。其中，加速度計(jì)使用重力加速度作為錨定量，測(cè)量靜止?fàn)顟B(tài)下三軸加速度之間的夾角，可以計(jì)算出角；磁力計(jì)（或叫地磁計(jì)）利用地球磁場(chǎng)北極作為錨定量，可以計(jì)算出角。使用這兩者對(duì)角速度計(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，可以看這篇文章 最后得出的形式也非常簡(jiǎn)單： ? ? ?

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

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

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

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

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

加速度計(jì)只能計(jì)算出Roll角和Pitch角，因此yaw角無(wú)法得到互補(bǔ)信息

當(dāng)車輛處于較大的加速度運(yùn)動(dòng)狀態(tài)時(shí)，三軸加速度的合向量跟重力加速度會(huì)有偏差，因此互補(bǔ)結(jié)果應(yīng)該根據(jù)這個(gè)偏差的大小做改變。

互補(bǔ)濾波器

互補(bǔ)濾波器使用角速度的積分結(jié)果和加速度與磁力計(jì)的計(jì)算結(jié)果進(jìn)行插值，得到更好的結(jié)果 ? 其中，是對(duì)角速度積分得出的姿態(tài)（用四元素表達(dá)）；是使用加速度計(jì)和磁力計(jì)計(jì)算出的姿態(tài)。

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

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

卡爾曼濾波器輸出姿態(tài)角的誤差和角速度的誤差

將當(dāng)前時(shí)刻角速度的輸出加上角速度的誤差，并利用積分公式算出姿態(tài)角

將步驟2算出的姿態(tài)角加上步驟1輸出的姿態(tài)角誤差

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

狀態(tài)向量 ? 其中是系統(tǒng)的角；是三軸角速度。 測(cè)量向量 ? 其中，是使用加速度計(jì)和磁力計(jì)計(jì)算出的系統(tǒng)角

狀態(tài)方程

我們可以推導(dǎo)出姿態(tài)角對(duì)于時(shí)間的導(dǎo)數(shù)，通常這個(gè)導(dǎo)數(shù)是姿態(tài)角和角速度的函數(shù)，即 ? 從公式（4-1）的推導(dǎo)過(guò)程，以及泰勒展開(kāi)，我們可以得到的遞推公式 ? 我們直接假設(shè)角速度的誤差是一個(gè)常量誤差，即 ? 則我們可以將上面兩式寫(xiě)成矩陣形式 ?

測(cè)量方程

預(yù)測(cè)&&更新過(guò)程

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

9后話

卡爾曼濾波是一個(gè)很古老的算法，但同時(shí)又是被廣泛應(yīng)用的算法。即使在今天姿態(tài)解算中很多用了因子圖，但是對(duì)IMU的預(yù)積分依舊要使用卡爾曼濾波。但是卡爾曼濾波算法只是一個(gè)工具，不同的系統(tǒng)建模方式會(huì)產(chǎn)生不同形式的卡爾曼濾波器，這也導(dǎo)致了初學(xué)者不知道從哪里入手。

在查資料的過(guò)程中發(fā)現(xiàn)，卡爾曼濾波的一些變種如Error-State Kalman Filter和Complimentary Kalman Filter其實(shí)并不是嚴(yán)格定義的。

筆者對(duì)卡爾曼濾波并沒(méi)有很豐富的應(yīng)用經(jīng)驗(yàn)，本身也不是專門(mén)做運(yùn)動(dòng)控制和姿態(tài)解算的。本文的敘述在追求通俗易懂之外盡力保證公式和符號(hào)定義的準(zhǔn)確。但無(wú)法保證沒(méi)有錯(cuò)誤。

對(duì)于卡爾曼濾波器中各個(gè)變量的初始值設(shè)置是門(mén)學(xué)問(wèn)，論文中都會(huì)有獨(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ǔ)知識(shí)及公式推導(dǎo) [9] AHRS: Attitude and Heading Reference Systems

編輯：黃飛

?