卡爾曼濾波器簡(jiǎn)介

1、匯報(bào)人：長安大學(xué) 余洪凱卡爾曼濾波器簡(jiǎn)介1、Kalman Filter的起源：卡爾曼全名Rudolf Emil Kalman，匈牙利數(shù)學(xué)家，1930年出生于匈牙利首都布達(dá)佩斯。1953，1954年于麻省理工學(xué)院分別獲得電機(jī)工程學(xué)士及碩士學(xué)位。1957年于哥倫比亞大學(xué)獲得博士學(xué)位。我們現(xiàn)在要學(xué)習(xí)的卡爾曼濾波器，正是源于他的博士論文和1960年發(fā)表的論文A New Approach to Linear Filtering and Prediction Problems 2、Kalman Filter的應(yīng)用：飛躍：卡爾曼濾波器對(duì)于解決阿波羅計(jì)劃的軌道預(yù)測(cè)很有用，后來阿波羅飛船的導(dǎo)航電腦使用了這種濾波

2、器。他的廣泛應(yīng)用已經(jīng)超過30年，包括導(dǎo)航，控制，傳感器數(shù)據(jù)融合甚至在軍事方面的雷達(dá)系統(tǒng)以及導(dǎo)彈追蹤等等,尤其是在自動(dòng)或輔助導(dǎo)航系統(tǒng)。近年來更被應(yīng)用于計(jì)算機(jī)視覺領(lǐng)域，例如人臉識(shí)別，運(yùn)動(dòng)物體跟蹤等等。 3、Kalman Filter的思想：基本思想：卡爾曼濾波器提供了一種有效的以最小均方誤差來估算系統(tǒng)狀態(tài)計(jì)算遞歸方法。若有一組強(qiáng)而合理的假設(shè)，給出系統(tǒng)的歷史測(cè)量值，則可以建立最大化這些早前測(cè)量值的后驗(yàn)概率的系統(tǒng)狀態(tài)模型。并且無需存儲(chǔ)很長的早前測(cè)量歷史，我們也可以最大化后驗(yàn)概率，即重復(fù)更新系統(tǒng)狀態(tài)模型，并只為下一次更新保存模型。這樣就大大地簡(jiǎn)化了這個(gè)方法的計(jì)算機(jī)實(shí)現(xiàn)。Li113、Kalman Filt

3、er的思想：卡爾曼濾波器的兩個(gè)重要假設(shè)：1.被建模的系統(tǒng)是線性的：K時(shí)刻的系統(tǒng)狀態(tài)可以用某個(gè)矩陣與K-1時(shí)刻的系統(tǒng)狀態(tài)的乘積表示。2.影響測(cè)量的噪聲屬于高斯分布的白噪聲：噪聲與時(shí)間不相關(guān)，且只用均值和協(xié)方差（也就是噪聲完全由一階距和二階距描述）就可以準(zhǔn)確地為幅值建模。 這些假設(shè)實(shí)際上可以運(yùn)用在非常廣泛的普通環(huán)境中。4、Kalman Filter的數(shù)學(xué)基礎(chǔ)：假設(shè)你想知道某一個(gè)橫向運(yùn)動(dòng)物體的X坐標(biāo)，你將會(huì)得到兩個(gè)大概的測(cè)量值x1和x2。但這兩個(gè)測(cè)量值都不是很準(zhǔn)，即存在高斯不確定性，存在均值與標(biāo)準(zhǔn)差。我們希望在同時(shí)給出這兩個(gè)測(cè)量條件下出現(xiàn)某一x值的概率密度與p12(x)=p1(x)*p2(x)成比例

4、：121222()()( )exp22x xx xp x4、Kalman Filter的數(shù)學(xué)基礎(chǔ)：高斯分布在平均值處取得最大值，也就是說導(dǎo)數(shù)為0的點(diǎn)是高斯分布的均值。121211221212121222( ) 0 xdpxxxxp xdx 1212()0px2112121212222222xxx 4、Kalman Filter的數(shù)學(xué)基礎(chǔ)：上面我們得到了新的均值是兩個(gè)測(cè)量值的加權(quán)組合。我們把 代入 的表達(dá)式整理，得到：12x12( )px121222222 這說明：兩個(gè)高斯測(cè)量相結(jié)合等價(jià)于一個(gè)高斯測(cè)量（其均值與不確定性是可以計(jì)算的）。這個(gè)性質(zhì)的意思是，當(dāng)有M個(gè)高斯測(cè)量時(shí)，可以先合并前兩個(gè)，再講前

5、兩個(gè)合并的結(jié)果與第三個(gè)合并計(jì)算，以此類推。4、Kalman Filter的數(shù)學(xué)基礎(chǔ)：我們假設(shè)x1是時(shí)刻1的測(cè)量值， x2是時(shí)刻2的測(cè)量值。X2是時(shí)刻2的預(yù)測(cè)值。211212122222222Xxx 1121122222()Xxxx 4、Kalman Filter的數(shù)學(xué)基礎(chǔ)：我們成功計(jì)算出了預(yù)測(cè)值，那么這個(gè)預(yù)測(cè)值的協(xié)方差也可以表示出來：121222222 112222221 4、Kalman Filter的數(shù)學(xué)基礎(chǔ)：由此，我們可以得到遞歸循環(huán)迭代公式：其中K就是大名鼎鼎的卡爾曼增益。1212()X x Kx x 112222K 1221K 5、Kalman Filter的一個(gè)小例子假設(shè)我們要研究

