Kalman濾波在運(yùn)動(dòng)跟蹤中的建模教案資料

1、Good is good, but better carries it.精益求精，善益求善。Kalman濾波在運(yùn)動(dòng)跟蹤中的建模目錄TOCo1-3hzuHYPERLINKl_Toc299915294一、kalman濾波簡(jiǎn)介PAGEREF_Toc299915294h2HYPERLINKl_Toc299915295二、kalman濾波基本原理PAGEREF_Toc299915295h2HYPERLINKl_Toc299915296三、Kalman濾波在運(yùn)動(dòng)跟蹤中的應(yīng)用的建模PAGEREF_Toc299915296h4HYPERLINKl_Toc299915297四、仿真結(jié)果PAGEREF_Toc29

2、9915297h7HYPERLINKl_Toc2999152981、kalman的濾波效果PAGEREF_Toc299915298h7HYPERLINKl_Toc2999152992、簡(jiǎn)單軌跡的kalman的預(yù)測(cè)效果PAGEREF_Toc299915299h8HYPERLINKl_Toc2999153003、橢圓運(yùn)動(dòng)軌跡的預(yù)測(cè)PAGEREF_Toc299915300h10HYPERLINKl_Toc2999153014、往返運(yùn)動(dòng)歸軌跡的預(yù)測(cè)PAGEREF_Toc299915301h11HYPERLINKl_Toc299915302五、參數(shù)的選取PAGEREF_Toc299915302h12HY

3、PERLINKl_Toc299915303附錄：PAGEREF_Toc299915303h14HYPERLINKl_Toc299915304Matlab程序：PAGEREF_Toc299915304h14HYPERLINKl_Toc299915305C語(yǔ)言程序：PAGEREF_Toc299915305h14Kalman濾波在運(yùn)動(dòng)跟蹤中的應(yīng)用kalman濾波簡(jiǎn)介最佳線(xiàn)性濾波理論起源于40年代美國(guó)科學(xué)家Wiener和前蘇聯(lián)科學(xué)家等人的研究工作，后人統(tǒng)稱(chēng)為維納濾波理論。從理論上說(shuō)，維納濾波的最大缺點(diǎn)是必須用到無(wú)限過(guò)去的數(shù)據(jù)，不適用于實(shí)時(shí)處理。為了克服這一缺點(diǎn)，60年代Kalman把狀態(tài)空間模型引入濾

4、波理論，并導(dǎo)出了一套遞推估計(jì)算法，后人稱(chēng)之為卡爾曼濾波理論?？柭鼮V波是以最小均方誤差為估計(jì)的最佳準(zhǔn)則，來(lái)尋求一套遞推估計(jì)的算法，其基本思想是：采用信號(hào)與噪聲的狀態(tài)空間模型，利用前一時(shí)刻地估計(jì)值和現(xiàn)時(shí)刻的觀(guān)測(cè)值來(lái)更新對(duì)狀態(tài)變量的估計(jì)，求出現(xiàn)時(shí)刻的估計(jì)值。它適合于實(shí)時(shí)處理和計(jì)算機(jī)運(yùn)算。Kalman濾波是卡爾曼(R.E.kalman)于1960年提出的從與被提取信號(hào)的有關(guān)的觀(guān)測(cè)量中通過(guò)算法估計(jì)出所需信號(hào)的一種濾波算法。他把狀態(tài)空間的概念引入到隨機(jī)估計(jì)理論中，把信號(hào)過(guò)程視為白噪聲作用下的個(gè)線(xiàn)性系統(tǒng)的輸出，用狀方程來(lái)描述這種輸入輸出關(guān)系，估計(jì)過(guò)程中利用系統(tǒng)狀態(tài)方程、觀(guān)測(cè)方程和白噪聲激勵(lì)(系統(tǒng)噪聲和觀(guān)測(cè)

5、噪聲)的統(tǒng)計(jì)特性形成濾波算法，由于所用的信息都是時(shí)域內(nèi)的量，所以不但可以對(duì)平穩(wěn)的一維隨機(jī)過(guò)程進(jìn)估計(jì)，也可以對(duì)非平穩(wěn)的、多維隨機(jī)過(guò)程進(jìn)行估汁。Kalman濾波是一套由計(jì)算機(jī)實(shí)現(xiàn)的實(shí)時(shí)遞推算法它所處理的對(duì)象是隨機(jī)信號(hào)，利用系統(tǒng)噪聲和觀(guān)測(cè)噪聲的統(tǒng)計(jì)特性，以系統(tǒng)的觀(guān)測(cè)量作為濾波器的輸入，以所要估計(jì)值(系統(tǒng)的狀態(tài)或參數(shù))作為濾波器的輸出，濾波器的輸入與輸出之間是由時(shí)間更新和觀(guān)測(cè)更新算法聯(lián)系在一起的，根據(jù)系統(tǒng)方程和觀(guān)測(cè)方程估計(jì)出所有需要處理的信號(hào)。所以，Kalman濾波與常規(guī)濾波的涵義與方法不同，它實(shí)質(zhì)上是一種最優(yōu)估計(jì)法。卡爾曼濾波器是一個(gè)“optimalrecursivedataprocessingal

