基于反饋判決的魯棒自適應機動目標跟蹤算法

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

(1.空軍工程大學信息與導航學院，西安，710077； 2.93184部隊，北京，100076)

機動目標跟蹤系統在導航制導、軍事打擊等領域有著廣泛的應用，然而針對干擾源目標，采用主動式的探測跟蹤易于暴露己方所在位置，被動的純方位無源跟蹤(bearings-only tracking，BOT)不主動發射信號，因而具有良好的隱蔽性[1-4]。

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

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

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

1 機動目標跟蹤系統模型和SRCKF算法

1.1 系統模型

所構建的目標相對運動系統方程可表示成：

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

(1)

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

(2)

(3)

Qk-1=ΓΓT·q

(4)

式中：q為過程噪聲強度；Γ是過程噪聲轉移矩陣，可表示為:

(5)

雙觀測站純方位跟蹤的量測方程為：

(6)

1.2 平方根容積卡爾曼濾波

1.2.1 初始化

初始化狀態向量x0|0，誤差協方差P0|0并對其進行Cholesky分解，獲得誤差協方差的平方根更新初值S0|0,即：

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

(7)

1.2.2 時間更新

1)利用上一時刻Sk-1|k-1計算容積點并進行容積點的傳播：

(8)

(9)

式中：m=2nx。

2)計算狀態一步預測值及預測誤差協方差的平方根：

(10)

(11)

其中：

(12)

(13)

1.2.3 量測更新

1)基于Sk|k-1再次計算容積點并進行容積點的傳播：

(14)

(15)

2)估計預測量測及新息協方差陣的平方根：

(16)

(17)

其中：

(18)

(19)

3)估計互協方差矩陣：

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

(20)

(21)

4)計算濾波增益：

(22)

5)更新狀態估計值：

(23)

6)估計狀態協方差的平方根：

(24)

2 基于反饋判決的魯棒自適應SRCKF算法

2.1 魯棒SRCKF和自適應濾波算法

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

(25)

(26)

下面討論系統模型與實際模型不匹配的情況。SRCKF算法需要較為精確的動態模型，一旦動態模型建模失準，濾波器容易發散。針對這一現象，利用STF的正交性原理，可以得到基于SRCKF的強跟蹤濾波算法，其通過設計漸消因子λk實時調整預測誤差協方差Pk|k-1，使殘差序列保持正交以快速跟蹤系統的真實狀態。強跟蹤濾波算法應使得以下條件成立：

(27)

(28)

將單漸消因子替換為多重漸消因子，以對預測誤差協方差的多個通道分別進行調節，這樣調整后預測誤差協方差平方根的計算式(11)可改寫為：

(29)

多重漸消因子矩陣Λk的計算應滿足：

(30)

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

(31)

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

(32)

(33)

假設Λk=diag[λk,1,λk,2,…,λk,nx]，同時考慮到Hk僅僅與目標跟蹤位置信息相關，多重漸消因子計算方式如下：

(34)

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

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

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

上述無論是魯棒方法還是自適應方法，都是基于殘差進行處理，只能有效解決量測異常和模型失準中的一種問題。為了區分正常數據和異常數據，本文構建基于量測殘差的Mahalanobis距離作為濾波異常的判別因子

(35)

τk反映了真實測量值與預估測量值之間的偏離程度，若εk服從標準正態分布，則有：

τk～χ2(nz)

(36)

式中：nz為量測向量的維數，nz=2。

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

p(τk>δα)=α

(37)

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

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

圖1 FRA-SRCKF流程圖

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

3 仿真與分析

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

RMSEpos(k)=

(38)

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

3.1 場景1：測量存在異常誤差

目標和無人機觀測站的運動軌跡設定為：目標做勻加速直線運動，無人機觀測站航向保持為45°，速度為100 m/s。異常誤差參數設置如表1所示。

表1 異常誤差參數設置

圖2為量測異常時目標運動真實軌跡、雙觀測站軌跡以及各算法的濾波估計軌跡圖。如圖3所示，量測異常存在時τk超出閾值，FRA-SRCKF比SRCKF具有更好的調節能力。

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

圖2 目標、觀測站及各算法濾波軌跡

圖3 量測異常時濾波判別因子τk

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

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

3.2 場景2：測量異常和系統模型失準同時存在

目標和無人機觀測站的運動軌跡設定為：目標在0～30 s做勻加速直線運動；在30～80 s進行轉彎運動，轉彎速率Ω=π/10 rad/s，運動方程詳細參數可參照文獻[19]，此時間段濾波模型仍采用勻加速直線運動模型，同時在第50 s、60 s、70 s出現量測噪聲異常值10σ；在80～100 s恢復勻加速直線運動。無人機觀測站運動方式同場景1。為了避免算法受到初始狀態估計帶來的誤差的影響，設置目標跟蹤的初始狀態值與初始真實狀態接近。

圖6為目標運動軌跡、各算法的濾波估計軌跡圖(此處忽略雙觀測站的運動軌跡)。從圖中可以看出，FRA-SRCKF和RMCKF在雙重濾波誤差存在的前提下相較于其他算法具有較好的濾波特性。

圖6 目標運動軌跡及各算法濾波軌跡

圖7記錄了場景2中SRCKF以及FRA-SRCKF的濾波異常判別因子τk。可以看出無論是量測異常還是模型失準，基本濾波框架SRCKF中的τk均會超出閾值δα，這表明濾波誤差產生后，單獨依賴馬氏距離作為后續處理方式的根據不具有合理性；FRA-SRCKF由于存在反饋判決可以使τk很快調節到閾值范圍以內，能夠在抑制2種濾波誤差的過程中均發揮作用。

圖7 測量異常和模型失準存在時τk

圖8和圖9分別展示了在量測異常和模型失準同時發生情況下各算法的RMSEpos和RMSEvel。從圖中可以看出FRA-SRCKF表現出較好的魯棒以及自適應特性，能夠有效抑制量測異常值引起的離散大誤差以及模型失準帶來的連續誤差。盡管RMCKF整體上也具有較好的性能，但在異常量測處會產生大波動，并且由于量測噪聲協方差被錯誤地修正導致其在跟蹤起始階段就存在較大誤差，而這些缺點使得該算法在實際運用中可能無法有效定位跟蹤。在目標進行轉彎運動以及恢復直線運動過程中，UKF、SRCKF、MR-CKF、RSRCKF受模型失準和量測異常的雙重影響，估計精度均明顯降低，其中UKF、SRCKF的濾波性能相似，而具有魯棒特性的MR-CKF、RSRCKF比前者性能要差，這是因為其估計過程并不總是選擇l2范數進行代價函數的最小化，正常測量的權重可能會以不恰當的方式被削弱。

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

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

表2顯示了2個場景中各算法的相對運行時間，以UKF在MATLAB中CPU計算時間為基準單位。

表2 各算法的相對運行時間

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

4 結語

針對機動目標非線性跟蹤系統中，量測異?；蛳到y建模失準的出現使得估計精度降低的問題，本文提出了一種基于反饋判決的魯棒自適應SRCKF算法。該算法利用卡方檢驗判斷濾波正常與否，在濾波異常時分解為魯棒濾波器和自適應濾波器，通過下一濾波周期判別因子的大小確定當前濾波器的最優選擇，屬于反饋式的后驗判決，最終實現以恰當的算法進行濾波處理。仿真實驗表明，所提算法在目標跟蹤系統中可以有效對抗觀測異常和建模失準等問題，增強了濾波的魯棒性和自適應性能，具有一定的工程應用價值。