《目標(biāo)跟蹤系統(tǒng)中的濾波方法》課件第9章

第9章非線性濾波算法在目標(biāo)跟蹤中的應(yīng)用

9.1基于高斯粒子濾波的機(jī)載GMTI雷達(dá)跟蹤9.2基于邊緣粒子濾波的目標(biāo)跟蹤算法9.3基于求積分卡爾曼濾波的交互式多模型算法9.4小結(jié)9.1基于高斯粒子濾波的機(jī)載GMTI雷達(dá)跟蹤

9.1.1概述

在現(xiàn)代戰(zhàn)爭(zhēng)中，海陸空三軍聯(lián)合的立體化作戰(zhàn)網(wǎng)絡(luò)能夠最大程度地發(fā)揮戰(zhàn)斗實(shí)體的威力。利用直升機(jī)或者無(wú)人駕駛戰(zhàn)斗機(jī)對(duì)地面目標(biāo)進(jìn)行識(shí)別、定位和跟蹤在聯(lián)合作戰(zhàn)過(guò)程中起著重要作用，它能夠迅速準(zhǔn)確地識(shí)別目標(biāo)，對(duì)敵方目標(biāo)進(jìn)行定位或者跟蹤，從而實(shí)現(xiàn)精確打擊。本節(jié)討論機(jī)載地面移動(dòng)目標(biāo)指示(GroundMovingTargetIndicator，GMTI)雷達(dá)跟蹤過(guò)程中的非線性濾波問(wèn)題。針對(duì)機(jī)載GMTI雷達(dá)非線性濾波問(wèn)題，常規(guī)的解決方法是采用擴(kuò)展卡爾曼濾波(EKF)或者不敏卡爾曼濾波(UnscentedKalmanFilter，UKF)。這兩種算法的運(yùn)算速度雖然相對(duì)較快，但是現(xiàn)代戰(zhàn)爭(zhēng)需要更精確的濾波結(jié)果。為了進(jìn)一步提高濾波的精確程度，文獻(xiàn)[1]將粒子濾波算法(ParticleFilter，PF)[2-4]應(yīng)用到機(jī)載GMTI目標(biāo)跟蹤過(guò)程中。該文獻(xiàn)實(shí)驗(yàn)結(jié)果表明，為了使常規(guī)粒子濾波算法的誤差性能高于EKF和UKF，需要參與濾波的粒子數(shù)要超過(guò)10000個(gè)，時(shí)間復(fù)雜度很高。文獻(xiàn)[5]提出了不敏粒子濾波算法(UnscentedParticleFilter，UPF)，其濾波誤差比常規(guī)粒子濾波算法低，但是時(shí)間復(fù)雜度仍然很高。針對(duì)機(jī)動(dòng)目標(biāo)的機(jī)載GMTI雷達(dá)跟蹤問(wèn)題，文獻(xiàn)[6]提出了一種變結(jié)構(gòu)模型的粒子濾波器，文獻(xiàn)[7]提出了交互式多模型方法(InteractiveMultipleModel，IMM)，其中交互式多模型方法在機(jī)載GMTI雷達(dá)跟蹤過(guò)程中的運(yùn)算誤差較低，但是時(shí)間復(fù)雜度相對(duì)較高。此外還有學(xué)者對(duì)機(jī)載GMTI雷達(dá)的數(shù)據(jù)融合問(wèn)題展開(kāi)了研究[8,9]。文獻(xiàn)[10]針對(duì)狀態(tài)變量在處理過(guò)程中服從高斯分布的情況，提出了高斯粒子濾波(GaussianParticleFilter，GPF)算法。文獻(xiàn)[11]將高斯粒子濾波算法應(yīng)用到機(jī)動(dòng)目標(biāo)跟蹤過(guò)程中。文獻(xiàn)[12]采用信息距離度量方法分析了應(yīng)用于高斯分布的EKF、UKF和高斯粒子濾波算法，結(jié)果表明：高斯粒子濾波算法比EKF、UKF具有更高的濾波精度；與常規(guī)粒子濾波算法相比，在使用同樣的粒子數(shù)參與濾波的情況下，能取得更好的濾波效果。鑒于高斯粒子濾波具有上述優(yōu)點(diǎn)，本節(jié)假定目標(biāo)狀態(tài)向量服從高斯分布，將高斯粒子濾波算法應(yīng)用到機(jī)載GMTI雷達(dá)跟蹤中，取得了較好的效果。計(jì)算機(jī)仿真結(jié)果表明GPF的性能優(yōu)于常規(guī)PF。9.1.2機(jī)載GMTI雷達(dá)

機(jī)載GMTI雷達(dá)通過(guò)安裝在飛機(jī)上的雷達(dá)裝置對(duì)地面移動(dòng)目標(biāo)進(jìn)行跟蹤。其測(cè)量分量包括方位角α、徑向距離r和徑向距離的導(dǎo)數(shù)r三個(gè)分量。假定目標(biāo)在地面勻速運(yùn)動(dòng)，而機(jī)載GMTI雷達(dá)傳感器在離地面H公里處勻速運(yùn)動(dòng)。若x

