基于卡爾曼濾波器原理應(yīng)用

1、基于卡爾曼濾波器的應(yīng)用四院八隊(duì) 萬(wàn)子平 2015.04.22The Kalman filter Contents 斯坦利施密特(Stanley Schmidt)首次實(shí)現(xiàn)了卡爾曼濾波器。卡爾曼在NASA埃姆斯研究中心訪問(wèn)時(shí),發(fā)現(xiàn)他的方法對(duì)于解決阿波羅計(jì)劃的軌道預(yù)測(cè)很有用,后來(lái)阿波羅飛船的導(dǎo)航電腦使用了這種濾波器。關(guān)于這種濾波器的論文由Swerling (1958), Kalman (1960)與 Kalman and Bucy (1961)發(fā)表。Emil Kalman匈牙利數(shù)學(xué)家哥倫比亞大學(xué)博士1930年生ABOUT Kalman FilterFirst ApplicationFirst App

2、licationSignal Processingv Digital filtering：通過(guò)一種算法排除隨機(jī)的系統(tǒng)干擾，提高接收精度的一種手段。v 線性系統(tǒng)：F（a+b）=F（a）+F（b）。v 數(shù)學(xué)處理方法（Kalman）：過(guò)去的測(cè)量估計(jì)誤差合并到新的測(cè)量誤差中來(lái)估計(jì)將來(lái)的誤差。v 輸入與輸出：過(guò)程中減少噪聲。 Same and Difference： Same:卡爾曼(Kalman)濾波和維納(Wiener)濾波都是以最小均方誤差為準(zhǔn)則的最佳線性估計(jì)或?yàn)V波。 2. Difference：維納濾波只適用于平穩(wěn)隨機(jī)過(guò)程(信號(hào))。卡爾曼濾波沒(méi)有這個(gè)限制，信號(hào)可以是平穩(wěn)的，也可以是非平穩(wěn)的。Us

3、e For 機(jī)器人導(dǎo)航，控制。 傳感器數(shù)據(jù)融合。 雷達(dá)系統(tǒng)以及導(dǎo)彈跟蹤。 計(jì)算機(jī)圖像處理。 頭臉識(shí)別 圖像分割 圖像邊緣檢測(cè) Temperature Problem：1. 預(yù)測(cè)值： 我們預(yù)測(cè)這個(gè)房間一分鐘以后的溫度。但是預(yù)測(cè)不會(huì)100%正確，可能會(huì)有上下偏差幾度。我們把這些偏差看成是高斯白噪聲，也就是這些偏差跟前后時(shí)間是沒(méi)有關(guān)系的而且符合高斯分配。2.測(cè)量值： 我們?cè)诜块g里放一個(gè)溫度計(jì)，但是這個(gè)溫度計(jì)也不準(zhǔn)確的，測(cè)量值會(huì)比實(shí)際值偏差。我們也把這些偏差看成是高斯白噪聲。3.卡爾曼增益： 由于我們用于估算一分鐘后的實(shí)際溫度有兩個(gè)溫度值，預(yù)測(cè)值和測(cè)量值分別是x度和y度，并設(shè)真實(shí)值是z度，這時(shí)我們可以

4、用他們的covariance來(lái)判斷。 Kg（卡爾曼增益）2=（z-y）2/(z-y）2+（z-x）2，所以Kg=0.78。4.最優(yōu)估計(jì)值： 可以估算出一分鐘后的實(shí)際溫度值是x+Kg*(y-x)=Z(含最優(yōu)估計(jì)偏差）。5.計(jì)算估計(jì)偏差： 算法如下：(1-Kg)*（z-y）2)0.5=最優(yōu)估計(jì)偏差。 The description of the algorithm：首先，我們先要引入一個(gè)離散控制過(guò)程的系統(tǒng)。該系統(tǒng)可用一個(gè)線性隨機(jī)微分方程（Linear Stochastic Difference equation）來(lái)描述：X(k)=A X(k-1)+B U(k)+W(k)再加上系統(tǒng)的測(cè)量值：Z(k)

5、=H X(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)榫仃嚒(k)是k時(shí)刻的測(cè)量值，H是測(cè)量系統(tǒng)的參數(shù)，對(duì)于多測(cè)量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過(guò)程和測(cè)量的噪聲。他們被假設(shè)成高斯白噪聲(White Gaussian Noise)，他們的covariance 分別是Q，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。然后我們要利用系統(tǒng)的過(guò)程模型，來(lái)預(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)=A X(k-1|k-1)+B U(k)

6、. (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)的控制量，如果沒(méi)有控制量，它可以為0。其次我們用P表示covariance： P(k|k-1)=A P(k-1|k-1) A+Q (2)式(2)中，P(k|k-1)是X(k|k-1)對(duì)應(yīng)的covariance，P(k-1|k-1)是X(k-1|k-1)對(duì)應(yīng)的covariance，A表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過(guò)程的covariance。式子1，2就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系統(tǒng)的預(yù)測(cè)。現(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測(cè)結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測(cè)量值。結(jié)合預(yù)測(cè)

7、值和測(cè)量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)： X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1) (3)其中Kg為卡爾曼增益(Kalman Gain)： Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (4) 到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要令卡爾曼濾波器不斷的運(yùn)行下去直到系統(tǒng)過(guò)程結(jié)束，我們最后還要更新k狀態(tài)下X(k|k)的covariance：P(k|k)=（I-Kg(k) H）P(k|k-1) (5)其中I 為1的矩陣，對(duì)于單模型單測(cè)量，I=1。當(dāng)系統(tǒng)進(jìn)入k+1狀態(tài)時(shí)，P(k|k)就是式子(2)的P(k-1|k-1)。這樣，算法就可以自回歸的運(yùn)算下去。The program flow chart：The Rresult of Experiment： 從圖中可以發(fā)現(xiàn)，卡爾曼濾波器能夠非常有效地在