6、gorithm（最優(yōu)化自回歸數(shù)據(jù)處理算法），對(duì)于解決很大部分的問(wèn)題，他是最優(yōu)，效率最高甚至是最有用的kalman濾波基本原理Kalman濾波器是目標(biāo)狀態(tài)估計(jì)算法解決狀態(tài)最優(yōu)估計(jì)的一種常用方法具有計(jì)算量小、存儲(chǔ)量低、實(shí)時(shí)性高的優(yōu)點(diǎn)。實(shí)際應(yīng)用中，可以將物理系統(tǒng)的運(yùn)行過(guò)程看作是一個(gè)狀態(tài)轉(zhuǎn)換過(guò)程，卡爾曼濾波將狀態(tài)空間理論引入到對(duì)物理系統(tǒng)的數(shù)學(xué)建模過(guò)程中來(lái)。其基本思想是給系統(tǒng)信號(hào)和噪聲的狀態(tài)空間建立方程和觀(guān)測(cè)方程，只用信號(hào)的前一個(gè)估計(jì)值和最近一個(gè)觀(guān)察值就可以在線(xiàn)性無(wú)偏最小方差估計(jì)準(zhǔn)則下對(duì)信號(hào)的當(dāng)前值做出最優(yōu)估計(jì)。設(shè)一系統(tǒng)所建立的模型為：狀態(tài)方程：個(gè)離散控制過(guò)程的系統(tǒng)，X(k)=AX(k-1)+BU(k)

7、+W(k)觀(guān)測(cè)方程：系統(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)分別表示過(guò)程和測(cè)量的噪聲。他們被假設(shè)成高斯白噪聲(WhiteGaussianNoise)，他們的covariance分別是Q，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。對(duì)于滿(mǎn)足上面的條件(線(xiàn)性隨機(jī)微分系統(tǒng)，過(guò)程和測(cè)量都是高斯白噪聲)，卡爾曼濾波器是最優(yōu)的信息處理器。下面我們來(lái)用他們結(jié)合他們的covariances來(lái)估算系統(tǒng)的

8、最優(yōu)化輸出（類(lèi)似上一節(jié)那個(gè)溫度的例子）。首先我們要利用系統(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)=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)的控制量，如果沒(méi)有控制量，它可以為0。到現(xiàn)在為止，我們的系統(tǒng)結(jié)果已經(jīng)更新了，可是，對(duì)應(yīng)于X(k|k-1)的covariance還沒(méi)更新。我們用P表示covariance：P(k|k-1)=AP(k-1|k-1)A+Q(2)式(2)中，P(k|k-1)是X(

9、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è)?，F(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)+Kg(k)(Z(k)-HX(k|k-1)(3)其中Kg為卡爾曼增益(KalmanGain)：Kg(k)=P(k|k-1)H/(HP(k|k-1)H+R)(4)到現(xiàn)在為止，我們已經(jīng)得到了k狀態(tài)下最優(yōu)

10、的估算值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)算下去?？偨Y(jié)以上內(nèi)容克制，Kalman濾波器實(shí)現(xiàn)的主要五個(gè)方程為：(1)狀態(tài)向量預(yù)報(bào)方程(2)狀態(tài)向量協(xié)方差預(yù)報(bào)方程(3)Kalman加權(quán)矩陣(或增益矩陣)(4)狀態(tài)向量更新方程(5)狀態(tài)向量協(xié)方差更新方程Kalman濾波預(yù)估計(jì)就是用前面兩個(gè)時(shí)間更新方程獲得先驗(yàn)估計(jì)

