卡爾曼濾波在目標(biāo)跟蹤中應(yīng)用仿真研究【摘要】目標(biāo)跟蹤問題的應(yīng)用背景是雷達(dá)數(shù)據(jù)處理，即雷達(dá)在搜索到目標(biāo)并記錄目標(biāo)的位置數(shù)據(jù)，對測量到的目標(biāo)位置數(shù)據(jù)（稱為點(diǎn)跡）進(jìn)行處理，自動(dòng)形成航跡，并對目標(biāo)在下一時(shí)刻的位置進(jìn)行預(yù)測。本文簡要討論了用KALMAN濾波方法對單個(gè)目標(biāo)航跡進(jìn)行預(yù)測，并借助于MATLAB仿真工具，對實(shí)驗(yàn)的效果進(jìn)行評估。關(guān)鍵詞KALMAN濾波、目標(biāo)跟蹤、MATLAB仿真1情景假設(shè)假定有一個(gè)二座標(biāo)雷達(dá)對一平面上運(yùn)動(dòng)的目標(biāo)進(jìn)行觀測，目標(biāo)在04T秒沿Y軸作恒速直線運(yùn)動(dòng)，運(yùn)動(dòng)速度為15米/秒，目標(biāo)的起始點(diǎn)為2000米,10000米，在406T秒向軸方向做的慢轉(zhuǎn)彎，加速度均為0075米/秒2，完成慢轉(zhuǎn)彎后加X09速度將降為零，從秒開始做的快轉(zhuǎn)彎，加速度為03米/秒2，在660秒結(jié)束轉(zhuǎn)1T彎，加速度降至零。雷達(dá)掃描周期秒，和獨(dú)立地進(jìn)行觀測，觀測噪聲的標(biāo)準(zhǔn)差2TXY均為100米。2KALMAN濾波算法分析為了簡單起見，僅對軸方向進(jìn)行考慮。X首先，目標(biāo)運(yùn)動(dòng)沿軸方向的運(yùn)動(dòng)可以用下面的狀態(tài)方程描述MERGEFORMAT2121/XXKTKUKU用矩陣的形式表述為，MERGEFORMAT22XKKW在上式中，。XK10T21XKU考慮雷達(dá)的觀測，得出觀測方程為ZKCXKVMERGEFORMAT23在MERGEFORMAT23中，10，為零均值的噪聲序列，方差已知。對目標(biāo)進(jìn)行預(yù)測，由相關(guān)理論可得到下面的迭代式/XKKMERGEFORMAT24在MERGEFORMAT24中，11|KEXZ，反映了由前1K各觀測值對目前狀態(tài)的估計(jì)。而預(yù)測的誤差協(xié)方差可由下式表出，/1/11TTXXPKKQKMERGEFORMAT25對于最佳濾波，迭代表達(dá)式為/KKKZKCXKMERGEFORMAT26在式MERGEFORMAT26中，為KALMAN增益。而濾波誤差的協(xié)方差為，MERGEFORMAT27/1XXPKIKKCPK在應(yīng)用上面的公式進(jìn)行KALMAN濾波時(shí)，需要指定初值。由于實(shí)際中通常無法得到目標(biāo)的初始狀態(tài)，我們可以利用前幾個(gè)觀測值建立狀態(tài)的初始估計(jì)，比如采用前兩個(gè)觀測值，MERGEFORMAT282/21/TXXXZZ此時(shí)，估計(jì)誤差為MERGEFORMAT2/2TXXXVTXVU29而誤差協(xié)方差矩陣為，MERGEFORMAT21022/XXXTP3仿真計(jì)算與結(jié)果為了真實(shí)地反映出KALMAN濾波的效果，采用了MONTECARLO方法，采用多次實(shí)驗(yàn)取均值的方法進(jìn)行研究，可以計(jì)算出估計(jì)的誤差均值和方差，其表達(dá)式為MERGEFORMAT2111|MXIIIEKXK而誤差的標(biāo)準(zhǔn)差可以表示為MERGEFORMAT21|MXIIXIXKEK212在MERGEFORMAT211和MERGEFORMAT212中，就是進(jìn)行MONTECARLO仿M真的次數(shù)，而為取樣點(diǎn)數(shù)。當(dāng)仿真的次數(shù)越多時(shí)，實(shí)驗(yàn)的效果越接近于實(shí)際，但是計(jì)算K的速度會(huì)明顯變慢。在仿真時(shí)，需要根據(jù)實(shí)際適當(dāng)選取。在本程序中，取。50另外，在仿真過程中，為了進(jìn)一步研究目標(biāo)的航跡，在660秒后又進(jìn)行了一段時(shí)間的勻速運(yùn)動(dòng)仿真。下面是仿真的結(jié)果，仿真源文件分別為TRAJECTORYM，KALMAN_FILTERM和FILTER_RESULTM，分別產(chǎn)生原始目標(biāo)航跡、卡爾曼濾波估計(jì)和最終的輸出，為了方便觀察，將原始航跡、觀測數(shù)據(jù)和經(jīng)過濾波后的估計(jì)航跡反映在一張圖上。并且，畫出了給定MONTECARLO仿真的次數(shù)情況下，沿軸方向和軸方向估計(jì)誤差值的均值和標(biāo)準(zhǔn)差隨著采XY樣點(diǎn)數(shù)的增多而發(fā)生變化的情況。首先給出原始航跡，31原始航跡圖上圖中可以看出目標(biāo)運(yùn)動(dòng)的真實(shí)軌跡，首先沿軸方向運(yùn)動(dòng)，并經(jīng)過兩個(gè)轉(zhuǎn)彎。Y接下來經(jīng)過KALMAN濾波后的估計(jì)的X軸方向上的航跡和理論值為32濾波輸出X軸方向航跡圖而對應(yīng)地，估計(jì)的Y軸方向上的航跡和理論值為33濾波輸出Y軸方向航跡圖將測量數(shù)據(jù)和理論值對比，得出的圖形為34理論航跡與觀測值而將輸出的X和Y方向的航跡估計(jì)顯示在直角坐標(biāo)系下，為下圖所示35理論航跡與濾波估計(jì)值下面對濾波估計(jì)值的誤差均值和方差進(jìn)行簡單分析，對X和Y方向的航跡估計(jì)進(jìn)行分別討論，假定MONTECARLO仿真的次數(shù)50。由MERGEFORMAT211和MMERGEFORMAT212式就可以求出實(shí)際濾波輸出每時(shí)刻誤差的均值和標(biāo)準(zhǔn)差。36估計(jì)誤差均值曲線（左X軸右Y軸）對應(yīng)的標(biāo)準(zhǔn)差曲線為37估計(jì)誤差標(biāo)準(zhǔn)差曲線（上X軸下Y軸）經(jīng)過上面的仿真分析，可以看出KALMAN濾波算法對于動(dòng)態(tài)目標(biāo)的跟蹤有著比較好的效果，而且可以較好地抑止環(huán)境中的噪聲影響。附錄1TRAJECTORYMFUNCTIONX,YTRAJECTORYTS,OFFTIME產(chǎn)生真實(shí)航跡X,Y，并在直角坐標(biāo)系下顯示出TS為雷達(dá)掃描周期，每隔TS秒取一個(gè)觀測數(shù)據(jù)最初做勻速運(yùn)動(dòng)，接下來進(jìn)行兩個(gè)90度的機(jī)動(dòng)轉(zhuǎn)彎IFNARGIN2ERROR輸入的變量過多，請檢查ENDIFOFFTIME3ERRORTOOMANYINPUTARGUMENTSENDOFFTIME800產(chǎn)生理論的航跡X,YTRAJECTORYTS,OFFTIMEPVDDNCEILOFFTIME/TSRANDNSTATE,SUM100CLOCK設(shè)置隨機(jī)數(shù)發(fā)生器FORI1NVXIDRANDN1觀測噪聲，兩者獨(dú)立VYIDRANDN1ZXIXIVXI實(shí)際觀測值ZYIYIVYIEND產(chǎn)生觀測數(shù)據(jù)FORN1MON用卡爾曼濾波得到估計(jì)的航跡XEKALMAN_FILTERTS,OFFTIME,D,0YEKALMAN_FILTERTS,OFFTIME,D,1誤差矩陣XER1N,NX1NXE1NYER1N,NY1NYE1NEND濾波誤差的均值XERBMEANXER,2YERBMEANYER,2濾波誤差的標(biāo)準(zhǔn)差XSTDSTDXER,1,2計(jì)算有偏的估計(jì)值，F(xiàn)LAG1YSTDSTDYER,1,2作圖FIGUREPLOTX,Y,RHOLDONPLOTZX,ZY,GHOLDONPLOTXE,YE,BHOLDOFFAXIS15005000100010000,GRIDONLEGEND真實(shí)軌跡,觀測數(shù)據(jù),濾波估計(jì)FIGURESUBPLOT2,2,1PLOTXERBAXIS05005050XLABEL觀測次數(shù)YLABELX方向?yàn)V波誤差均值,GRIDONSUBPLOT2,2,2PLOTYERBAXIS05005050XLABEL觀測次數(shù)YLABELY方向?yàn)V波誤差均值,GRIDONSUBPLOT2,2,3PLOTXSTDAXIS05000150XLABEL觀測次數(shù)YLABELX方向?yàn)V波誤差標(biāo)準(zhǔn)值,GRIDONSUBPLOT2,2,4PLOTYSTDAXIS05000150XLABEL觀測次數(shù)YLABELY方向?yàn)V波誤差標(biāo)準(zhǔn)值,GRIDONXXERYYER附錄KALMAN_FILTERMFUNCTIONXEKALMAN_FILTERTS,OFFTIME,D,FLAGKALMAN_FILTER采用KALMAN濾波方法，從觀測數(shù)值中得到航跡的估計(jì)XE輸出X軸方向上的誤差TS采樣時(shí)間，即雷達(dá)工作周期OFFTIME仿真截止時(shí)間D噪聲的標(biāo)準(zhǔn)差值FLAG判斷計(jì)算X軸或Y軸數(shù)據(jù),0X,1YIFNARGIN4ERROR輸入的變量過多，請檢查ENDIFOFFTIME600ERROR仿真時(shí)間必須大于600S，請重新輸入ENDPVDD噪聲的功率NCEILOFFTIME/TS采樣點(diǎn)數(shù)SIGMA10加速度方向的的擾動(dòng)SWITCHFLAGCASE0AZEROS1,4000075ONES1,200ZEROS1,1003ONES1,50ZEROS1,OFFTIME660對不同時(shí)段的加速度進(jìn)行描述CASE1AZEROS1,4000075ONES1,200ZEROS1,1003ONES1,50ZEROS1,OFFTIME660OTHERWISEERROR輸入僅能為0或1END定義系統(tǒng)的狀態(tài)方程PHI1,TS0,1GAMMATSTS/2TSC10RPVQSIGMA2WRANDNSTATE,SUM100CLOCK設(shè)置隨機(jī)數(shù)發(fā)生器FORN0TSOFFTIME1WN/TS1AN1SIGMARANDN1,1ENDXESTZEROS2,1用前K1時(shí)刻的輸出值估計(jì)K時(shí)刻的預(yù)測值XFLIZEROS2,1K時(shí)刻KALMAN濾波器的輸出值XESZEROS2,1預(yù)測輸出誤差XEFZEROS2,1濾波后輸出的誤差PXEZEROS2,1預(yù)測輸出誤差均方差矩陣PXZEROS2,1濾波輸出誤差均方差矩陣XEZEROS1,N得到最終的濾波輸出值，僅僅考慮距離分量X,YTRAJECTORYTS,OFFTIME產(chǎn)生理論的航跡FORI1NVXIDRANDN1觀測噪聲，兩者獨(dú)立VYIDRANDN1ZXIXIVXI實(shí)際觀測值ZYIYIVYIENDSWITCHFLAGCASE0XFLIZX2ZX2ZX1/TS利用前兩個(gè)觀測值來對初始條件進(jìn)行估計(jì)XEFVX2TSW1/2VX1VX2/TSPXPV,PV/TSPV/TS,2PV/TSTSTSQ/4FORK3NXESTPHIXFLI更新該時(shí)刻的預(yù)測值XESPHIXEFGAMMAWK1預(yù)測輸出誤差PXEPHIPXPHIGAMMAQGAMMA預(yù)測誤差的協(xié)方差陣KPXECINVCPXECRKALMAN濾波增益XFLIXESTKZXKCXESTXEFEYE2KCXESKVXKPXEYE2KCPXEXEKXFLI1,1ENDXE1ZX1XE2ZX2CASE1XFLIZY2ZY2ZY1/TS利用前兩個(gè)觀測值來對初始條件進(jìn)行估計(jì)XEFVY2TSW1/2VY1VY2/TSPXPV,PV/TSPV/TS,2PV/TSTSTS