基于反饋判決的魯棒自適應(yīng)機(jī)動(dòng)目標(biāo)跟蹤算法

汪家寶，陳樹新，吳 昊，何仁珂，郝思沖

(1.空軍工程大學(xué)信息與導(dǎo)航學(xué)院，西安，710077； 2.93184部隊(duì)，北京，100076)

機(jī)動(dòng)目標(biāo)跟蹤系統(tǒng)在導(dǎo)航制導(dǎo)、軍事打擊等領(lǐng)域有著廣泛的應(yīng)用，然而針對(duì)干擾源目標(biāo)，采用主動(dòng)式的探測(cè)跟蹤易于暴露己方所在位置，被動(dòng)的純方位無(wú)源跟蹤(bearings-only tracking，BOT)不主動(dòng)發(fā)射信號(hào)，因而具有良好的隱蔽性[1-4]。

純方位跟蹤需解決的問題本質(zhì)上是一種基于信息非完整獲取條件下的非線性估計(jì)問題。為此，學(xué)者們相繼提出擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)[5]、無(wú)跡卡爾曼濾波(unscented Kalman filter，UKF)[6-7]、容積卡爾曼濾波(cubature Kalman filter，CKF)[8-9]等非線性濾波算法。EKF將非線性方程線性化進(jìn)行一階近似，但在系統(tǒng)非線性程度較高時(shí)會(huì)引起較大的截?cái)嗾`差。UKF和CKF都是通過確定性采樣策略來(lái)近似狀態(tài)的后驗(yàn)概率密度，相比而言，CKF和UKF均能達(dá)到三階估計(jì)精度，但CKF所用的采樣點(diǎn)比UKF少,且其數(shù)值穩(wěn)定性優(yōu)于UKF[10]。文獻(xiàn)[11]提出了平方根容積卡爾曼濾波算法(square root CKF，SRCKF)，通過傳遞狀態(tài)協(xié)方差的平方根，提高了濾波數(shù)值穩(wěn)定性。上述算法只有構(gòu)建的系統(tǒng)模型與實(shí)際情況較為匹配，且噪聲統(tǒng)計(jì)特性服從高斯分布時(shí)，才會(huì)取得較好的濾波特性，而在實(shí)際情況中不能完全滿足要求。

針對(duì)系統(tǒng)建模失準(zhǔn)問題，周東華等人[12]將單漸消因子引入卡爾曼濾波算法，形成強(qiáng)跟蹤濾波器(strong tracking filter，STF)，充分利用了殘差序列的有效信息，對(duì)模型建構(gòu)失準(zhǔn)、過程參數(shù)變動(dòng)具有較強(qiáng)的自適應(yīng)性。文獻(xiàn)[13]利用多重漸消因子對(duì)估計(jì)均方誤差各通道分別加權(quán)修正，相比單漸消因子更能提高漸消濾波的自適應(yīng)調(diào)節(jié)能力。在實(shí)際系統(tǒng)中，傳感器獲得的觀測(cè)信息由于存在外部干擾或者儀器本身故障可能會(huì)出現(xiàn)野值，從而導(dǎo)致濾波發(fā)散。Huber函數(shù)通過引入權(quán)函數(shù)，降低了受野值干擾的觀測(cè)值的權(quán)重，有效地抑制野值對(duì)濾波造成的影響[14]。文獻(xiàn)[15]提出了基于廣義M估計(jì)的魯棒CKF算法MR-CKF，引入馬氏距離作為異常誤差的判別量，利用三段等價(jià)權(quán)函數(shù)合理地處理大小異常誤差，提高了CKF的魯棒性。上述的研究?jī)H針對(duì)一種異常情形，為同時(shí)處理非線性系統(tǒng)的模型失準(zhǔn)和觀測(cè)異常問題，需兼顧抗差能力和自適應(yīng)性，但這兩種需求往往存在矛盾，不能同時(shí)滿足[16]。文獻(xiàn)[17]提出了一種魯棒多漸消因子CKF(robust multiple fading factors CKF，RMCKF)，利用Huber函數(shù)構(gòu)造修正的測(cè)量噪聲協(xié)方差，并以此為前提計(jì)算多漸消因子抑制濾波異常，RMCKF可以減小動(dòng)力學(xué)模型誤差和測(cè)量模型誤差的影響。

為了更好地解決系統(tǒng)出現(xiàn)的模型失準(zhǔn)和觀測(cè)異常問題，本文提出了一種基于反饋判決的魯棒自適應(yīng)SRCKF算法(feedback decision based robust adaptive SRCKF，F(xiàn)RA-SRCKF)，在每一次迭代過程中，以基于測(cè)量殘差的馬氏距離作為異常誤差的判別因子，一旦有濾波異常存在，濾波器拆分為魯棒濾波器和自適應(yīng)濾波器，分別作用至下一時(shí)間步生成判別因子后進(jìn)行二次判斷，從而確定上一濾波周期所需的濾波器類型，達(dá)到反饋判決的效果。

1 機(jī)動(dòng)目標(biāo)跟蹤系統(tǒng)模型和SRCKF算法

1.1 系統(tǒng)模型

所構(gòu)建的目標(biāo)相對(duì)運(yùn)動(dòng)系統(tǒng)方程可表示成：

xk=Fxk-1-uk-1+wk-1

(1)

式中：xk∈nx，nx為狀態(tài)向量維數(shù)；過程噪聲wk-1是滿足均值為零、協(xié)方差為Qk-1的高斯白噪聲。轉(zhuǎn)移矩陣F、確定性輸入uk-1以及Qk-1的表達(dá)式分別為:

(2)

(3)

Qk-1=ΓΓT·q

(4)

式中：q為過程噪聲強(qiáng)度；Γ是過程噪聲轉(zhuǎn)移矩陣，可表示為:

(5)

雙觀測(cè)站純方位跟蹤的量測(cè)方程為：

(6)

1.2 平方根容積卡爾曼濾波

1.2.1 初始化

初始化狀態(tài)向量x0|0，誤差協(xié)方差P0|0并對(duì)其進(jìn)行Cholesky分解，獲得誤差協(xié)方差的平方根更新初值S0|0,即：

S0∣0=[chol(P0|0)]T

(7)

1.2.2 時(shí)間更新

1)利用上一時(shí)刻Sk-1|k-1計(jì)算容積點(diǎn)并進(jìn)行容積點(diǎn)的傳播：

(8)

(9)

式中：m=2nx。

2)計(jì)算狀態(tài)一步預(yù)測(cè)值及預(yù)測(cè)誤差協(xié)方差的平方根：

(10)

(11)

其中：

(12)

(13)

1.2.3 量測(cè)更新

1)基于Sk|k-1再次計(jì)算容積點(diǎn)并進(jìn)行容積點(diǎn)的傳播：

(14)

(15)

2)估計(jì)預(yù)測(cè)量測(cè)及新息協(xié)方差陣的平方根：

(16)

(17)

其中：

(18)

(19)

3)估計(jì)互協(xié)方差矩陣：

Pxz,k|k-1=γ′k|k-1(Z′k|k-1)T

(20)

(21)

4)計(jì)算濾波增益：

(22)

5)更新狀態(tài)估計(jì)值：

(23)

6)估計(jì)狀態(tài)協(xié)方差的平方根：

(24)

2 基于反饋判決的魯棒自適應(yīng)SRCKF算法

2.1 魯棒SRCKF和自適應(yīng)濾波算法

首先討論測(cè)量噪聲存在異常的情況。觀測(cè)值受異常分布噪聲影響或出現(xiàn)野值時(shí)，以上算法性能下降，即魯棒性不強(qiáng)。為此，可引入M估計(jì)方法，利用Huber代價(jià)函數(shù)，針對(duì)大殘差提供l1范數(shù)來(lái)約束殘差信息的權(quán)重。Huber等價(jià)權(quán)函數(shù)可表示為[15]：

(25)

(26)

下面討論系統(tǒng)模型與實(shí)際模型不匹配的情況。SRCKF算法需要較為精確的動(dòng)態(tài)模型，一旦動(dòng)態(tài)模型建模失準(zhǔn)，濾波器容易發(fā)散。針對(duì)這一現(xiàn)象，利用STF的正交性原理，可以得到基于SRCKF的強(qiáng)跟蹤濾波算法，其通過設(shè)計(jì)漸消因子λk實(shí)時(shí)調(diào)整預(yù)測(cè)誤差協(xié)方差Pk|k-1，使殘差序列保持正交以快速跟蹤系統(tǒng)的真實(shí)狀態(tài)。強(qiáng)跟蹤濾波算法應(yīng)使得以下條件成立：

(27)

(28)

將單漸消因子替換為多重漸消因子，以對(duì)預(yù)測(cè)誤差協(xié)方差的多個(gè)通道分別進(jìn)行調(diào)節(jié)，這樣調(diào)整后預(yù)測(cè)誤差協(xié)方差平方根的計(jì)算式(11)可改寫為：

(29)

多重漸消因子矩陣Λk的計(jì)算應(yīng)滿足：

(30)

式中：β為弱化因子，其目的是為了防止造成過調(diào)節(jié)，一般取β≥1。Vk為殘差序列協(xié)方差的估計(jì)值，其計(jì)算方法為：

(31)

式中：ρ為ek的遺忘因子，0<ρ≤1，一般取ρ=0.95。

(32)

(33)

假設(shè)Λk=diag[λk,1,λk,2,…,λk,nx]，同時(shí)考慮到Hk僅僅與目標(biāo)跟蹤位置信息相關(guān)，多重漸消因子計(jì)算方式如下：

(34)

式中：tr(·)表示矩陣(·)的求跡運(yùn)算。

以上基于SRCKF的強(qiáng)跟蹤濾波算法稱為自適應(yīng)SRCKF算法(adaptive SRCKF，ASRCKF)。

2.2 濾波異常判別與反饋判決

上述無(wú)論是魯棒方法還是自適應(yīng)方法，都是基于殘差進(jìn)行處理，只能有效解決量測(cè)異常和模型失準(zhǔn)中的一種問題。為了區(qū)分正常數(shù)據(jù)和異常數(shù)據(jù)，本文構(gòu)建基于量測(cè)殘差的Mahalanobis距離作為濾波異常的判別因子

(35)

τk反映了真實(shí)測(cè)量值與預(yù)估測(cè)量值之間的偏離程度，若εk服從標(biāo)準(zhǔn)正態(tài)分布，則有：

τk～χ2(nz)

(36)

式中：nz為量測(cè)向量的維數(shù)，nz=2。

選擇顯著水平α=0.05，定義χ2(2)的上分位點(diǎn)為δα，于是有：

p(τk>δα)=α

(37)

表示在正常濾波情況下，τk大于δα的概率只有5%。此時(shí)，δα就可以作為閾值對(duì)每次計(jì)算的判別因子進(jìn)行濾波是否正常的檢驗(yàn)。

研究表明，單獨(dú)使用馬氏距離作為判別因子不足以反映濾波異常的來(lái)源，模型失準(zhǔn)和量測(cè)異常都可能使τk超出閾值，后續(xù)仿真實(shí)驗(yàn)會(huì)對(duì)此進(jìn)一步驗(yàn)證。由于2種誤差的解決方案具有矛盾性(前者強(qiáng)化觀測(cè)信息，后者弱化觀測(cè)信息)，因此不能同時(shí)使用。本文的解決思路是當(dāng)濾波出現(xiàn)異常時(shí)，濾波器分解為魯棒濾波器和自適應(yīng)濾波器，分別作用至下一時(shí)刻生成判別因子，然后比較兩個(gè)判別因子的大小，從而確定上一濾波周期所需的濾波器類型，此方法能夠選擇合適的算法對(duì)濾波異常進(jìn)行處理。FRA-SRCKF的流程見圖1，其中N為總采樣次數(shù)。

圖1 FRA-SRCKF流程圖

如圖1所示，若在k時(shí)刻，τk不大于閾值，則繼續(xù)按照SRCKF進(jìn)行濾波處理；否則，SRCKF分解為RSRCKF和ASRCKF兩種子濾波器。然后各自按照對(duì)應(yīng)的濾波步驟并行更新(分別繼續(xù)k時(shí)刻的量測(cè)更新以及k+1時(shí)刻的時(shí)間更新)，進(jìn)而能夠在k+1時(shí)刻，獲得子濾波器的濾波異常判別因子τk+1,RSRCKF和τk+1,ASRCKF。若τk+1,RSRCKF≤τk+1,ASRCKF，則在k時(shí)刻選取RSRCKF濾波獲得的狀態(tài)估計(jì)值與誤差協(xié)方差矩陣，并令τk+1=τk+1,RSRCKF，進(jìn)行k+1時(shí)刻的異常判別，否則，在k時(shí)刻選取ASRCKF的濾波結(jié)果，相應(yīng)地令τk+1=τk+1,ASRCKF。該做法的依據(jù)是k時(shí)刻選擇合適的濾波算法可以使k+1時(shí)刻的預(yù)測(cè)量測(cè)更接近真實(shí)的量測(cè)值，因此獲得的異常判別因子更小。以上算法稱為基于反饋判決的魯棒自適應(yīng)SRCKF算法(feedback decision based robust adaptive SRCKF，F(xiàn)RA-SRCKF)。

3 仿真與分析

系統(tǒng)模型構(gòu)建及相關(guān)參數(shù)定義見式(1)～(6)。在二維笛卡爾坐標(biāo)系中，選取目標(biāo)的初始位置為(2 000 m,4 000 m)，初始速度為(6 m/s,-10 m/s)，初始加速度為(0.2 m/s2,-0.3 m/s2)，目標(biāo)初始狀態(tài)估計(jì)x0|0為[2 400,1 500,10,15,1,1]T。初始狀態(tài)協(xié)方差矩陣P0|0為diag([100,100,1,1,0.01,0.01])。假定系統(tǒng)的過程噪聲強(qiáng)度為q=10-2m/s2，離散采樣間隔Δt=1 s，測(cè)量時(shí)間為100 s，即N=100。在仿真實(shí)驗(yàn)中，雙觀測(cè)站的初始位置設(shè)為s1=[0,0]，s2=[2 000 m,0]。方位角的測(cè)量噪聲標(biāo)準(zhǔn)差均為1°。定義位置均方根誤差RMSEpos為：

RMSEpos(k)=

(38)

本文選取文獻(xiàn)[7]中的UKF、文獻(xiàn)[15]中的MR-CKF、前文所述的SRCKF和RSRCKF、文獻(xiàn)[17]中的RMCKF以及本文提出的FRA-SRCKF算法進(jìn)行對(duì)比實(shí)驗(yàn)，所有濾波方法的初始條件均相同。FRA-SRCKF算法判別因子閾值δα=5.991，弱化因子β=2。MR-CKF算法權(quán)函數(shù)的閾值選擇χα1=5.991，χα2=10 000。

3.1 場(chǎng)景1：測(cè)量存在異常誤差

目標(biāo)和無(wú)人機(jī)觀測(cè)站的運(yùn)動(dòng)軌跡設(shè)定為：目標(biāo)做勻加速直線運(yùn)動(dòng)，無(wú)人機(jī)觀測(cè)站航向保持為45°，速度為100 m/s。異常誤差參數(shù)設(shè)置如表1所示。

表1 異常誤差參數(shù)設(shè)置

圖2為量測(cè)異常時(shí)目標(biāo)運(yùn)動(dòng)真實(shí)軌跡、雙觀測(cè)站軌跡以及各算法的濾波估計(jì)軌跡圖。如圖3所示，量測(cè)異常存在時(shí)τk超出閾值，F(xiàn)RA-SRCKF比SRCKF具有更好的調(diào)節(jié)能力。

圖4和圖5分別給出了量測(cè)異常時(shí)各算法的RMSEpos和RMSEvel。從這兩幅圖可以發(fā)現(xiàn)，UKF、SRCKF、RMCKF在異常量測(cè)點(diǎn)處受到較大的影響，出現(xiàn)大幅度波動(dòng)，雖然具有一定的抑制擾動(dòng)的能力，但UKF、SRCKF的調(diào)節(jié)能力有限，其恢復(fù)到正常狀態(tài)的時(shí)間較長(zhǎng)，并且誤差隨異常值增大而明顯增大，在70 s呈發(fā)散趨勢(shì)，而RMCKF能夠快速抑制量測(cè)異常，由于其對(duì)量測(cè)噪聲協(xié)方差的修正，故對(duì)異常值的大小不敏感。值得注意的一點(diǎn)是，盡管MR-CKF、RSRCKF和FRA-SRCKF跟蹤效果在面對(duì)量測(cè)異常時(shí)得到良好的改善，其均方誤差表現(xiàn)并非相同，這是由于MR-CKF、RSRCKF缺乏對(duì)狀態(tài)預(yù)測(cè)協(xié)方差的自適應(yīng)調(diào)整，因而未能快速收斂到穩(wěn)定值，表現(xiàn)為存在量測(cè)異常時(shí)誤差仍較大。而FRA-SRCKF在濾波跟蹤初期通過反饋判決選擇自適應(yīng)濾波器，能夠強(qiáng)化觀測(cè)量的作用，具有強(qiáng)跟蹤能力，在離散量測(cè)誤差產(chǎn)生時(shí)選取魯棒濾波器，能夠有效地抑制異常量測(cè)對(duì)跟蹤的影響。

圖2 目標(biāo)、觀測(cè)站及各算法濾波軌跡

圖3 量測(cè)異常時(shí)濾波判別因子τk

圖4 各算法的RMSEpos比較(場(chǎng)景1)

圖5 各算法的RMSEvel比較(場(chǎng)景1)

3.2 場(chǎng)景2：測(cè)量異常和系統(tǒng)模型失準(zhǔn)同時(shí)存在

目標(biāo)和無(wú)人機(jī)觀測(cè)站的運(yùn)動(dòng)軌跡設(shè)定為：目標(biāo)在0～30 s做勻加速直線運(yùn)動(dòng)；在30～80 s進(jìn)行轉(zhuǎn)彎運(yùn)動(dòng)，轉(zhuǎn)彎速率Ω=π/10 rad/s，運(yùn)動(dòng)方程詳細(xì)參數(shù)可參照文獻(xiàn)[19]，此時(shí)間段濾波模型仍采用勻加速直線運(yùn)動(dòng)模型，同時(shí)在第50 s、60 s、70 s出現(xiàn)量測(cè)噪聲異常值10σ；在80～100 s恢復(fù)勻加速直線運(yùn)動(dòng)。無(wú)人機(jī)觀測(cè)站運(yùn)動(dòng)方式同場(chǎng)景1。為了避免算法受到初始狀態(tài)估計(jì)帶來(lái)的誤差的影響，設(shè)置目標(biāo)跟蹤的初始狀態(tài)值與初始真實(shí)狀態(tài)接近。

圖6為目標(biāo)運(yùn)動(dòng)軌跡、各算法的濾波估計(jì)軌跡圖(此處忽略雙觀測(cè)站的運(yùn)動(dòng)軌跡)。從圖中可以看出，F(xiàn)RA-SRCKF和RMCKF在雙重濾波誤差存在的前提下相較于其他算法具有較好的濾波特性。

圖6 目標(biāo)運(yùn)動(dòng)軌跡及各算法濾波軌跡

圖7記錄了場(chǎng)景2中SRCKF以及FRA-SRCKF的濾波異常判別因子τk?？梢钥闯鰺o(wú)論是量測(cè)異常還是模型失準(zhǔn)，基本濾波框架SRCKF中的τk均會(huì)超出閾值δα，這表明濾波誤差產(chǎn)生后，單獨(dú)依賴馬氏距離作為后續(xù)處理方式的根據(jù)不具有合理性；FRA-SRCKF由于存在反饋判決可以使τk很快調(diào)節(jié)到閾值范圍以內(nèi)，能夠在抑制2種濾波誤差的過程中均發(fā)揮作用。

圖7 測(cè)量異常和模型失準(zhǔn)存在時(shí)τk

圖8和圖9分別展示了在量測(cè)異常和模型失準(zhǔn)同時(shí)發(fā)生情況下各算法的RMSEpos和RMSEvel。從圖中可以看出FRA-SRCKF表現(xiàn)出較好的魯棒以及自適應(yīng)特性，能夠有效抑制量測(cè)異常值引起的離散大誤差以及模型失準(zhǔn)帶來(lái)的連續(xù)誤差。盡管RMCKF整體上也具有較好的性能，但在異常量測(cè)處會(huì)產(chǎn)生大波動(dòng)，并且由于量測(cè)噪聲協(xié)方差被錯(cuò)誤地修正導(dǎo)致其在跟蹤起始階段就存在較大誤差，而這些缺點(diǎn)使得該算法在實(shí)際運(yùn)用中可能無(wú)法有效定位跟蹤。在目標(biāo)進(jìn)行轉(zhuǎn)彎運(yùn)動(dòng)以及恢復(fù)直線運(yùn)動(dòng)過程中，UKF、SRCKF、MR-CKF、RSRCKF受模型失準(zhǔn)和量測(cè)異常的雙重影響，估計(jì)精度均明顯降低，其中UKF、SRCKF的濾波性能相似，而具有魯棒特性的MR-CKF、RSRCKF比前者性能要差，這是因?yàn)槠涔烙?jì)過程并不總是選擇l2范數(shù)進(jìn)行代價(jià)函數(shù)的最小化，正常測(cè)量的權(quán)重可能會(huì)以不恰當(dāng)?shù)姆绞奖幌魅酢?/p>

圖8 各算法的RMSEpos比較(場(chǎng)景2)

圖9 各算法的RMSEvel比較(場(chǎng)景2)

表2顯示了2個(gè)場(chǎng)景中各算法的相對(duì)運(yùn)行時(shí)間，以UKF在MATLAB中CPU計(jì)算時(shí)間為基準(zhǔn)單位。

表2 各算法的相對(duì)運(yùn)行時(shí)間

從計(jì)算復(fù)雜度來(lái)看，每周期UKF采樣點(diǎn)個(gè)數(shù)比SRCKF多1，故而近似狀態(tài)后驗(yàn)概率密度的運(yùn)算量大；而基于CKF的魯棒算法MR-CKF和RSRCKF僅在重構(gòu)噪聲協(xié)方差的方式上有所不同，因此用時(shí)大致相同；FRA-SRCKF在濾波誤差產(chǎn)生時(shí)分解為2個(gè)濾波器并行更新，運(yùn)算量有所增加。與FRA-SRCKF僅在濾波異常的前提下進(jìn)行調(diào)節(jié)不同，RMCKF無(wú)論濾波正常與否均會(huì)進(jìn)行魯棒處理，又涉及到多重漸消因子的計(jì)算，因而時(shí)間代價(jià)最大?？梢园l(fā)現(xiàn)，F(xiàn)RA-SRCKF在運(yùn)算量適中的情況下可實(shí)現(xiàn)更好的跟蹤效果。

4 結(jié)語(yǔ)

針對(duì)機(jī)動(dòng)目標(biāo)非線性跟蹤系統(tǒng)中，量測(cè)異?；蛳到y(tǒng)建模失準(zhǔn)的出現(xiàn)使得估計(jì)精度降低的問題，本文提出了一種基于反饋判決的魯棒自適應(yīng)SRCKF算法。該算法利用卡方檢驗(yàn)判斷濾波正常與否，在濾波異常時(shí)分解為魯棒濾波器和自適應(yīng)濾波器，通過下一濾波周期判別因子的大小確定當(dāng)前濾波器的最優(yōu)選擇，屬于反饋式的后驗(yàn)判決，最終實(shí)現(xiàn)以恰當(dāng)?shù)乃惴ㄟM(jìn)行濾波處理。仿真實(shí)驗(yàn)表明，所提算法在目標(biāo)跟蹤系統(tǒng)中可以有效對(duì)抗觀測(cè)異常和建模失準(zhǔn)等問題，增強(qiáng)了濾波的魯棒性和自適應(yīng)性能，具有一定的工程應(yīng)用價(jià)值。