11、然后通過(guò)后面三個(gè)狀態(tài)更新方程對(duì)先驗(yàn)估計(jì)矯正獲得最優(yōu)估計(jì)。根據(jù)這5個(gè)公式，可以很容易的實(shí)現(xiàn)計(jì)算機(jī)的程序?？柭鼮V波的算法流程為：預(yù)估計(jì)X(k)=F(k,k-1)X(k-1)計(jì)算預(yù)估計(jì)協(xié)方差矩陣C(k)=F(k,k-1)C(k)F(k,k-1)+T(k,k-1)Q(k)T(k,k-1)Q(k)=U(k)U(k)計(jì)算卡爾曼增益矩陣K(k)=C(k)H(k)H(k)C(k)H(k)+R(k)(-1)R(k)=N(k)N(k)更新估計(jì)X(k)=X(k)+K(k)Y(k)-H(k)X(k)計(jì)算更新后估計(jì)協(xié)防差矩陣C(k)=I-K(k)H(k)C(k)I-K(k)H(k)+K(k)R(k)K(k)X(k+1

12、)=X(k)C(k+1)=C(k)重復(fù)以上六個(gè)步驟。Kalman濾波在運(yùn)動(dòng)跟蹤中的應(yīng)用的建?？柭鼮V波器是一個(gè)對(duì)動(dòng)態(tài)系統(tǒng)的狀態(tài)序列進(jìn)行線(xiàn)性最小誤差估計(jì)的算法，一般用于線(xiàn)性系統(tǒng)。一般在運(yùn)動(dòng)跟蹤領(lǐng)域中攝像機(jī)相對(duì)于目標(biāo)物體運(yùn)動(dòng)有時(shí)屬于非線(xiàn)性系統(tǒng)，但由于在一般運(yùn)動(dòng)跟蹤問(wèn)題中圖像采集時(shí)間間隔較短，可近似將單位時(shí)間內(nèi)目標(biāo)在圖像中的運(yùn)動(dòng)看作勻速運(yùn)動(dòng)，采用卡爾曼濾波器可以實(shí)現(xiàn)對(duì)目標(biāo)運(yùn)動(dòng)參數(shù)的估計(jì)。對(duì)于復(fù)雜背景下運(yùn)動(dòng)目標(biāo)識(shí)別與跟蹤問(wèn)題，要實(shí)現(xiàn)實(shí)時(shí)控制，對(duì)算法的實(shí)時(shí)性和準(zhǔn)確性都有較高的要求。如果目標(biāo)識(shí)別算法都是基于像素的全局搜索，則存在顯著缺點(diǎn)：1)全局搜索計(jì)算量大、耗時(shí)，實(shí)時(shí)性無(wú)法滿(mǎn)足；2)全局搜索抗干擾能力差

13、，容易受到背景中相似特征物體的干擾?；诳柭鼮V波器預(yù)測(cè)功能的運(yùn)動(dòng)目標(biāo)快速跟蹤算法可以通過(guò)預(yù)測(cè)目標(biāo)物體在下一幀中的位置，將全局搜索問(wèn)題轉(zhuǎn)化為局部搜索，縮小搜索范圍，提高算法的實(shí)時(shí)性。在跟蹤運(yùn)動(dòng)目標(biāo)的過(guò)程中，由于目標(biāo)物體單位時(shí)間內(nèi)在圖像中的運(yùn)動(dòng)可以看作勻速運(yùn)動(dòng)，所以可以采用目標(biāo)某一時(shí)刻在圖像中的位置和速度來(lái)表示目標(biāo)的運(yùn)動(dòng)狀態(tài)。為了簡(jiǎn)化算法的計(jì)算復(fù)雜度，可以設(shè)計(jì)2個(gè)卡爾曼濾波器分別描述目標(biāo)在X軸和Y軸方向上位置和速度的變化。下面僅討論X軸方向上卡爾曼濾波器的實(shí)現(xiàn)過(guò)程，Y軸方向上同理。目標(biāo)物體運(yùn)動(dòng)方程為：式中，分別為目標(biāo)在t=k時(shí)刻的X軸方向的位置、速度和加速度；T為k幀圖像和k+1幀圖像之間的時(shí)間