k=[ξxk

ξyk

ξxkξyk]T表示目標(biāo)在k時(shí)刻的狀態(tài)向量，則目標(biāo)運(yùn)動(dòng)的離散動(dòng)態(tài)方程為

xk+1=Φkxk+wk

(9-1)

其中：Φk表示目標(biāo)的狀態(tài)轉(zhuǎn)移矩陣；wk表示方差為Qk的零均值高斯噪聲。若Tk=tk－tk－1表示時(shí)間采樣間隔，則···其中q1、q2表示x或y方向的功率譜密度。

GMTI雷達(dá)測(cè)量向量為y=[r

α

r]T，其測(cè)量方程為

zk=h(xk，sk)+vk

(9-2)·其中：sk=[skx

sky

skz]T表示k時(shí)刻傳感器所處位置；vk=[vkr

vkα

vkr]T表示k時(shí)刻的量測(cè)噪聲。假設(shè)傳感器位置sk不存在誤差，且vk是零均值獨(dú)立高斯噪聲，其協(xié)方差矩陣為·各個(gè)量測(cè)分量方程為(為了書(shū)寫(xiě)方便，省略下標(biāo)k)(9-3)(9-4)(9-5)9.1.3算法描述及步驟

機(jī)載GMTI雷達(dá)跟蹤系統(tǒng)目標(biāo)運(yùn)動(dòng)模型如式(9-1)所示，測(cè)量方程如式(9-2)所示。濾波問(wèn)題就是給定狀態(tài)向量的初始分布p(x0)，根據(jù)不同時(shí)刻的量測(cè)向量，計(jì)算相應(yīng)時(shí)刻的狀態(tài)估計(jì)值。

濾波過(guò)程分為預(yù)測(cè)階段和更新階段。假定狀態(tài)向量在k－1時(shí)刻的后驗(yàn)分布為p(xk－1|yk－1)，則一步預(yù)測(cè)概率密度函數(shù)為(9-6)由于狀態(tài)向量服從高斯分布p(xk－1|y0:k－1)~N(xk－1;μk－1,Σk－1),上式可近似為(9-7)式(9-7)可以通過(guò)MonteCarlo近似方法計(jì)算。首先針對(duì)k－1時(shí)刻狀態(tài)向量的后驗(yàn)密度函數(shù)抽樣，即(9-8)然后計(jì)算一步預(yù)測(cè)的概率密度函數(shù)為(9-9)由于狀態(tài)轉(zhuǎn)移概率密度函數(shù)p(xk|xik－1)可以通過(guò)式(9-1)獲得，從而計(jì)算出每一個(gè)粒子的狀態(tài)轉(zhuǎn)移值xik。此時(shí)，概率密度函數(shù)p(xk|y0:k－1)的均值和方差計(jì)算如下(9-10)因?yàn)闋顟B(tài)向量分布服從高斯分布，即p(xk|y0:k－1)≈N(xk;μk，Σk),所以利用k時(shí)刻的觀測(cè)向量更新后的狀態(tài)向量概率分布為－－(9-11)式(9-11)中的積分往往難于直接計(jì)算，通過(guò)帶有權(quán)值的M個(gè)粒子近似該后驗(yàn)分布，即(9-12)如果粒子從提議分布π(xk|y1:k)中獲得，則權(quán)值wik為(9-13)然后對(duì)權(quán)值歸一化，即(9-14)最終的狀態(tài)向量的估計(jì)值及其協(xié)方差計(jì)算如下(9-15)重要性提議分布π(·)的選擇和常規(guī)粒子濾波算法類似，其中最簡(jiǎn)單的重要性提議分布為(9-16)也可以采用EKF或者UKF算法獲得k時(shí)刻的狀態(tài)更新值μk|k及其協(xié)方差Σk|k,從而獲得更為準(zhǔn)確的重要性提議分布p(xk|y0:k－1)=N(xk;μk|k，Σk|k)。綜上所述，基于高斯粒子濾波算法的GMTI雷達(dá)跟蹤步驟如下:

輸入：狀態(tài)向量均值μk－1和方差Σk－1，量測(cè)值yk;

輸出：k時(shí)刻的狀態(tài)向量均值μk和方差Σk;

Step1：利用式(9-8)采樣，獲得粒子集{xik－1}Ni=1；

Step2：計(jì)算每個(gè)粒子的一步預(yù)測(cè)，得到粒子集{xik}Ni=1;

Step3：利用式(9-10)計(jì)算一步預(yù)測(cè)的均值μk及其協(xié)方差Σk；－－－－－－

Step4：把式(9-16)作為重要性函數(shù)進(jìn)行采樣，獲得粒子集{xik}Ni=1；

