卡爾曼濾波自從1960被Kalman發(fā)明并應(yīng)用到阿波羅登月計劃之后一直經(jīng)久不衰,，直到現(xiàn)在也被機(jī)器人,、自動駕駛、飛行控制等領(lǐng)域應(yīng)用,?；A(chǔ)卡爾曼濾波只能對線性系統(tǒng)建模；擴(kuò)展卡爾曼濾波對非線性方程做線性近似以便將卡爾曼濾波應(yīng)用到非線性系統(tǒng),。后來研究者發(fā)現(xiàn)將系統(tǒng)狀態(tài)分成主要成分和誤差,，并將卡爾曼濾波用來預(yù)測誤差，會使得系統(tǒng)的近似程度更高,，效果更好,。在姿態(tài)解算任務(wù)中，研究者們用輔助傳感器如加速度計和磁力計來修正角速度計的積分結(jié)果,，得到互補(bǔ)卡爾曼濾波的形式,。卡爾曼濾波是一種工具，對實(shí)際問題的不同建模方式會得到不同形式的卡爾曼濾波器,。這導(dǎo)致了對于初學(xué)者不知道從何看起是好。另外也似乎很少有文章對基礎(chǔ)卡爾曼濾波到各種形式的濾波形式做總結(jié)說明,，于是便有了這篇文章,。本文會從以下幾個方面分析和講解多種卡爾曼濾波器形式：- 基礎(chǔ)卡爾曼濾波——對線性系統(tǒng)的預(yù)測
- 擴(kuò)展卡爾曼濾波——基礎(chǔ)卡爾曼濾波在非線性系統(tǒng)的擴(kuò)展
- 基于四元素的卡爾曼濾波器——基于實(shí)際問題的講解
- 狀態(tài)誤差卡爾曼濾波——將狀態(tài)誤差引入狀態(tài)向量
- 互補(bǔ)卡爾曼濾波——一種只使用角度誤差和角速度誤差作為狀態(tài)向量和測量向量的濾波器形式
2符號定義小寫字母為變量；加粗小寫字母為向量,；大寫和加粗大寫為矩陣3基礎(chǔ)卡爾曼濾波器宏觀認(rèn)識- 預(yù)測（prediction）—— Dynamic model
- 更新（correction/measurment update）—— Observation model
所謂預(yù)測,，就是用一個數(shù)學(xué)模型，根據(jù)當(dāng)前的傳感器輸入,，直接計算此時系統(tǒng)的狀態(tài),?？梢岳斫鉃橐粋€方程的計算就行。所謂的更新,，就是在某些時刻或者每一時刻,，獲取一些系統(tǒng)的其他狀態(tài)輸入（我們將這個值叫做測量值），比較此刻預(yù)測的系統(tǒng)狀態(tài)和測量的系統(tǒng)狀態(tài),，對預(yù)測出的系統(tǒng)狀態(tài)進(jìn)行修正,，因此也叫測量更新（measurment update）。狀態(tài)方程及測量方程其中是系統(tǒng)狀態(tài)向量,，是測量向量,。分別是過程噪聲和觀測噪聲，且滿足高斯分布預(yù)測過程其中,，是先驗(yàn)狀態(tài)的誤差協(xié)方差矩陣更新過程詳細(xì)公式推導(dǎo)本文作為一篇概述性文章,，為了不使篇幅過于冗長，不進(jìn)行基礎(chǔ)卡爾曼濾波器公式的推導(dǎo),。想完全理解基礎(chǔ)卡爾曼濾波器可以參考下面這幾篇資料：- 卡爾曼濾波基礎(chǔ)知識及公式推導(dǎo)——較為形象化地講解預(yù)測和更新這兩個過程之間地概率分布關(guān)系
- wiki Kalman Filter——準(zhǔn)確的公式化推導(dǎo)
如何理解卡爾曼濾波器,？從概率分布的角度卡爾曼濾波器將系統(tǒng)狀態(tài)的變化中的過程噪聲假設(shè)為均值為0的高斯噪聲，使得狀態(tài)向量也變?yōu)橐粋€符合高斯分布的隨機(jī)向量,。同時對觀測過程的噪聲也假設(shè)為均值為0的高斯噪聲,。通過測量方程，即公式（1-2）得到將狀態(tài)向量映射到測量向量的函數(shù),。于是,，當(dāng)?shù)玫綔y量值的時候，可以利用測量值與狀態(tài)向量之間的關(guān)系得出另外一個對狀態(tài)向量的估計,。利用測量值得出的狀態(tài)估計和狀態(tài)方程計算的狀態(tài)均符合高斯分布,，兩個高斯分布的聯(lián)合概率分布依舊保持高斯特性。進(jìn)一步推導(dǎo)可以得到公式（1-5）到公式（1-7）,。關(guān)于這個角度的理解可以閱讀上面推薦的第一篇文章,。從最小化誤差的角度卡爾曼濾波的最終輸出是，真實(shí)的狀態(tài)為,，令對誤差的平方求最小值,，同樣可以推導(dǎo)出公式（1-5）到公式（1-7）。因此卡爾曼濾波器也是系統(tǒng)狀態(tài)的最優(yōu)估計,。關(guān)于這個角度的理解和推導(dǎo)可以參考上面推薦的第二篇文章,。4擴(kuò)展卡爾曼濾波非線性方程的線性近似卡爾曼濾波器建立在線性的狀態(tài)方程和測量方程也就是公式（1-1）和公式（1-2）。但是在實(shí)際應(yīng)用中,，更多的關(guān)系是非線形關(guān)系,，比如如何從連續(xù)的角速度計算出車輛當(dāng)前的姿態(tài)角等。為了能夠利用基本卡爾曼濾波器的預(yù)測和更新過程，對于非線性的狀態(tài)方程和觀測方程,，我們利用一階的泰勒展開,，將非線性公式近似為線性公式。狀態(tài)方程及測量方程公式（2-1,，2-2）可以類比基礎(chǔ)卡爾曼濾波器中的公式（1-1,，1-2）一階泰勒展開我們先假設(shè)已知時刻濾波器的輸出，也就是時刻的狀態(tài)后驗(yàn),，以及對應(yīng)的協(xié)方差矩陣為對公式（2-1）在這一點(diǎn)做展開，并只保留一次項(xiàng)同時,，對公式（2-2）在這一點(diǎn)做泰勒展開,，并只保留一次項(xiàng)預(yù)測方程及狀態(tài)協(xié)方差矩陣其中，公式（2-7）中的第二項(xiàng),，因?yàn)樵诰€性近似方程（2-4）中,，噪聲項(xiàng)滿足分布更新方程及卡爾曼增益5基礎(chǔ)卡爾曼濾波器 && 擴(kuò)展卡爾曼濾波器總結(jié)6基于四元數(shù)的拓展卡爾曼濾波器從實(shí)際問題講起在運(yùn)動物體的姿態(tài)估計，比如車輛的姿態(tài)計算中,，常常利用IMU(Inertial Meseasurment Unit)慣性測量單元計算物體的姿態(tài),。為了方便敘述，這里的姿態(tài)估計意味著我們希望解算車輛在每一時刻與起始坐標(biāo)系之間三個軸的偏轉(zhuǎn)角,，通常用roll,、pitch、yaw表示,。IMU(慣性測量單元)現(xiàn)在的IMU一般是六軸或者九軸,。六軸IMU可以輸出三個軸的加速度（acc）和角速度（gyro）；九軸則在六軸的基礎(chǔ)上增加了磁力計（Magnetometer）,，測量三軸的磁場方向,。為了簡化問題，我們用六軸IMU作為示例,。相關(guān)定義在姿態(tài)估計的各個領(lǐng)域中,，通常使用四元數(shù)來表示一個旋轉(zhuǎn)。四元數(shù)比歐拉角表達(dá)擁有更好的特性,，同時相比于旋轉(zhuǎn)矩陣又更加緊湊,。定義四元數(shù)如下為了估計系統(tǒng)的姿態(tài)，通常的做法是使用慣性測量單元IMU跟蹤角速度,，我們另每一時刻的角速度為另外,，我們需要知道IMU的輸出頻率。假設(shè)IMU的輸出時間間隔為四元素乘積的導(dǎo)數(shù)這部分是為了后面推導(dǎo)擴(kuò)展卡爾曼濾波的狀態(tài)方程中的雅各比矩陣準(zhǔn)備我們令時刻的旋轉(zhuǎn)為.時刻的旋轉(zhuǎn)為,。則的旋轉(zhuǎn)是時刻的旋轉(zhuǎn)經(jīng)過的變化得到的，即其中等于在時間間隔內(nèi)的角度變化量。在極短的時間間隔內(nèi),，我們認(rèn)為角度的變化是勻速的,，也就是。我們令在這個時間間隔內(nèi),，角度的旋轉(zhuǎn)軸為,，旋轉(zhuǎn)角度為,則根據(jù)四元數(shù)的基礎(chǔ)知識，我們可以得到擴(kuò)展卡爾曼濾波的重點(diǎn)之一在于求狀態(tài)方程相對于狀態(tài)的偏導(dǎo),；我們雖然可以從三軸角速度的輸出得出角度積分的離散形式公式（1）,，但是我們其實(shí)不能對其直接對求偏導(dǎo)。至于為什么不能直接求導(dǎo),，下面說得挺好:求導(dǎo)的定義是函數(shù)值的微增量關(guān)于自變量的微增量的極限,。表示旋轉(zhuǎn)的單位四元數(shù)作差后，其不再是單位四元數(shù),，也就不是旋轉(zhuǎn)四元數(shù)了 為了能夠?qū)剑?）求出偏導(dǎo)數(shù),，我們先求旋轉(zhuǎn)相對時間的導(dǎo)數(shù)。然后利用泰勒展開,，將公式（1）展開為線性方程,，并只取一次項(xiàng)，這樣就可以得到有了角度相對于時間的導(dǎo)數(shù)之后,，我們可以將公式（1）用泰勒展開狀態(tài)向量及控制輸入我們直接將車輛的姿態(tài)角以四元數(shù)形式表達(dá)作為系統(tǒng)狀態(tài)向量狀態(tài)方程及其雅各比矩陣有了上一部分關(guān)于四元素導(dǎo)數(shù)的推導(dǎo),，我們可以直接寫出狀態(tài)方程如下其中是隨機(jī)噪聲,，其協(xié)方差矩陣為預(yù)測方程由于在上面的推導(dǎo)中已經(jīng)求出雅各比矩陣，所以預(yù)測方程可以直接根據(jù)拓展卡爾曼濾波公式寫出來測量更新前文我們使用了角速度計的輸出作為控制輸入,，并構(gòu)建了狀態(tài)方程和預(yù)測方程,。IMU通常都會有加速度計輸出，這部分輸出可以用來作為測量量,，并對預(yù)測的狀態(tài)進(jìn)行測量更新,。我們先回顧測量更新中狀態(tài)向量的更新過程。測量模型現(xiàn)在我們已經(jīng)確定好了狀態(tài)向量為姿態(tài)角的四元數(shù)表達(dá),，測量向量為加速度計三個軸的輸出,，那么函數(shù)要采用什么形式可以將姿態(tài)角轉(zhuǎn)成三軸加速度呢？其中的關(guān)鍵聯(lián)系就是,，當(dāng)車輛靜止時,，加速度的合向量是重力加速度，垂直向下,！上圖中,，假設(shè)坐標(biāo)系是起始坐標(biāo)系，是小車移動后的坐標(biāo)系。在起始時,，小車靜止,，小車的重力加速度的輸出是垂直向下的，即我們采用歸一化的形式,，也就是將重力加速度當(dāng)作一個單位,。則當(dāng)小車運(yùn)動后，重力加速度在坐標(biāo)系下的表達(dá)為和是重力加速度在兩個不同坐標(biāo)系下的表達(dá),，同時,，這兩個坐標(biāo)系之間的旋轉(zhuǎn)是此時的狀態(tài)向量，因此是將四元數(shù)轉(zhuǎn)成旋轉(zhuǎn)矩陣的表達(dá),，旋轉(zhuǎn)矩陣左乘一個三位的列向量表示將該向量進(jìn)行三維旋轉(zhuǎn)。即下面的形式（是我們之前定義過的）雅各比矩陣從公式（3-7）我們可以得到測量模型中的轉(zhuǎn)換函數(shù)的雅各比矩陣更新方程7狀態(tài)誤差卡爾曼濾波器（ErKF : Error-state Kalman Filter)概述在使用卡爾曼濾波器做姿態(tài)估計（Attitude Estimation）中,，很大一部分都采用不是直接將系統(tǒng)姿態(tài)角作為卡爾曼濾波的狀態(tài),，而是將姿態(tài)角的積分誤差和角速度計的誤差作為系統(tǒng)狀態(tài)。將角速度計的輸出彌補(bǔ)上估計出的角速度計誤差,，然后對其積分,，得到姿態(tài)角的估計，再彌補(bǔ)上姿態(tài)角的誤差估計,。整個的流程圖大概如下面的圖,，引用自Intertial Head-Tracker Sensor Fusion by a Complementary Separate-Bias Kalman FilterPS: 要強(qiáng)調(diào)的是，各種卡爾曼濾波的形式多種多樣,，同時各種符號的定義也都并不完全一致,，這也是入門卡爾曼濾波比較難的地方，有時候找資料都不知道怎么找,。這也是寫這篇文章的目的,，提供一個基礎(chǔ)的脈絡(luò)給卡爾曼濾波的初學(xué)者。因此這里給出的ErKF只是形式之一,，主要是引用自論文Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation狀態(tài)誤差的遞推公式首先,，我們令表示時間連續(xù)形式的狀態(tài)向量。其相對于時間的導(dǎo)數(shù)為在等號的右邊輸入里加入微小擾動,，同時,，根據(jù)泰勒展開將函數(shù)展開于是，有了狀態(tài)誤差的遞推公式,，我們就可以像卡爾曼濾波一樣推導(dǎo)預(yù)測和更新過程預(yù)測過程與直接對系統(tǒng)狀態(tài)做卡爾曼濾波稍有不同,，使用誤差狀態(tài)的卡爾曼濾波在計算姿態(tài)角的時候可以看成三步：- 在卡爾曼濾波系統(tǒng)外使用積分算出此時的系統(tǒng)狀態(tài)
- 使用卡爾曼濾波算出此時系統(tǒng)狀態(tài)的誤差
- 將積分出來的系統(tǒng)狀態(tài)彌補(bǔ)上卡爾曼濾波計算出誤差
系統(tǒng)狀態(tài)計算上式只是一個公式化表達(dá)，其實(shí)就是將上一時刻的狀態(tài),，在加上這一時間段狀態(tài)的變化量,。姿態(tài)估計中,，狀態(tài)的變化量通常是角速度計的輸出乘以時間間隔。狀態(tài)誤差方程及預(yù)測方程由公式（4-1）可以得到狀態(tài)誤差的方程為測量方程及更新方程這里我們直接將使用其余傳感器如加速度計,、磁力計計算出的姿態(tài)作為系統(tǒng)的測量量。在這里先記得,，加速度計根據(jù)輸出的夾角可以計算出角,，由磁力計可以計算出角即可。當(dāng)然,，也可以采用其他能夠直接輸出系統(tǒng)姿態(tài)角的傳感器作為測量值,。- 更新狀態(tài)的估計（即把狀態(tài)誤差彌補(bǔ)到)
補(bǔ)充在上面的推導(dǎo)中,，將我們的目標(biāo)變量，即系統(tǒng)的姿態(tài)角作為外部一個單獨(dú)的積分計算,，但是實(shí)際上更多的做法是將姿態(tài)角和角速度的偏差直接放在狀態(tài)向量中進(jìn)行計算,。然后對每一時刻的角速度偏差應(yīng)用到角速度計的輸出，再將其作為系統(tǒng)輸入應(yīng)用到狀態(tài)方程,。也就是像概述中的圖示那樣,。但是其實(shí)各種卡爾曼濾波的建模方式都不一樣，Error-state Kalman Filter和Complimentarty Kalman Filter也沒有嚴(yán)格的定義,。所以索性這一章節(jié)當(dāng)作對狀態(tài)誤差的理解和推導(dǎo),，在下面的互補(bǔ)卡爾曼濾波給出一種似乎是應(yīng)用更廣泛的卡爾曼濾波器。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)識看起來都很不錯,，很適合想要將卡爾曼濾波應(yīng)用到姿態(tài)估計的工程師閱讀,。甚至有一些工作,，直接使用普通卡爾曼濾波輸出，然后利用互補(bǔ)濾波器的概念,，在多種姿態(tài)輸出之間做加權(quán)平均,，也叫互補(bǔ)卡爾曼濾波器，比如這篇專利：一種基于互補(bǔ)卡爾曼濾波算法計算融合姿態(tài)角度的方法互補(bǔ)的概念其實(shí)只要有了角速度計（Gyroscope）我們就可以根據(jù)其輸出,，并在時間上做積分解算出車輛的姿態(tài)角,。但是由于任何傳感器都是帶噪聲的，同時,，直接用積分解算,，誤差會隨機(jī)時間推移而累積，最終的姿態(tài)解算精度就非常差,。除了使用角速度計進(jìn)行積分可以得到姿態(tài)角之外,，用加速度計（Accelerometer）和磁力計（Magnatometer）也可以計算出動態(tài)系統(tǒng)的姿態(tài)。其中,，加速度計使用重力加速度作為錨定量,，測量靜止?fàn)顟B(tài)下三軸加速度之間的夾角，可以計算出角,；磁力計（或叫地磁計）利用地球磁場北極作為錨定量,，可以計算出角。使用這兩者對角速度計進(jìn)行補(bǔ)充,，可以得出更加準(zhǔn)確的姿態(tài)估計,。從加速度計計算姿態(tài)角roll、pitch加速度計（Accelerometer）可以輸出三個軸的加速度,，在靜止的情況下,，三個軸的合向量就是重力加速度。因此,，我們可以利用三個軸加速度之間的關(guān)系計算靜止?fàn)顟B(tài)下的俯仰角（pitch）和翻滾角（roll）關(guān)于如何推導(dǎo)從三個軸的加速度計算roll和pitch,，可以看這篇文章從磁力計計算姿態(tài)角yaw磁力計的三軸合向量會指向地磁北極，利用這一特性,，可以從磁力計的輸出獲得與地磁北極的偏轉(zhuǎn)角,。利用這一點(diǎn)，可以計算出相對于起始位置的角從加速度計算的姿態(tài)彌補(bǔ)從加速度計可以計算出roll角和pitch角,，因此,，可以將這個結(jié)果和角速度的積分結(jié)果結(jié)合起來，得到一個更好的估計姿態(tài),。不過要注意的是,，從加速度計算的姿態(tài)彌補(bǔ)有兩個局限：- 加速度計只能計算出Roll角和Pitch角，因此yaw角無法得到互補(bǔ)信息
- 當(dāng)車輛處于較大的加速度運(yùn)動狀態(tài)時,，三軸加速度的合向量跟重力加速度會有偏差,，因此互補(bǔ)結(jié)果應(yīng)該根據(jù)這個偏差的大小做改變,。
互補(bǔ)濾波器互補(bǔ)濾波器使用角速度的積分結(jié)果和加速度與磁力計的計算結(jié)果進(jìn)行插值，得到更好的結(jié)果其中,，是對角速度積分得出的姿態(tài)（用四元素表達(dá)）,；是使用加速度計和磁力計計算出的姿態(tài)。互補(bǔ)卡爾曼濾波器互補(bǔ)卡爾曼濾波器將姿態(tài)角的誤差,、角速度的誤差當(dāng)作狀態(tài)向量,；并利用其余傳感器如加速度計和磁力計計算出的姿態(tài)角與估計的姿態(tài)角之間的差作為測量量。通過以下步驟得到系統(tǒng)的姿態(tài)角：- 卡爾曼濾波器輸出姿態(tài)角的誤差和角速度的誤差
- 將當(dāng)前時刻角速度的輸出加上角速度的誤差,，并利用積分公式算出姿態(tài)角
- 將步驟2算出的姿態(tài)角加上步驟1輸出的姿態(tài)角誤差
狀態(tài)向量和測量向量其中,，是使用加速度計和磁力計計算出的系統(tǒng)角狀態(tài)方程我們可以推導(dǎo)出姿態(tài)角對于時間的導(dǎo)數(shù)，通常這個導(dǎo)數(shù)是姿態(tài)角和角速度的函數(shù),，即從公式（4-1）的推導(dǎo)過程,，以及泰勒展開，我們可以得到的遞推公式我們直接假設(shè)角速度的誤差是一個常量誤差,，即測量方程預(yù)測&&更新過程有了上面狀態(tài)方程和測量方程的推導(dǎo),，剩下的就是按照公式（4-3）到公式（4-10）的過程代入。這里唯一不同的就是卡爾曼濾波輸出的向量是角速度的誤差和姿態(tài)的誤差,。在計算姿態(tài)的時候先將角速度的誤差應(yīng)用到角速度計的數(shù)據(jù),，對角度積分，將角度的誤差應(yīng)用到角度積分結(jié)果,，得到最終的角度輸出,。整個框架如下圖9后話- 卡爾曼濾波是一個很古老的算法，但同時又是被廣泛應(yīng)用的算法,。即使在今天姿態(tài)解算中很多用了因子圖,，但是對IMU的預(yù)積分依舊要使用卡爾曼濾波。但是卡爾曼濾波算法只是一個工具,，不同的系統(tǒng)建模方式會產(chǎn)生不同形式的卡爾曼濾波器,，這也導(dǎo)致了初學(xué)者不知道從哪里入手。
- 在查資料的過程中發(fā)現(xiàn),，卡爾曼濾波的一些變種如Error-State Kalman Filter和Complimentary Kalman Filter其實(shí)并不是嚴(yán)格定義的,。
- 筆者對卡爾曼濾波并沒有很豐富的應(yīng)用經(jīng)驗(yàn)，本身也不是專門做運(yùn)動控制和姿態(tài)解算的,。本文的敘述在追求通俗易懂之外盡力保證公式和符號定義的準(zhǔn)確,。但無法保證沒有錯誤。
- 對于卡爾曼濾波器中各個變量的初始值設(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)換[4] 一種基于互補(bǔ)卡爾曼濾波算法計算融合姿態(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[8] 卡爾曼濾波基礎(chǔ)知識及公式推導(dǎo)[9] AHRS: Attitude and Heading Reference Systems本文僅做學(xué)術(shù)分享,，如有侵權(quán)，請聯(lián)系刪文,。
|