14、間隔，可以當(dāng)作白噪聲處理。寫(xiě)成矩陣形式如下：系統(tǒng)狀態(tài)方程為：卡爾曼濾波器系統(tǒng)狀態(tài)矢量為：狀態(tài)轉(zhuǎn)移矩陣為：系統(tǒng)動(dòng)態(tài)噪聲矢量為：系統(tǒng)觀(guān)測(cè)方程為：卡爾曼濾波器系統(tǒng)觀(guān)測(cè)矢量為：觀(guān)測(cè)系數(shù)矩陣為：由觀(guān)測(cè)方程可知，觀(guān)測(cè)噪聲為0，所以=0。建立了上述系統(tǒng)狀態(tài)方程和觀(guān)測(cè)方程之后，就可以利用卡爾曼濾波方程式通過(guò)遞推方法，不斷預(yù)測(cè)目標(biāo)在下一幀中的位置。在t=k時(shí)刻，對(duì)第k幀圖像利用目標(biāo)識(shí)別算法識(shí)別出的目標(biāo)位置記為，當(dāng)目標(biāo)首次出現(xiàn)時(shí)，根據(jù)此時(shí)目標(biāo)的觀(guān)測(cè)位置初始化濾波器=，0。系統(tǒng)初始狀態(tài)向量協(xié)方差矩陣可以在對(duì)角線(xiàn)上取較大值，取值根據(jù)實(shí)際測(cè)量情況來(lái)獲得，但在濾波啟動(dòng)一段時(shí)間后影響就不大了。?。?。系統(tǒng)動(dòng)態(tài)噪聲協(xié)方差為，可

15、取為：。通過(guò)公式(1)，計(jì)算得到目標(biāo)在下一幀圖像中的預(yù)測(cè)位置。在該位置附近，對(duì)下一幀圖像進(jìn)行局部搜索，識(shí)別出的目標(biāo)質(zhì)心位置即為，通過(guò)公式(2)至公式(5)完成對(duì)狀態(tài)向量和狀態(tài)向量協(xié)方差矩陣的更新，為目標(biāo)位置的下一步預(yù)測(cè)做好準(zhǔn)備，得出新的預(yù)測(cè)位置，采用圖像處理算法，在該位置進(jìn)行局部搜索，從而得出新的目標(biāo)質(zhì)心位置，一直迭代計(jì)算下去，從而實(shí)現(xiàn)對(duì)目標(biāo)物體的跟蹤。簡(jiǎn)化計(jì)算的到以下的實(shí)際編程公式：(1)狀態(tài)向量預(yù)報(bào)方程簡(jiǎn)化：Xy=Xb1+Vb1*T;Vy=Vb1;(2)狀態(tài)向量協(xié)方差預(yù)報(bào)方程簡(jiǎn)化：Py=*+QPy1=(Pbb1+Pbb3*T)+(Pbb2+Pbb4)*T;Py2=Pbb2+Pbb4*T;P

16、y3=Pbb3+Pbb4*T;Py4=Pbb4;(3)Kalman加權(quán)矩陣(或增益矩陣)簡(jiǎn)化：Kg=Kg1=Py1/(Py1+R)Kg2=Py3/(Py1+R)(4)狀態(tài)向量更新方程簡(jiǎn)化：Xb=Xy+Kg1(Xnew+Xy);b=Vy+Kg2(Xnew+Xy);(5)狀態(tài)向量協(xié)方差更新方程簡(jiǎn)化：Pb1=(1-Kg1)*Py1-Kg1Py3;Pb2=(1-Kg1)*Py2-Kg1Py4;Pb3=(1-Kg1)*Py3-Kg1Py1;Pb3=(1-Kg1)*Py4-Kg1Py2;仿真結(jié)果1、kalman的濾波效果對(duì)一個(gè)定值函數(shù)y=25，加高斯白噪聲噪聲之后，經(jīng)卡拉曼濾波可得到的檢測(cè)對(duì)比加噪聲的之后

17、的信號(hào)（測(cè)量信號(hào)）和經(jīng)過(guò)kalman濾波后的信號(hào)（濾波后的信號(hào)），對(duì)比兩個(gè)信號(hào)。從圖1的濾波結(jié)果中可以看出，經(jīng)過(guò)kalman濾波之后，信號(hào)基本上與加高斯白噪聲之前的信號(hào)一直，誤差大大減少，由此可以看出，kalaman濾波對(duì)于白噪聲有比較好的濾除作用。圖1kalaman濾波濾除信號(hào)中的白噪聲2、簡(jiǎn)單軌跡的kalman的預(yù)測(cè)效果使用基本的kalman濾波方法，預(yù)測(cè)下一個(gè)x的位置。建立模型的方法是：將目標(biāo)物體在T時(shí)間內(nèi)的運(yùn)動(dòng)可以看作勻速運(yùn)動(dòng)，采用目標(biāo)某一時(shí)刻在圖像中的位置和速度來(lái)表示目標(biāo)的運(yùn)動(dòng)狀態(tài)。為了簡(jiǎn)化算法的計(jì)算復(fù)雜度，使用兩個(gè)個(gè)卡爾曼濾波器分別描述目標(biāo)在X軸和Y軸方向上位置和速度的變化，兩個(gè)濾