Step5：利用式(9-13)計(jì)算每個(gè)粒子相應(yīng)的權(quán)值，利用式(9-14)對(duì)權(quán)值進(jìn)行歸一化，得到粒子集相對(duì)應(yīng)的權(quán)值集{wi}Ni=1；

Step6：利用式(9-15)計(jì)算k時(shí)刻的均值μk和協(xié)方差Σk。9.1.4仿真實(shí)驗(yàn)及結(jié)果分析

目標(biāo)運(yùn)動(dòng)方程和GMTI測(cè)量方程為式(9-1)和式(9-2)，圖9.1為機(jī)載雷達(dá)地面指示跟蹤示意圖。實(shí)驗(yàn)中其它參數(shù)設(shè)置為：T=1秒；x(t0)=[100

200

9.62

5.56]T；q1=q2=0.1；GMTI雷達(dá)關(guān)于徑向距離r、方位角α、徑向距離導(dǎo)數(shù)r的測(cè)量誤差標(biāo)準(zhǔn)差分別為σr=20米、σα=0.001弧度、σr=1米/秒。另外，初始狀態(tài)和初始協(xié)方差的求解方法參見(jiàn)文獻(xiàn)[1]。

固定500個(gè)粒子參與濾波，MonteCarlo仿真50次，仿真總步數(shù)為500步。狀態(tài)估計(jì)的均方根誤差計(jì)算公式為··(9-17)圖9.1機(jī)載雷達(dá)地面指示跟蹤示意圖其中，xj表示第j次MonteCarlo循環(huán)中狀態(tài)向量估計(jì)值，xj表示狀態(tài)向量的真實(shí)值。均方根誤差如圖9.2(a)和(b)所示。在濾波的初始階段，如圖中1～100步，與PF相比，GPF的均方根誤差性能改善很多，但是在150步以后，兩種濾波算法的誤差性能基本相當(dāng)。圖9.3給出了目標(biāo)運(yùn)動(dòng)軌跡和濾波軌跡。圖9.4給出了用幾種算法濾波所需要的時(shí)間?？梢钥闯?，雖然GPF算法的時(shí)間復(fù)雜度比EKF、UKF算法高很多，但是與粒子濾波算法相比，其時(shí)間復(fù)雜度低將近30%。^圖9.2均方根誤差圖9.3目標(biāo)真實(shí)軌跡和濾波軌跡圖9.4耗費(fèi)時(shí)間對(duì)比為了說(shuō)明GPF算法在濾波過(guò)程中誤差收斂速度快的特點(diǎn)，采用EKF、UKF以及使用不同粒子數(shù)的PF和GPF算法，進(jìn)行100步濾波，采用MonteCarlo方法仿真50次。各種濾波算法的均方根誤差以及誤差標(biāo)準(zhǔn)差結(jié)果如表9.1所示。從中可以看出，當(dāng)粒子數(shù)目不斷增大時(shí)，PF和GPF兩種濾波算法的均方根誤差和標(biāo)準(zhǔn)差不斷減小；PF算法在使用大約超過(guò)

10000個(gè)粒子的情況下，誤差性能優(yōu)于EKF和UKF；GPF算法在使用超過(guò)5000個(gè)粒子的情況下，誤差性能優(yōu)于EKF和UKF。并且，在使用同樣粒子數(shù)的條件下，GPF的均方根誤差及其標(biāo)準(zhǔn)差比PF的均方根誤差及其標(biāo)準(zhǔn)差小。表9.1數(shù)據(jù)表明，在處理高斯分布的機(jī)載GMTI雷達(dá)目標(biāo)跟蹤過(guò)程中，GPF是一種比PF更有效的非線性濾波算法。9.2基于邊緣粒子濾波的目標(biāo)跟蹤算法

9.2.1概述

粒子濾波(ParticleFilter，PF)算法[2-4]思想是采用隨機(jī)采樣的方法獲取粒子，然后用大量粒子來(lái)近似狀態(tài)變量的后驗(yàn)分布。該算法在強(qiáng)非線性系統(tǒng)中能夠取得較好的濾波結(jié)果。該算法最大的問(wèn)題是時(shí)間復(fù)雜度高。粒子濾波算法在目標(biāo)跟蹤中的應(yīng)用，已經(jīng)有很多成果供借鑒[13-16]。為了進(jìn)一步提高粒子濾波算法在目標(biāo)跟蹤過(guò)程中的精度，許多學(xué)者采用改進(jìn)的或者新型的粒子濾波算法

