kalman濾波在運(yùn)動(dòng)跟蹤中的建模

1、目錄一、kalman濾波簡介..........................................................................2二、kalman濾波基本原理..................................................................2三、Kalman濾波在運(yùn)動(dòng)跟蹤中的應(yīng)用的建模............................

2、.......4四、仿真結(jié)果.......................................................................................71、kalman的濾波效果......................................................................72、簡單軌跡的kalman的預(yù)測效果................

3、..................................83、橢圓運(yùn)動(dòng)軌跡的預(yù)測.................................................................104、往返運(yùn)動(dòng)歸軌跡的預(yù)測.............................................................11五、參數(shù)的選取.........................

4、........................................................12附錄：.....................................................................................................14Matlab程序：...............................................

5、....................................14C語言程序：....................................................................................14Kalman濾波在運(yùn)動(dòng)跟蹤中的應(yīng)用2狀態(tài)方程：個(gè)離散控制過程的系統(tǒng)，X(k)=AX(k1)BU(k)W(k)觀測方程：系統(tǒng)的測量值Z(k)=HX(k)V(k)上兩式子中，X(k)是

6、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í)刻的測量值，H是測量系統(tǒng)的參數(shù)，對(duì)于多測量系統(tǒng)，H為矩陣。W(k)和V(k)分別表示過程和測量的噪聲。他們被假設(shè)成高斯白噪聲(WhiteGaussianNoise)，他們的covariance分別是Q，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。對(duì)于滿足上面的條件(線性隨機(jī)微分系統(tǒng)，過程和測量都是高斯白噪聲)，卡爾曼濾波器是最

7、優(yōu)的信息處理器。下面我們來用他們結(jié)合他們的covariances來估算系統(tǒng)的最優(yōu)化輸出（類似上一節(jié)那個(gè)溫度的例子）。首先我們要利用系統(tǒng)的過程模型，來預(yù)測下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài)：X(k|k1)=AX(k1|k1)BU(k)………(1)式(1)中，X(k|k1)是利用上一狀態(tài)預(yù)測的結(jié)果，X(k1|k1)是上一狀態(tài)最優(yōu)的結(jié)果，U(k)為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它

8、可以為0。到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于X(k|k1)的covariance還沒更新。我們用P表示covariance：P(k|k1)=AP(k1|k1)A’Q………(2)式(2)中，P(k|k1)是X(k|k1)對(duì)應(yīng)的covariance，P(k1|k1)是X(k1|k1)對(duì)應(yīng)的covariance，A’表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的covariance。式子1，2就是卡爾曼濾波器5個(gè)公式當(dāng)中的前兩個(gè)，也就是對(duì)系

9、統(tǒng)的預(yù)測?，F(xiàn)在我們有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，然后我們?cè)偈占F(xiàn)在狀態(tài)的測量值。結(jié)合預(yù)測值和測量值，我們可以得到現(xiàn)在狀態(tài)(k)的最優(yōu)化估算值X(k|k)：X(k|k)=X(k|k1)Kg(k)(Z(k)HX(k|k1))………(3)其中Kg為卡爾曼增益(KalmanGain)：Kg(k)=P(k|k1)H’(HP(k|k1)H’R)………(4)到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)的估算值X(k|k)。但是為了要使卡爾曼濾波器不斷的運(yùn)行下去直