18、波器的原理相同。仿真的結(jié)果如下：左邊的運(yùn)動(dòng)的軌跡，右邊的是kalman的預(yù)測(cè)的誤差。圖2簡(jiǎn)單軌跡的kalman的預(yù)測(cè)效果可以看出當(dāng)物體勻速運(yùn)動(dòng)的時(shí)候，kalman的預(yù)測(cè)比較準(zhǔn)確；當(dāng)物體改變速度的幅度比較大的時(shí)候，這是因?yàn)樗俣茸兓^大的時(shí)候，表明加速度的變化比較大，這樣用之前的參數(shù)預(yù)測(cè)就會(huì)有較大的偏差，所以kalman的誤差會(huì)比較明顯。3、橢圓運(yùn)動(dòng)軌跡的預(yù)測(cè)下面是物體運(yùn)動(dòng)作橢圓運(yùn)動(dòng)的一個(gè)預(yù)測(cè)：藍(lán)色為實(shí)際軌跡，綠色為預(yù)測(cè)的軌跡，從x方向誤差看，kalman的效果比較明顯，y方向的預(yù)測(cè)效果就一般了，這個(gè)主要是跟kalman的的濾波參數(shù)選取有關(guān)，kalman是將運(yùn)動(dòng)的加速度看做白噪聲處理的，參數(shù)的選取

19、與加速度的方差有關(guān)?，F(xiàn)在用的參數(shù)是參考一些論文的設(shè)置，然后自己經(jīng)過(guò)測(cè)試選取的。速度變化大的時(shí)候就會(huì)預(yù)測(cè)不太準(zhǔn)確，現(xiàn)在寫(xiě)的kalman還不能對(duì)速度變化自適應(yīng)。圖3橢圓運(yùn)動(dòng)軌跡的預(yù)測(cè)4、往返運(yùn)動(dòng)歸軌跡的預(yù)測(cè)kalman在窗口縮小情況下的跟蹤效果，為了效果明顯，將運(yùn)動(dòng)的軌跡做了調(diào)整，在改變速度、改變窗口大小的情況下，做了多組跟蹤測(cè)試，下面是將其中一組測(cè)試速度變化較快、開(kāi)窗較小的數(shù)據(jù)，軌跡如下圖所示。結(jié)果分析：在速度變化比較大，開(kāi)窗比較小的情況下，用kalman的丟幀次數(shù)會(huì)減少一些，（硬件測(cè)試觀(guān)測(cè)的結(jié)果）；比較matlab分析的結(jié)果，用kalman的預(yù)測(cè)誤差（綠線(xiàn)）比原始的幀間誤差（藍(lán)線(xiàn)）要小一些，遇

20、到速度變化比較劇烈的時(shí)候，誤差較大，易造成丟幀。整體來(lái)說(shuō)，經(jīng)過(guò)kalman處理，預(yù)測(cè)數(shù)據(jù)與真實(shí)數(shù)據(jù)之間的誤差有所減小，而且在運(yùn)動(dòng)不突變的情況下，預(yù)測(cè)也相對(duì)準(zhǔn)確，這是kalman的優(yōu)勢(shì)。（1）運(yùn)動(dòng)軌跡（2）局部放大的軌跡（3）X方向的誤差分析（4）Y方向的誤差分析圖4kalman對(duì)往返運(yùn)動(dòng)的預(yù)測(cè)參數(shù)的選取建立模型，狀態(tài)方程X(k)=AX(k-1)+BU(k)+W(k)和觀(guān)測(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(

21、k)和V(k)分別表示過(guò)程和測(cè)量的噪聲。他們被假設(shè)成高斯白噪聲(WhiteGaussianNoise)，他們的covariance分別是Q，R（這里我們假設(shè)他們不隨系統(tǒng)狀態(tài)變化而變化）。需要選取的參數(shù)主要是一下三個(gè)：初始觀(guān)測(cè)位置：當(dāng)目標(biāo)首次出現(xiàn)時(shí)，根據(jù)此時(shí)目標(biāo)的觀(guān)測(cè)位置初始化濾波器=，0。初始位置的選取會(huì)影響到kalman在多少次運(yùn)算之后才趨向于準(zhǔn)確，因?yàn)閗alman是迭代收斂來(lái)逐漸趨向于真實(shí)值的，如果開(kāi)始的初始化位置與實(shí)際的位置相差比較大，就需要多迭代幾次，這時(shí)需要注意的是前幾幀的數(shù)據(jù)將會(huì)有較大的誤差。初始狀態(tài)向量協(xié)方差矩陣：系統(tǒng)初始狀態(tài)向量協(xié)方差矩陣可以在對(duì)角線(xiàn)上取較大值，取值根據(jù)實(shí)際測(cè)量