[17-18]。在這些研究工作中，通常以增加較大的時(shí)間復(fù)雜度來(lái)提高或者保證粒子濾波結(jié)果的精確程度。本節(jié)的目標(biāo)同樣是為了提高目標(biāo)跟蹤過(guò)程中粒子濾波算法的濾波性能。在目標(biāo)跟蹤過(guò)程中，狀態(tài)向量往往包括位置、速度、加速度，而傳感器量測(cè)值往往只和其中的一部分量有關(guān)系。將狀態(tài)向量中和量測(cè)值有關(guān)的部分作為非線性子向量，而與量測(cè)值無(wú)關(guān)的部分作為線性子向量，然后采用邊緣粒子濾波算法進(jìn)行濾波。該方法的特點(diǎn)是在計(jì)算量增加不大的條件下大幅度提高濾波結(jié)果的精度。仿真結(jié)果表明，在使用同樣粒子數(shù)的條件下，邊緣粒子濾波算法的誤差性能比標(biāo)準(zhǔn)粒子濾波算法的性能好，邊緣粒子濾波

算法的時(shí)間復(fù)雜度比標(biāo)準(zhǔn)粒子濾波算法的時(shí)間復(fù)雜度僅高6%左右；在粒子數(shù)較小的情況下，標(biāo)準(zhǔn)粒子濾波算法結(jié)果可能已經(jīng)發(fā)散，而邊緣粒子濾波算法結(jié)果仍能夠保持較好的濾波精度。9.2.2問(wèn)題描述

在目標(biāo)運(yùn)動(dòng)過(guò)程中，根據(jù)不同的目標(biāo)運(yùn)動(dòng)特點(diǎn)建立相應(yīng)的運(yùn)動(dòng)模型[19-20]。其中一類簡(jiǎn)單的模型是勻加速(ConstantAcceleration，CA)模型。假定目標(biāo)在二維空間中運(yùn)動(dòng)，目標(biāo)狀態(tài)向量為

則目標(biāo)勻加速運(yùn)動(dòng)模型的離散時(shí)間方程為(9-18)其中，T為采樣時(shí)間間隔，wk為均值為0、方差為Q的過(guò)程噪聲，該噪聲是直接作用在加速度分量的噪聲。假設(shè)在目標(biāo)跟蹤過(guò)程中，傳感器的量測(cè)值為目標(biāo)的徑向距離r和偏轉(zhuǎn)角θ。其量測(cè)方程為(9-19)由于在量測(cè)方程中，只用到了狀態(tài)向量的兩個(gè)位置分量，而剩余的4個(gè)分量，包括速度分量和加速度分量都與量測(cè)值無(wú)關(guān)。因此，可以將狀態(tài)變量xk看做線性子向量和非線性子向量?jī)蓚€(gè)部分，即xk=[xlk

xnk]T，則式(9-18)可以轉(zhuǎn)換為(9-20)

此時(shí)，式(9-21)采用卡爾曼濾波算法，式(9-20)采用粒子濾波算法。在線性高斯系統(tǒng)條件下，卡爾曼濾波是最優(yōu)的。在用粒子刻畫(huà)狀態(tài)向量的分布時(shí)，低維空間需要的粒子數(shù)少，而高維空間需要的粒子數(shù)多。因此，在相同粒子數(shù)條件下，邊緣粒子濾波算法的誤差性能比標(biāo)準(zhǔn)粒子濾波算法高。(9-21)9.2.3算法描述及步驟

邊緣粒子濾波算法[21-22]的思想是：將狀態(tài)向量劃分為線性子向量和非線性子向量?jī)蓚€(gè)部分，線性子向量采用卡爾曼濾波，而非線性子向量采用標(biāo)準(zhǔn)的粒子濾波。在粒子數(shù)相同的條件下，邊緣粒子濾波算法可以大幅度提高濾波精度。這是因?yàn)樵谶吘壛Ｗ訛V波算法中，卡爾曼濾波算法可以獲得線性高斯條件下的最優(yōu)狀態(tài)估計(jì)，而此時(shí)粒子濾波算法所處理的非線性狀態(tài)子向量維數(shù)有所降低，從而能夠較大幅度地提高濾波性能。

假定狀態(tài)向量xk=[xnk

xlk]T，其中，xlk表示線性狀態(tài)子向量，xnk表示非線性狀態(tài)子向量。此時(shí)，系統(tǒng)過(guò)程噪聲可以做如下劃分系統(tǒng)的動(dòng)態(tài)方程和量測(cè)方程分別為(9-23)(9-22)(9-24)其中，wk和ek分別為過(guò)程噪聲和量測(cè)噪聲。由于xnk和xlk相互獨(dú)立，根據(jù)貝葉斯定理得(9-25)其中，

。假定p(xlk|Xnk，Yk)是線性高斯系統(tǒng)，因此采用卡爾曼濾波可以獲得最小方差意義下的最優(yōu)估計(jì)，而p(Xnk|Yk)是非線性系統(tǒng)，采用粒子濾波可進(jìn)行狀態(tài)估計(jì)。

對(duì)于線性狀態(tài)子向量，更新概率密度分布為

