變結(jié)構(gòu)交互式多模型濾波和平滑算法

陳維義, 何 凡,*, 劉國(guó)強(qiáng), 毛偉偉

(1. 海軍工程大學(xué)兵器工程學(xué)院, 湖北 武漢 430030; 2. 海軍士官學(xué)校兵器系, 安徽 蚌埠 233000)

0 引 言

目前,卡爾曼濾波、粒子濾波、基于機(jī)器學(xué)習(xí)的濾波及其改進(jìn)形式已被廣泛應(yīng)用在目標(biāo)跟蹤、參數(shù)估計(jì)和狀態(tài)預(yù)測(cè)等領(lǐng)域[1-4]。對(duì)于基于單模型的濾波方法而言,當(dāng)目標(biāo)機(jī)動(dòng)十分復(fù)雜時(shí),會(huì)出現(xiàn)濾波模型與目標(biāo)機(jī)動(dòng)模型不匹配的情況,從而導(dǎo)致濾波精度大幅降低[5]。

針對(duì)這一問題,交互式多模型(interacting multiple model, IMM)[6-8]展現(xiàn)出更加優(yōu)越的性能。近些年,IMM算法得到了很多學(xué)者的關(guān)注,很多學(xué)者在結(jié)構(gòu)和參數(shù)等層面對(duì)IMM算法進(jìn)行了改進(jìn)。文獻(xiàn)[9]基于恒速和當(dāng)前統(tǒng)計(jì)模型設(shè)計(jì)了IMM算法,首先利用最小二乘法估計(jì)當(dāng)前統(tǒng)計(jì)模型的平均速度,然后將當(dāng)前統(tǒng)計(jì)模型應(yīng)用于IMM算法。該方法提高了模型精度,從而提高了濾波精度。文獻(xiàn)[10]從多個(gè)方面對(duì)IMM算法進(jìn)行了改進(jìn),包括采用改進(jìn)的卡爾曼濾波器作為子濾波器、不同模型之間的非對(duì)稱狀態(tài)估計(jì)和基于熵的模型概率更新公式。文獻(xiàn)[7,11]提出了IMM的一種替代方法,模型集中的模型由勻加速模型構(gòu)成,降低了模型集的復(fù)雜程度。在此基礎(chǔ)上,文獻(xiàn)[12]提出了一種自適應(yīng)IMM算法,首先利用濾波器對(duì)目標(biāo)的加速度進(jìn)行估計(jì),然后選取估計(jì)加速度附近的值構(gòu)建模型。該方法可以減少模型集中模型的數(shù)量,在降低計(jì)算量的同時(shí)提高了模型精度。文獻(xiàn)[13]基于二階馬爾可夫鏈提出了一種二階IMM算法,該算法利用了更多的先驗(yàn)信息,提高了濾波精度。由于上述IMM模型集中的模型種類和數(shù)量不變,所以又稱其為固定結(jié)構(gòu)IMM(fixed structure IMM, FSIMM)。

為了避免因模型不匹配而造成的精度誤差問題,在使用IMM算法時(shí),應(yīng)使用盡可能多的模型覆蓋目標(biāo)機(jī)動(dòng)模型。但值得注意的是,單個(gè)模型集中模型過(guò)多,同樣會(huì)降低濾波精度[14]。在此背景下,變結(jié)構(gòu)IMM(variable structure IMM,VSIMM)應(yīng)運(yùn)而生。VSIMM經(jīng)過(guò)不斷的發(fā)展和改進(jìn),大致可以分為4類:模型組切換(model group switching, MGS)、可能模型集(likely mode set, LMS)、期望模型增強(qiáng)(expected mode augmentation, EMA)和自適應(yīng)網(wǎng)格(adaptive grid,AG)[15]。其中,MGS將模型集分為模型子集,一個(gè)時(shí)間步長(zhǎng)只選擇一個(gè)模型子集進(jìn)行估計(jì),模型子集之間根據(jù)模型子集轉(zhuǎn)移概率進(jìn)行切換[16]。LMS將模型分為3種類型:不可能的、重要的和主要的模型,在每個(gè)時(shí)間步長(zhǎng)里,用于估計(jì)的模型子集由主要的模型和接近主要的模型構(gòu)成[17-18]。與MGS類似,EMA將一個(gè)大的模型集分成小的模型子集,然后計(jì)算下一個(gè)時(shí)間步長(zhǎng)所有模型子集的概率,選擇概率最大的模型子集用于估計(jì)[19]。AG算法結(jié)合圖理論,將所有的模型構(gòu)成一個(gè)網(wǎng)格,利用先驗(yàn)信息和當(dāng)前數(shù)據(jù)得到一個(gè)局部細(xì)化網(wǎng)格,細(xì)化網(wǎng)格中的模型構(gòu)成候選模型子集,然后根據(jù)規(guī)則選擇模型,構(gòu)成下一時(shí)刻用于濾波的模型子集[20]。