22、情況來(lái)獲得，但在濾波啟動(dòng)一段時(shí)間后影響就不大了。因?yàn)閗alman會(huì)自行迭代產(chǎn)生新的向量協(xié)方差矩陣，直至達(dá)到最優(yōu)。?。骸Ｏ到y(tǒng)動(dòng)態(tài)噪聲協(xié)方差矩：系統(tǒng)動(dòng)態(tài)噪聲協(xié)方差矩陣為，這個(gè)矩陣主要是表征了噪聲的特性，也就是加速度的變化特性，這里要根據(jù)實(shí)際的運(yùn)動(dòng)模型來(lái)選取，不同的狀態(tài)下噪聲的特性也不同。可以一般來(lái)說(shuō)，可取為：。也可以根據(jù)效果修改的到一個(gè)較好的參數(shù)。附錄：Matlab程序：functionX_next=kalman_f(D_y,N,A,T,Q,H,I,R,Pb1,Xb1);fork=1:N-1;Y=D_y(k);%Y=S(k);%測(cè)量值Y更新輸入數(shù)據(jù)Y=S(k),V(k);Xy=A*Xb1;%系統(tǒng)的

23、預(yù)測(cè)值前一時(shí)刻X的相關(guān)系數(shù)?Py=A*Pb1*A+Q;%Py(k)為X(kk-1)狀態(tài)的協(xié)方差Kg=(Py*H)*inv(H*Py*H+R);%卡爾曼增益Kg=(Py+H)/(H*Py*H+R);R=0;Xb=Xy+Kg*(Y-H*Xy);%系統(tǒng)的最佳估計(jì)值即經(jīng)過(guò)濾波后的信號(hào)Pb=(I-Kg*H)*(Py);%Pb(k)為X(kk)狀態(tài)的協(xié)方差即t狀態(tài)下x(t|t)的相關(guān)系數(shù)Xb1=Xb;%更新最佳估計(jì)值Pb1=Pb;%更新誤差Sb(k+1)=Xb(1,1);Vb(k+1)=Xb(2,1);x_next=A*Xb1;X_next(k+1)=x_next(1,1);EndC語(yǔ)言程序：/*基本的k

24、alman濾波，預(yù)測(cè)下一個(gè)x的位置；將目標(biāo)物體在T時(shí)間內(nèi)的運(yùn)動(dòng)可以看作勻速運(yùn)動(dòng)，采用目標(biāo)某一時(shí)刻在圖像中的位置和速度來(lái)表示目標(biāo)的運(yùn)動(dòng)狀態(tài)。設(shè)計(jì)2個(gè)卡爾曼濾波器分別描述目標(biāo)在X軸和Y軸方向上位置和速度的變化，為了簡(jiǎn)化算法的計(jì)算復(fù)雜度。下面是討論X軸方向上卡爾曼濾波器的實(shí)現(xiàn)過(guò)程，y軸方向上的相同.*/#include#include#includemath.h/初始化參數(shù)#defineT1/幀與幀間的時(shí)間間隔#defineR0/觀(guān)測(cè)誤差矩陣#defineQ00.002/系統(tǒng)動(dòng)態(tài)噪聲協(xié)方差#defineQ10#defineQ20#defineQ30.002inti;intX_new;floatX;floatX_best,V_best;/預(yù)測(cè)值floatX_y,V_y;/估計(jì)值floatX_next;intX_next1;floatPy4;/協(xié)方差預(yù)測(cè)矩陣floatP