。其中(9-26)線性狀態(tài)子向量的一步預(yù)測(cè)概率密度為p(xlk+1|Xnk+1，Yk)=N(xlk+1|k，Pk+1|k)。其中^(9-27)(9-28)式(9-27)和式(9-28)所涉及的一些參數(shù)定義如下詳細(xì)證明過(guò)程參見(jiàn)文獻(xiàn)[21]。非線性狀態(tài)子向量的處理采用粒子濾波方法，狀態(tài)更新概率密度函數(shù)和一步預(yù)測(cè)概率密度函數(shù)分別為9.2.4仿真實(shí)驗(yàn)及結(jié)果分析

仿真實(shí)驗(yàn)采用二維空間中的目標(biāo)跟蹤場(chǎng)景。假設(shè)目標(biāo)做勻加速運(yùn)動(dòng)，動(dòng)態(tài)方程和量測(cè)方程分別如式(9-18)和式(9-19)所示。實(shí)驗(yàn)過(guò)程中的參數(shù)設(shè)置為：T=1s；過(guò)程噪聲方差為Q=diag([1

1

1

1

0.01

0.01])；量測(cè)噪聲方差為R=diag([100

1e－6])；狀態(tài)向量初值為x0=[0

0

0

0

0

0]T；狀態(tài)向量初始協(xié)方差為P0=diag([100

100

10

10

1

1])；對(duì)目標(biāo)連續(xù)跟蹤50秒；MonteCarlo仿真次數(shù)為M=50。

實(shí)驗(yàn)一邊緣粒子濾波算法與標(biāo)準(zhǔn)粒子濾波算法性能比較

為了比較標(biāo)準(zhǔn)粒子濾波算法與邊緣粒子濾波算法在目標(biāo)跟蹤過(guò)程中不同時(shí)刻的性能，兩種算法均使用相同粒子數(shù)進(jìn)行濾波。實(shí)驗(yàn)中粒子數(shù)設(shè)定為N=2000。計(jì)算濾波結(jié)果的均方根誤差。均方根誤差計(jì)算公式為(9-31)仿真結(jié)果如圖9.5所示。圖9.5(a)表明，從時(shí)刻8開(kāi)始，邊緣粒子濾波的位置誤差低于標(biāo)準(zhǔn)粒子濾波的位置誤差；圖9.5(b)表明，從時(shí)刻15開(kāi)始，邊緣粒子濾波的速度誤差低于標(biāo)準(zhǔn)粒子濾波的速度誤差；加速度分量的誤差情形與圖9.5(a)和(b)類似，限于篇幅，結(jié)果沒(méi)有列出。這說(shuō)明邊緣粒子濾波在濾波的前期階段，表現(xiàn)出的誤差性能不如標(biāo)準(zhǔn)粒子濾波，但是在濾波的后期，邊緣粒子濾波算法表現(xiàn)出優(yōu)越的誤差性能，各個(gè)分量的誤差都低于標(biāo)準(zhǔn)粒子濾波算法。出現(xiàn)這種現(xiàn)象的原因是因?yàn)樵诜抡孢^(guò)程中，標(biāo)準(zhǔn)粒子濾波算法的噪聲各分量相互獨(dú)立，而邊緣粒子濾波算法使用的線性子向量和非線性子向量不是相互獨(dú)立的。這一現(xiàn)象也表明邊緣粒子濾波算法在噪聲相關(guān)情況下具有較強(qiáng)的魯棒性。圖9.5不同時(shí)刻的均方根誤差為了考察邊緣粒子濾波和標(biāo)準(zhǔn)粒子濾波的時(shí)間復(fù)雜度，對(duì)上述濾波過(guò)程兩種算法的運(yùn)行時(shí)間進(jìn)行統(tǒng)計(jì)。統(tǒng)計(jì)結(jié)果表明，邊緣粒子濾波和標(biāo)準(zhǔn)粒子濾波在實(shí)驗(yàn)過(guò)程中濾波一次需要的時(shí)間分別是8.13秒和7.67秒，即邊緣粒子濾波算法的時(shí)間復(fù)雜度要比標(biāo)準(zhǔn)粒子濾波算法的高一些。

實(shí)驗(yàn)二在粒子數(shù)不同的情況下兩種粒子濾波算法誤差性能分析