6、的對(duì)象是一個(gè)房間的溫度。我們把這些偏差看成是高斯白噪聲（White Gaussian Noise），也就是這些偏差跟前后時(shí)間是沒有關(guān)系的而且符合高斯分配（Gaussian Distribution）。另外，我們?cè)诜块g里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的，測(cè)量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。5、Kalman Filter的一個(gè)小例子假設(shè)時(shí)刻1的你憑感覺估算溫度是23度，估算偏差是5度。然后，你從溫度計(jì)那里得到了時(shí)刻2的測(cè)試值是25度，溫度計(jì)的測(cè)試偏差是4度。那么實(shí)際值是多少呢？K2=52/(52+42)，所以K=0.78 那么時(shí)刻2的卡爾曼預(yù)測(cè)值：23+K*(25-23)

7、=24.56度 所以我們可以看出由于溫度計(jì)的協(xié)方差比較小，那么我們的預(yù)測(cè)值也就更偏向與溫度計(jì)。5、Kalman Filter的一個(gè)小例子那么時(shí)刻3的預(yù)測(cè)值怎么算呢？我們先算出時(shí)刻2那個(gè)預(yù)測(cè)值（24.56度）的偏差。算法如下：(1-K)*52)0.5=2.35。 那么就可以迭代了，假設(shè)時(shí)刻3溫度計(jì)測(cè)量值是26度。那么此時(shí)的 K2=2.352/(2.352+42)，所以K=0.25 那么時(shí)刻3的卡爾曼預(yù)測(cè)值：24.56+K*(26-24.56)=24.92度以后時(shí)刻的溫度值以此類推，慢慢迭代。5、Kalman Filter的一個(gè)小例子就是這樣，卡爾曼濾波器不斷把協(xié)方差遞歸，從而進(jìn)行估值預(yù)測(cè)。而且他

8、只保留上一個(gè)時(shí)刻的協(xié)方差，而且可以根據(jù)不同時(shí)刻的測(cè)量值進(jìn)而改變自己的估算值。如果在現(xiàn)在計(jì)算機(jī)中實(shí)現(xiàn)，程序簡(jiǎn)單，計(jì)算也十分快速。以上只是卡爾曼濾波器的數(shù)學(xué)基礎(chǔ)，下面我們介紹卡爾曼濾波器算法。6、卡爾曼濾波器算法 我們先要引入一個(gè)離散控制過程的系統(tǒng) ：X ( k ) = A X ( k - 1 ) + B U ( k ) + W ( k ) 系統(tǒng)的測(cè)量值：Z(k)=HX(k)+V(k) X(k)是k時(shí)刻的系統(tǒng)狀態(tài)，U(k)是k時(shí)刻對(duì)系統(tǒng)的控制量。A和B是系統(tǒng)參數(shù)，對(duì)于多模型系統(tǒng)，他們?yōu)榫仃?。Z(k)是k時(shí)刻的測(cè)量值，H是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示系統(tǒng)和測(cè)量

9、的噪聲，均值為0，協(xié)方差分別是Q,R。）。6、卡爾曼濾波器算法 首先我們要利用系統(tǒng)的過程模型，來預(yù)測(cè)下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測(cè)出現(xiàn)在的狀態(tài)：X(k|k-1)=AX(k-1|k-1)+BU(k) （1） 式(1)中，X(k|k-1)是利用上一狀態(tài)預(yù)測(cè)的結(jié)果，X(k-1|k-1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。6、卡爾曼濾波器算法 到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于X(k|k-1)的協(xié)方差還沒更新。我們用P表示協(xié)方差：P(k|k-1)=AP(k-1|k-1) A+Q （2）式(2

10、)中，P(k|k-1)是X(k|k-1)對(duì)應(yīng)的協(xié)方差，P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的協(xié)方差，A表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的協(xié)方差。式子1，2就是卡爾曼濾波器5個(gè)公式 當(dāng) 中 的 前 兩 個(gè) ， 也 就 是 對(duì) 系 統(tǒng) 的 預(yù) 測(cè) 。6、卡爾曼濾波器算法 現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)值和測(cè)量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)：X(k|k)= X(k|k-1)+K(k) (Z(k)-H X(k|k-1) (3)其中K為卡爾曼增益(Kalman Gain)：K(k)= P(k|k-1) H/ (H P(k|k-1) H+ R) (4)6、卡爾曼濾波器算法 到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過程結(jié)束，我們還要更新k狀態(tài)下X(k|k)的協(xié)方差：P(k|k)=（I-K(k) H）P(k|k-1) (5)卡爾曼濾波器的原理基本描述了，式子1，2，3，4和5就是他的5 個(gè)基本公式。根據(jù)這5個(gè)公式，可以很容易實(shí)現(xiàn)計(jì)算機(jī)編程以上就是卡