值得注意的是,文獻(xiàn)[19]所提到的EMA方法在計(jì)算似然函數(shù)和模型子集概率時(shí),對(duì)公式進(jìn)行了近似。針對(duì)這一問題,本文首先設(shè)計(jì)了一種新的VSIMM(novel VSIMM, NVSIMM)濾波算法,給出了精確的數(shù)學(xué)模型。另外,在前向NVSIMM濾波的基礎(chǔ)上,對(duì)數(shù)據(jù)進(jìn)行平滑處理,設(shè)計(jì)了VSIMM平滑(VSIMM smoothing, VSIMMS)算法,前向NVSIMM和后向VSIMMS相結(jié)合即為VSIMM濾波和平滑(VSIMM filtering and smoothing, VSIMMFS)算法。

本文安排如下:第1節(jié)對(duì)濾波和平滑問題進(jìn)行了描述,并給出了IMM濾波算法和IMM平滑算法的數(shù)學(xué)模型;第2節(jié)設(shè)計(jì)了VSIMMFS算法,包括前向NVSIMM濾波和后向VSIMM平滑兩個(gè)部分;第3節(jié)對(duì)本文所提算法的有效性進(jìn)行仿真驗(yàn)證和討論;最后,第4節(jié)給出了本文的結(jié)論。

1 IMM濾波和平滑算法

1.1 問題描述

假設(shè)目標(biāo)可能有r個(gè)運(yùn)動(dòng)模型,模型集記為Ω={M1,M2,…,Mr},模型之間的轉(zhuǎn)換概率矩陣為

(1)

式中:pij(1≤i≤r,1≤j≤r)為模型i到模型j的轉(zhuǎn)移概率。

系統(tǒng)離散化狀態(tài)方程如下:

(2)

模型j的量測(cè)方程為

(3)

1.2 前向IMM濾波

IMM算法框圖如圖1所示。

圖1 IMM算法框圖Fig.1 Block diagram of the IMM algorithm

IMM算法一般可以分為以下4個(gè)步驟[6-7]。

步驟 1前向輸入交互

(4)

(5)

(6)

步驟 2濾波

分別基于每一個(gè)模型進(jìn)行卡爾曼濾波:

步驟 2.1預(yù)測(cè)狀態(tài)

(7)

步驟 2.2預(yù)測(cè)協(xié)方差矩陣

(8)

步驟 2.3計(jì)算卡爾曼增益

(9)

步驟 2.4濾波估計(jì)

(10)

步驟 2.5濾波值的協(xié)方差

(11)

步驟 3模型概率更新

模型概率更新公式為

(12)

(13)

(14)

步驟 4輸出

IMM濾波的狀態(tài)估計(jì)和協(xié)方差估計(jì)分別為

(15)

(16)

1.3 后向IMM平滑

在前向IMM濾波的基礎(chǔ)上,對(duì)數(shù)據(jù)進(jìn)一步進(jìn)行平滑處理,得到后向IMM平滑算法,其算法框圖如圖2所示,包括以下幾個(gè)步驟[21]。

步驟 1后向狀態(tài)交互

后向混合狀態(tài)和對(duì)應(yīng)的協(xié)方差為

(17)

(18)

(19)

(20)

步驟 2卡爾曼平滑[22]

平滑值和對(duì)應(yīng)的協(xié)方差為

(21)

(22)

步驟 3平滑模型概率更新

平滑后的模型概率計(jì)算公式為

(23)

步驟 4IMM平滑狀態(tài)交互

后向IMM平滑的狀態(tài)輸出和對(duì)應(yīng)的協(xié)方差為

(24)

(25)

圖2 IMM平滑算法框圖Fig.2 Block diagram of the IMM smoothing algorithm

2 VSIMMFS算法

VSIMMFS算法包括前向NVSIMM濾波和后向VSIMM平滑兩個(gè)部分,這兩個(gè)部分分別在后續(xù)兩個(gè)小節(jié)中展開。

2.1 VSIMM濾波

為了避免模型集中模型數(shù)量過(guò)多,減少模型誤差,本節(jié)設(shè)計(jì)了NVSIMM算法。NVSIMM濾波算法包括多個(gè)模型子集,每個(gè)模型子集獨(dú)立運(yùn)行IMM,選取概率最高的模型子集的估計(jì)結(jié)果作為最終的估計(jì)狀態(tài)輸出。NVSIMM算法框圖如圖3所示。

圖3 NVSIMM算法框圖Fig.3 Block diagram of NVSIMM algorithm

算法包括以下4個(gè)步驟。

步驟 1并行獨(dú)立IMM狀態(tài)估計(jì)