為了考察邊緣粒子濾波算法在粒子數(shù)不同的情況下的誤差性能，計(jì)算分別采用不同的粒子數(shù)情況下的濾波誤差，其中粒子數(shù)依次選取為PN={600，700，…，2000}。此時(shí)的均方根誤差計(jì)算為(9-32)仿真結(jié)果如圖9.6所示。圖9.6(a)和(b)表明，隨著粒子數(shù)的增加，邊緣粒子濾波各個(gè)分量的均方根誤差盡管呈緩慢減小趨勢(shì)，但是減小的幅度很小，也就是說(shuō)，均方根誤差表現(xiàn)較為平穩(wěn)，并且沒(méi)有出現(xiàn)濾波結(jié)果發(fā)散的現(xiàn)象。隨著粒子數(shù)的增加，標(biāo)準(zhǔn)粒子濾波各個(gè)分量的均方根誤差大體上呈減小趨勢(shì)，然而其中有濾波發(fā)散的現(xiàn)象，如當(dāng)粒子數(shù)為600、700、900、1200時(shí)，甚至當(dāng)粒子數(shù)達(dá)到1600時(shí)，在MonteCarlo仿真過(guò)程中都出現(xiàn)了發(fā)散的現(xiàn)象，因此，在這些時(shí)刻對(duì)應(yīng)的均方根誤差為無(wú)窮大，在圖9.6中無(wú)法顯示這些均方根誤差值為無(wú)窮大時(shí)的誤差軌跡。需要指出的是，加速度分量誤差情況和圖9.6(a)、(b)類似，限于篇幅，相應(yīng)結(jié)果沒(méi)有列出。實(shí)際上，隨著MonteCarlo仿真次數(shù)的增加，只要粒子數(shù)不是足夠多(例如粒子數(shù)要大于10000個(gè))，在使用標(biāo)準(zhǔn)粒子濾波時(shí)，就有可能出現(xiàn)濾波結(jié)果發(fā)散的現(xiàn)象。相對(duì)而言，邊緣粒子濾波比較穩(wěn)定，在粒子數(shù)較少的情況下濾波結(jié)果不會(huì)發(fā)散，而且濾波誤差較為穩(wěn)定。如圖9.6所示，在整個(gè)粒子數(shù)從600增加到2000的過(guò)程中，邊緣粒子濾波算法沒(méi)有出現(xiàn)濾波結(jié)果發(fā)散的情形。實(shí)際上，使用更少的粒子數(shù)，例如當(dāng)粒子數(shù)取200到600之間時(shí)，邊緣粒子濾波結(jié)果仍然是收斂的。進(jìn)一步的仿真實(shí)驗(yàn)結(jié)果表明，邊緣粒子濾波算法在濾波過(guò)程中沒(méi)有出現(xiàn)發(fā)散的現(xiàn)象，當(dāng)然，此時(shí)濾波結(jié)果誤差會(huì)變大。圖9.6在不同粒子數(shù)的條件下的均方根誤差9.3基于求積分卡爾曼濾波的交互式多模型算法

9.3.1概述

