卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用

1、卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用摘要：機(jī)動(dòng)卡爾曼算法(VD算法)在擴(kuò)展卡爾曼濾波諸算法中原理較為簡(jiǎn)單，目 標(biāo)跟蹤效果也較好。一.模型建立(1)非機(jī)動(dòng)模型(勻速直線運(yùn)動(dòng))系統(tǒng)模型X(k 1) =：X(k) GW(k)其中o o T 1 o o 1 o T 1 o o1 o o o -o OT2 1-Gx(k) 1X(k)二Vx(k) y(k)Vy(k)Wx(k)lW(k)/(k)EW(k) =0 ；EW(k)WT(j)HQ kj測(cè)量模型為：Z(k) =HX(k) V(k);其中000Ito010V(k)為零均值,協(xié)方差陣為R白噪聲,與W(k)不相關(guān)。(2)機(jī)動(dòng)模型系統(tǒng)模型Xm(k 1)：mX m(k

2、) Gm*Wm(k);其中Vxm(k)ym(k)壬mm; =Vy (k)aym(k)fy (k)10000EWm(k)Xm(k)二T1000000100000T100T22T00100 10TZT01EWm(k)Wm (j)kj觀測(cè)模型與機(jī)動(dòng)模型的相同,只是H矩陣為Hm o0T24T201H=0 0.Kalman濾波算法作為一般的Kalman濾波算法其算法可以描述如下:刃(k/k -1) =)?(k -1/k -1)P(k/k -1)=：P(k -1)/k -1) ：T GQ(k -1)GTTT-AK(k) =P(k/k-1)H HP(k/k-1)H R刃(k/k) = >?(k/k -

3、1)K(k)Z(k) - HX (k/k - 1)P(k/k) =P(k/k -1) - K(k)HP(k/k -1)起始估計(jì)值為一 Zx(2)X 2/2 二|N(2) Zxj/T|Zy(2)二 Zy(2)-Zy(1)/T_起始估計(jì)的估計(jì)誤差為一-vx(2)1I.u +vx(1) vx(2)X (2/2)二2 xT-Vy(2)T ，Vy(1)-Vy(2) 三 uy(1)-起始估計(jì)的估計(jì)誤差協(xié)方差矩陣為-2 CT<i2/T0012T 弒Px(2/2)a/T4T200=002 CT<t2/T十222T Ouy2%00cj2/Ty + y1一-4T2三.VD算法描述VD算法采用兩種模型，

4、即非機(jī)動(dòng)模型和機(jī)動(dòng)模型，無(wú)機(jī)動(dòng)時(shí)濾波器工作于正 常模式(低階模型),用機(jī)動(dòng)檢測(cè)器監(jiān)視機(jī)動(dòng)，一旦檢測(cè)到機(jī)動(dòng)，模型中立即增加一 個(gè)狀態(tài)變量，用機(jī)動(dòng)模型跟蹤直至下一次判決而退回到正常的非機(jī)動(dòng)模型。由于米用機(jī)動(dòng)的VD算法，因此涉及到的一個(gè)關(guān)鍵環(huán)節(jié)就是機(jī)動(dòng)檢測(cè)冋題。濾波器開(kāi)始工作于正常模式，其輸出的新息序列為v(k),令(k) _ "(k _1)、.(k)、(k)二vT(k)S(k)v(k)其中,S(k)是v(k)的協(xié)方差矩陣。取丄=(1 -作為檢測(cè)機(jī)動(dòng)的有效窗口長(zhǎng)度，機(jī)動(dòng)檢測(cè)的方法為：1.如果(k)_Th,則認(rèn)為目標(biāo)在開(kāi)始有一恒定的加速度加入,這時(shí)目標(biāo) 模型應(yīng)由低階模型轉(zhuǎn)向高階模型。由高階機(jī)