步驟 2計(jì)算每個(gè)模型子集的似然函數(shù)

模型子集的似然函數(shù)為

(26)

(27)

步驟 3計(jì)算模型子集概率

記模型子集概率p(Πk(m)|Zk)=κk(n),則有:

(28)

式中:p(zk|Zk-1,Πk(n))在步驟2中已經(jīng)求出,分母為歸一化因子,p(Πk(n)|Zk-1)根據(jù)全概率公式有:

(29)

式中:κk-1(m)=p(Πk-1(m)|Zk-1)為上一時(shí)刻的模型子集概率,應(yīng)用馬爾可夫性質(zhì)可知模型子集轉(zhuǎn)移概率與觀測(cè)值無(wú)關(guān),即:

(30)

步驟 4NVSIMM濾波狀態(tài)估計(jì)

選擇概率最大的模型子集的IMM估計(jì)結(jié)果作為最終的狀態(tài)估計(jì)輸出,首先求出概率最高的模型子集編號(hào):

(31)

從而可以得到最終的狀態(tài)估計(jì)和對(duì)應(yīng)的協(xié)方差為

(32)

Pk|k=Pk|k(nm)

(33)

2.2 VSIMMS算法

本節(jié)在NVSIMM的基礎(chǔ)上,對(duì)數(shù)據(jù)進(jìn)行進(jìn)一步平滑,得到VSIMMS算法。VSIMMS算法框圖如圖4所示。

圖4 VSIMMS算法框圖Fig.4 Block diagram of VSIMMS algorithm

算法包括以下4個(gè)步驟。

步驟 1并行獨(dú)立IMM狀態(tài)平滑

步驟 2計(jì)算平滑后的模型子集的似然函數(shù)

應(yīng)用全概率公式和馬爾可夫性質(zhì),可以得到模型子集的似然函數(shù)為

(34)

式中:p(zk|Zk-1,Πk(m))為前向NVSIMM濾波過(guò)程中模型子集的似然函數(shù),應(yīng)用全概率公式和馬爾可夫性質(zhì),p(Πk(m)|Πt(n))可以轉(zhuǎn)化為

(35)

(36)

(37)

步驟 3計(jì)算平滑后的模型子集概率

根據(jù)貝葉斯定理可以得到模型子集概率為

(38)

(39)

步驟 4VSIMMS狀態(tài)估計(jì)

概率最高的模型子集編號(hào)為

(40)

判斷概率最高的模型子集在Tswitch時(shí)間段內(nèi)是否發(fā)生變化。若發(fā)生變化,VSIMMS的狀態(tài)估計(jì)和對(duì)應(yīng)的協(xié)方差為

(41)

Pt|k=Pt|k(Ω)

(42)

若不發(fā)生變化,VSIMMS的狀態(tài)估計(jì)和對(duì)應(yīng)的協(xié)方差為

(43)

(44)

3 仿真與討論

為了驗(yàn)證VSIMMFS算法的有效性,本節(jié)首先針對(duì)三維空間中的目標(biāo)運(yùn)動(dòng)軌跡展開分析討論。目標(biāo)的初始位置、速度和加速度分別為(10 km, 40 km, 30 km)、(300 m/s, 0, 0)和(0, 0, 0)。目標(biāo)的機(jī)動(dòng)參數(shù)如下:

(1) 30 s的勻速直線運(yùn)動(dòng);

(2) 30 s的勻加速運(yùn)動(dòng),加速度大小為(-10 m/s2, -10 m/s2, -10 m/s2);

(3) 30 s的勻速轉(zhuǎn)彎運(yùn)動(dòng),轉(zhuǎn)彎角速率為(0.2 rad/s, 0.2 rad/s, 0);

(4) 30 s的勻速直線運(yùn)動(dòng)。

目標(biāo)運(yùn)動(dòng)軌跡如圖5所示。

圖5 目標(biāo)運(yùn)動(dòng)軌跡Fig.5 Target motion trajectory

利用蒙特卡羅法分析對(duì)比IMM、NVSIMM和VSIMMFS的性能,其中IMM算法包括3個(gè)模型:勻速直線運(yùn)動(dòng)(constant velocity, CV)模型、勻加速運(yùn)動(dòng)(constant acceleration, CA)模型和勻速轉(zhuǎn)彎(constant turn, CT)模型[23-25]。VSIMMFS算法包括兩個(gè)模型子集,第一個(gè)模型子集包括CV、CA,第二個(gè)模型子集包括CV、CT;NVSIMM濾波算法包括的模型子集和VSIMM相同?？梢宰⒁獾?VSIMMFS和NVSIMM僅需兩個(gè)原始模型子集即可包括所有的模型,但這兩種算法的原始模型子集并不包括模型子集CA、CT。設(shè)模型轉(zhuǎn)移概率矩陣為