卡爾曼濾波針對(duì)的是模型單一的線性系統(tǒng)濾波問(wèn)題。在機(jī)動(dòng)目標(biāo)跟蹤系統(tǒng)中，系統(tǒng)動(dòng)態(tài)模型難以用單一模型來(lái)描述，此時(shí)需要使用其它方法獲得更準(zhǔn)確的狀態(tài)估計(jì)，其中，多模型算法是一類有效的濾波算法。在過(guò)去的三十年中，人們對(duì)多模型算法進(jìn)行了深入研究，取得了許多重要成果[23-25]。從最開(kāi)始的靜態(tài)多模型和動(dòng)態(tài)多模型，一直到變結(jié)構(gòu)多模型，多模型算法經(jīng)歷了三代變遷，其中的交互式多模型算法(InteractingMultipleModel，IMM)與其它多模型算法相比，運(yùn)算效率相對(duì)較高，估計(jì)精度相對(duì)較好，因此受到了廣泛關(guān)注[23-26]。對(duì)于非線性系統(tǒng)，需要使用非線性濾波方法。常用的方法是擴(kuò)展卡爾曼濾波，該方法需要計(jì)算非線性方程的雅可比矩陣，在強(qiáng)非線性系統(tǒng)中具有較高的線性截?cái)嗾`差。文獻(xiàn)[27]在不敏變換的基礎(chǔ)上，提出了基于不敏卡爾曼濾波的IMM算法(InteractingMultipleModelBasedonUnscentedKalmanFilter，IMM_UKF)。該算法可以避免計(jì)算非線性方程的雅可比矩陣，具有較高的濾波精度。針對(duì)非高斯非線性系統(tǒng)，文獻(xiàn)[28]在粒子濾波(ParticleFilter，PF)算法的基礎(chǔ)上，提出了基于PF的IMM算法，取得了更高的濾波精度。此外，文獻(xiàn)[29]在不敏粒子濾波算法的基礎(chǔ)上，給出了基于UPF的IMM算法(InteractingMultipleModelBasedonUnscentedParticleFilter，IMM_UPF)。然而，由于IMM算法本身的時(shí)間復(fù)雜度與模型個(gè)數(shù)成正比，時(shí)間復(fù)雜度很高，故而其實(shí)用性受到限制。近年來(lái)，人們對(duì)求積分卡爾曼濾波(QuadratureKalmanFilter，QKF)算法有較多關(guān)注，圍繞該算法有一系列的研究工作[30-33]。該算法是一種較新型的非線性濾波算法，與UKF算法類似，都是確定點(diǎn)采樣方法，不需要計(jì)算非線性函數(shù)的雅可比矩陣。QKF選取的采樣點(diǎn)可以根據(jù)求積分點(diǎn)個(gè)數(shù)的不同而不同，故而能夠取得比UKF稍好的濾波精度。QKF算法的缺點(diǎn)是：如果求積分點(diǎn)選取過(guò)多，時(shí)間復(fù)雜度就會(huì)增大。本節(jié)將QKF算法應(yīng)用到交互式多模型算法中，提出了一種基于求積分卡爾曼濾波的交互式多模型算法(InteractingMultipleModelBasedonQuadratureKalmanFilter，IMM_QKF)。該算法可以處理非線性多模型濾波問(wèn)題，具有和UKF_IMM算法類似或更高的濾波精度，為交互式多模型算法在機(jī)動(dòng)目標(biāo)跟蹤中的應(yīng)用提供了新的實(shí)現(xiàn)途徑，具有一定的參考價(jià)值。9.3.2求積分卡爾曼濾波

假定非線性系統(tǒng)的動(dòng)態(tài)方程和測(cè)量方程如下

xk=fk(xk－1)+vk-1

(9-33)

zk=hk(xk)+nk

(9-34)

其中，f和h表示狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)，如果兩個(gè)函數(shù)中有非線性函數(shù)，則稱系統(tǒng)為非線性系統(tǒng)。vk和nk分別表示過(guò)程噪聲和量測(cè)噪聲，噪聲序列均假定為零均值高斯分布，方差分別為Qk和Rk，且相互獨(dú)立。如果狀態(tài)向量初值及其協(xié)方差已知，則使用測(cè)量值可以進(jìn)行每一步的狀態(tài)向量估計(jì)。

假定在k－1時(shí)刻的后驗(yàn)概率密度p(xk－1|z1:k－1)已知，根據(jù)狀態(tài)轉(zhuǎn)移方程，狀態(tài)向量的一步預(yù)測(cè)概率密度p(xk|z1:k－1)可以通過(guò)如下積分計(jì)算

(9-35)當(dāng)接收到一個(gè)新的量測(cè)zk時(shí)，使用貝葉斯定理，后驗(yàn)概率密度可以用下式表示(9-36)對(duì)式(9-35)和式(9-36)遞推求解獲得狀態(tài)估計(jì)的方法就是最優(yōu)貝葉斯濾波。然而，非線性濾波系統(tǒng)中，通常情況下難以獲得式(9-35)和式(9-36)的解析形式，只能獲得近似解。求積分卡爾曼濾波方法就是一種求積分的數(shù)值解法，可以用來(lái)求解式(9-35)和式(9-36)。下面介紹求積分?jǐn)?shù)值解法。考慮積分g(x)和另外一個(gè)函數(shù)W(x)可使用下列形式進(jìn)行積分(9-37)其中，函數(shù)W(x)可以看做一個(gè)權(quán)值函數(shù)，其值恒為正?？梢允褂孟旅娴臄?shù)值解法近似該積分，即(9-38)其中，m表示求積分點(diǎn)的個(gè)數(shù)，ξl是第l個(gè)積分點(diǎn)，ωl表示相應(yīng)的權(quán)值。假定J是一個(gè)對(duì)稱的三對(duì)角矩陣，其對(duì)角線元素全為0，并且Ji，i+1=，1≤i≤(m－1)，則求積分點(diǎn)表示為ξl=

εl，此處ε

l是J的第l個(gè)特征值，其相關(guān)權(quán)值計(jì)算為ωl=(vl)21，其中(vl)1是矩陣J正則化后的第l個(gè)特征向量的第一個(gè)元素。假如狀態(tài)向量的維數(shù)大于1，即p(x)=N(x;0，Inx)，其中Inx表示維數(shù)為nx×nx的單位矩陣。因?yàn)闋顟B(tài)向量的各個(gè)分量相互獨(dú)立，故上述求積分公式可以由一維擴(kuò)展為多維。具體過(guò)程為

(9-39)其中，。通常情況下，概率密度函數(shù)

各分量之間相互關(guān)聯(lián)。為了改善此時(shí)的積分結(jié)果，使用解耦技術(shù)。下面介紹解耦過(guò)程。首先，對(duì)下列協(xié)方差矩陣進(jìn)行分解(9-40)然后，實(shí)施線性變換(9-41)則新向量具有單位方差。接著求g(x)的期望(9-42)9.3.3算法描述及步驟

考慮一個(gè)待估計(jì)系統(tǒng)。系統(tǒng)當(dāng)前模型來(lái)自于具有n個(gè)模型的集合，該模型集為M={M1，M2，…，Mn}。假定模型Mj的先驗(yàn)概率μj0=P{Mj0}以及模型Mi和模型Mj之間的轉(zhuǎn)換概率pij=P{Mjk|Mik－1}均已知，其中i=1，…，n，j=1，…n。實(shí)際上，可以將其看做概率轉(zhuǎn)移矩陣已知的一階馬爾可夫鏈，因此，具有上述假設(shè)的系統(tǒng)通常稱為馬爾可夫鏈。最優(yōu)多模型算法要求對(duì)所有可能的模型序列進(jìn)行濾波，即：對(duì)于n個(gè)模型，在k時(shí)刻需要nk個(gè)濾波器來(lái)處理第k個(gè)量測(cè)。最優(yōu)多模型算法的時(shí)間復(fù)雜度太高，實(shí)際應(yīng)用中經(jīng)常使用次優(yōu)多模型算法，交互式多模型算法就是其中使用較為廣泛的一種次優(yōu)算法。基于求積分卡爾曼濾波算法和交互式多模型算法過(guò)程相同，都是依據(jù)一個(gè)有限狀態(tài)離散時(shí)間馬爾可夫鏈進(jìn)行模型切換的，所不同的是，針對(duì)非線性系統(tǒng)的處理上使用了求積分卡爾曼濾波。該算法由三個(gè)階段組成：交互、濾波和組合。首先計(jì)算模型之間的交互概率，并計(jì)算與該模型匹配的混合輸入；接著針對(duì)不同模型使用不同的濾波器，從而獲得n個(gè)當(dāng)前時(shí)刻的狀態(tài)估計(jì)；然后對(duì)這n個(gè)濾波器結(jié)果加權(quán)，從而獲得最終的狀態(tài)向量及協(xié)方差估計(jì)，其中權(quán)值是通過(guò)模型概率和濾波過(guò)程中量測(cè)值相對(duì)各個(gè)模型的似然量計(jì)算獲得的。下面給出每個(gè)階段的計(jì)算步驟。

Step1：交互階段。計(jì)算模型Mi和模型Mj之間的交互概率μi|jk為

(9-43)其中，μik－1是k－1時(shí)刻模型Mi的概率，cj是一個(gè)歸一化因子。每個(gè)濾波器的混合輸入用下式計(jì)算－(9-44)(9-45)式中mik－1和Pik－1分別表示k－1時(shí)刻模型i的狀態(tài)估計(jì)值及其協(xié)方差。

Step2：濾波階段。采用求積分卡爾曼濾波算法獲得每個(gè)非線性模型的狀態(tài)估計(jì)值及其協(xié)方差mik，Pik，i=1，…，n。當(dāng)然，若是線性模型，則使用卡爾曼濾波算法即可。在濾波過(guò)程中還需計(jì)算當(dāng)前時(shí)刻量測(cè)相對(duì)每個(gè)濾波器的似然值，即

Λik=N(vik;0，Sik)

(9-46)

其中vik表示使用模型i濾波過(guò)程中計(jì)算所得的量測(cè)值的殘差，Sik表示該量測(cè)值殘差的協(xié)方差。在時(shí)刻k，每個(gè)模型Mi的概率用下式計(jì)算(9-47)式中c表示歸一化因子。

Step3：組合階段。通過(guò)加權(quán)方法計(jì)算最終狀態(tài)估計(jì)及其協(xié)方差，即(9-48)(9-49)9.3.4仿真實(shí)驗(yàn)及結(jié)果分析

1.實(shí)驗(yàn)?zāi)Ｐ?/p>

針對(duì)二維空間中的單個(gè)機(jī)動(dòng)目標(biāo)進(jìn)行跟蹤濾波。為了捕捉目標(biāo)轉(zhuǎn)彎運(yùn)動(dòng)，通常的建模方式是采用轉(zhuǎn)彎模型[19]。該模型的思路是將目標(biāo)轉(zhuǎn)彎率參數(shù)作為狀態(tài)向量的一部分，與目標(biāo)的位置和速度分量一起參與濾波。該模型狀態(tài)向量表示為

，狀態(tài)轉(zhuǎn)移方程為(9-50)仿真200步，目標(biāo)的運(yùn)動(dòng)過(guò)程如下：①目標(biāo)從原點(diǎn)開(kāi)始以速度(x0，y0)=(1，0)運(yùn)動(dòng)；②在第4秒，目標(biāo)以角速度=1左轉(zhuǎn)；③在第9秒，目標(biāo)停止轉(zhuǎn)彎，并沿直線運(yùn)動(dòng)2秒；④在第11秒，目標(biāo)以角速度=－1開(kāi)始右轉(zhuǎn)；⑤在第16秒，目標(biāo)停止轉(zhuǎn)彎，并沿直線運(yùn)動(dòng)4秒。上述運(yùn)動(dòng)軌跡如圖9.7中目標(biāo)真實(shí)軌跡所示。

假定傳感器直接對(duì)目標(biāo)位置進(jìn)行觀測(cè)，觀測(cè)噪聲假定為零均值加性高斯噪聲，其方差為σ2r=0.05。濾波過(guò)程中，我們使用下面的模型:

(1)單模型卡爾曼濾波方法。具有過(guò)程噪聲方差為q1=0.05的標(biāo)準(zhǔn)維納過(guò)程