5、動(dòng)模型退回低階非機(jī)動(dòng)模型的檢測(cè)方法是檢測(cè)加速度估計(jì)值是否 有統(tǒng)計(jì)顯著性意義。k令'a(k)八 ' a(j)j 4其中，=(k)二aT(k/k)Pam(k/k)%(k/k) , a(k./k)是加速度分量的估計(jì)值,Pam(k/k)是協(xié)方差矩陣的對(duì)應(yīng)塊。2如果(k) <Ta，則加速度估計(jì)無(wú)顯著性意義,濾波器退出機(jī)動(dòng)模型。前面提到了起始狀態(tài)的初始化，下面講一下當(dāng)檢測(cè)到機(jī)動(dòng)時(shí)，濾波器的初始 化問(wèn)題。當(dāng)在第K次檢測(cè)到機(jī)動(dòng)時(shí)，濾波器假定在k-1開(kāi)始有一恒定的加速度 在窗內(nèi)的狀態(tài)估計(jì)應(yīng)修正如下： 首先,加速度在k - .:的估計(jì)為am（k 一.：/k- =T-Zx（k-：）-Zx（k-

6、 /k： -1）a?m（k-：./k- . ） 2Zy（k . J Zy（k ：/k,；.1）Z是對(duì)測(cè)量的預(yù)測(cè)值,Z?x（k _ / k -1） = ?k 八 / k- -1）Zy（k：/k.： -1） = ?（k- / k- -1）在k - ."：的位置估計(jì)為：5?m（k ：/k,；.1） = Zx（k ：）?m（k - ：/k _ ： _1） =Zy（k - ：）在k -厶的速度估計(jì)為：V（k 一厶/k 一 ：）=Vx（k - ： -1/k - ： -1） Tax（k - ： -1/k - ： -1）V；(k _ /k _ ) =Vy(k _ ： 1/k _ ： -1) Tay(

7、k _一1/k _ ： _1)協(xié)方差矩陣修正為：2 2R；(k _A/k _) =Rn ; R；(k_A/k_A) =Rn ; R?(k_ A/k _)=尹隔42P5(k _/ k=尹Rt +邱(1< _A1 / k _A 1) +2TR2(k_A -1 / k1)幵 2F22(k 1/ k -1)P；(k 兇 k ® =豐 R1 +半 Rdk 也一1/k A1) +甩仮一也1/k 也1)斗 R2(k 也1/k 也-1)4426p25(k 也/k 0 3R|1 +3p1(k 1/k A1) +及2(k 心1/k 1) +-2 p2(k心一1 /k 1)T3T3TT2四.仿真實(shí)驗(yàn)

8、假定有一二坐標(biāo)雷達(dá)對(duì)一平面上運(yùn)動(dòng)的目標(biāo)進(jìn)行觀測(cè)，目標(biāo)在t =0-400秒沿著y軸作恒速直線運(yùn)動(dòng)，運(yùn)動(dòng)速度為-15米/秒，目標(biāo)的起始點(diǎn)為（2000米,10000 米），在t= 400-600秒向軸x方向做900的慢轉(zhuǎn)彎，加速度為Ux二Uy二0.075米 /秒2，完成慢轉(zhuǎn)彎后加速度將降為零，從t=610秒開(kāi)始做90度的快轉(zhuǎn)彎，加速度為0.3米/秒2，在660秒結(jié)束轉(zhuǎn)彎，加速度降至零。雷達(dá)掃描周期 T=2秒，X 和丫獨(dú)立地進(jìn)行觀測(cè)，觀測(cè)噪聲的標(biāo)準(zhǔn)差均為100米。描述如下：120001I1IIF10000 L8000 .6000 .4000 2000 -0 -2000 :IIIIII15002000250030003500400045005000程序算法中各參數(shù)為：加權(quán)衰減因子:=0.85,機(jī)動(dòng)檢測(cè)門(mén)限=25 ；退出 機(jī)動(dòng)的檢測(cè)門(mén)限Ta =9.49。在跟蹤的開(kāi)始,首先采用非機(jī)動(dòng)模型,從第20次采樣 開(kāi)始,激活機(jī)動(dòng)檢測(cè)器。120001000080006000400020000-2000X坐標(biāo)濾波誤差標(biāo)準(zhǔn)差曲線Y坐標(biāo)濾波誤差均值曲線150100- '550- .Il；W K»i0-FEd0100200300400Y坐標(biāo)濾波誤差標(biāo)準(zhǔn)差曲線通過(guò)上圖，可看到：VD算法有4次機(jī)動(dòng),分別對(duì)應(yīng)目標(biāo)的2次加速運(yùn)