假設(shè)系統(tǒng)過(guò)程噪聲的標(biāo)準(zhǔn)差為1 m/s2,測(cè)量噪聲的標(biāo)準(zhǔn)差為50 m,Tswitch=20 s,仿真結(jié)果如圖6～圖9所示。圖6給出了IMM算法中模型概率變化曲線,其中,概率最大的曲線所對(duì)應(yīng)的模型與目標(biāo)實(shí)際機(jī)動(dòng)模型相同。圖7給出了NVSIMM和VSIMMFS中模型子集概率曲線,由圖7 可知,平滑后(VSIMMFS)的模型子集概率大于NVSIMM的模型子集概率,說(shuō)明平滑后的模型子集與目標(biāo)機(jī)動(dòng)模型匹配程度更高。另外,對(duì)比圖6和圖7可知,圖6中概率最大的曲線所對(duì)應(yīng)的模型屬于圖7中概率高的模型子集,也就是說(shuō),概率最高的模型子集始終包括目標(biāo)實(shí)際的機(jī)動(dòng)模型。圖8給出了IMM、NVSIMM和VSIMMFS對(duì)目標(biāo)運(yùn)動(dòng)軌跡的跟蹤效果,這3種方法都能夠?qū)δ繕?biāo)軌跡進(jìn)行較好的估計(jì),三者之間的差異具體體現(xiàn)在圖9中。圖9給出了IMM、NVSIMM和VSIMMFS對(duì)目標(biāo)位置估計(jì)的均方根誤差(root mean square error, RMSE)曲線。從圖9中可以看出,VSIMMFS的RMSE最小,VSIMMFS次之,IMM的RMSE最大。值得注意的是,概率最大的模型子集發(fā)生變化時(shí),NVSIMM算法的性能會(huì)短暫下降,而VSIMMFS算法對(duì)位置估計(jì)的RMSE與IMM相同,原因在于在設(shè)計(jì)VSIMMFS算法時(shí),當(dāng)最大概率的模型子集短暫發(fā)生時(shí),利用基于模型全集的IMM估計(jì)值作為VSIMMFS的估計(jì)值,從而避免了VSIMMFS算法短暫的性能下降。

圖6 IMM算法中的模型概率Fig.6 Model probability in IMM algorithm

圖7 NVSIMM和VSIMMFS中模型子集概率Fig.7 Model subset probabilities in NVSIMM and VSIMMFS

圖8 目標(biāo)運(yùn)動(dòng)軌跡跟蹤效果Fig.8 Effect of target trajectory tracking

圖9 位置跟蹤的RMSEFig.9 RMSE of position tracking

為了進(jìn)一步體現(xiàn)VSIMMFS算法的優(yōu)越性,將VSIMMFS與基于深度強(qiáng)化學(xué)習(xí)的目標(biāo)跟蹤算法(maneuvering target tracking based on deep reinforcement learning, MTTDRL)[27]、粒子濾波(particle filter, PF)[28]、基于粒子群優(yōu)化的PF(particle swarm optimization algorithm-based PF, PSO-PF)[29]、基于混沌PSO-PF(chaos PSO-PF, CPSO-PF)[26,30]進(jìn)行對(duì)比。為了保證對(duì)比的有效性,仿真參數(shù)參考文獻(xiàn)[26]和文獻(xiàn)[27]設(shè)計(jì),目標(biāo)的運(yùn)動(dòng)軌跡如圖10所示。圖11分析對(duì)比了上述算法對(duì)目標(biāo)位置的跟蹤性能。由圖11(a)可知,相比于PF、PSO-PF和CPSO-PF,VSIMMFS的位置跟蹤的RMSE較小;從圖11(b)可以看出,利用VSIMMFS對(duì)目標(biāo)跟蹤時(shí)的位置偏差更小。所以綜上可知,VSIMMFS的跟蹤性能優(yōu)于上述其他算法。

圖10 目標(biāo)運(yùn)動(dòng)軌跡Fig.10 Target motion trajectory

圖11 算法性能對(duì)比Fig.11 Performance comparison of algorithms

4 結(jié) 論

為了進(jìn)一步提高對(duì)機(jī)動(dòng)目標(biāo)的跟蹤精度,本文設(shè)計(jì)了一種VSIMMFS算法。該方法包括前向VSIMM濾波和后向VSIMMS兩個(gè)部分,通過(guò)并行獨(dú)立運(yùn)行IMM濾波和IMM平滑,選擇概率最大的模型子集的估計(jì)結(jié)果作為最終的狀態(tài)估計(jì)值。通過(guò)與IMM、NVSIMM、PF、PSO-PF、CPSO-PF和MTTDRL算法比較,VSIMMFS具有更好的跟蹤性能。