彭美康 郭蘊華 汪敬東牟軍敏胡 義

(1.武漢理工大學(xué)高性能艦船技術(shù)教育部重點實驗室,湖北武漢 430063;2.武漢理工大學(xué)能源與動力工程學(xué)院,湖北武漢 430063;3.武漢理工大學(xué)航運學(xué)院,湖北武漢 430063)

1 引言

目標跟蹤是環(huán)境感知系統(tǒng)的核心問題之一,在無人機、動力定位、軍事打擊等領(lǐng)域有著廣泛的應(yīng)用[1-5].為了實現(xiàn)對非線性系統(tǒng)的狀態(tài)估計,研究人員陸續(xù)提出了拓展卡爾曼濾波算法(extended Kalman filter,EKF)[6]、無跡卡爾曼濾波算法(unscented Kalman filter,UKF)[7]、容積卡爾曼濾波算法(cubature Kalman filter,CKF)[8]等非線性濾波算法.這些算法在觀測噪聲嚴格服從高斯分布的情況下具有良好的性能,然而,當觀測噪聲含有污染分布時,其性能將嚴重退化.

文獻[9]指出,偏離觀測樣本主體太遠的觀測值可以看作由不同機制產(chǎn)生的異常值,即野值.在實際工程中,由于設(shè)備本身故障、外界干擾等因素,目標跟蹤系統(tǒng)的觀測值中可能會出現(xiàn)大量的野值.針對這個問題,Huber提出了經(jīng)過嚴格推導(dǎo)的廣義最大似然估計(maximum likelihood type estimates,MLE),即M估計[10].該方法通過Huber權(quán)函數(shù)修正二次型性能指標[11],降低了受野值干擾的觀測值的權(quán)重,是一種解決野值的有效方法,得到了廣泛的研究和應(yīng)用.Wang等將Huber方法與UKF結(jié)合起來,成功應(yīng)用于視覺相對導(dǎo)航領(lǐng)域[12].Zhu等在基于Huber方法的魯棒UKF算法的基礎(chǔ)上,通過投影統(tǒng)計算法來自適應(yīng)選取Huber權(quán)函數(shù)的閾值[13].Karlgaard等提出將Huber方法應(yīng)用于分開差分濾波算法(divided difference filter,DDF)中[14],通過批回歸處理的方式實現(xiàn)非線性濾波的魯棒化.在Karlgaard的基礎(chǔ)上,Chang等構(gòu)建了新的Huber魯棒濾波框架,無需對非線性方程進行線性化處理,提高了濾波算法的估計精度[15].Tseng等在不對非線性模型線性化的基礎(chǔ)上,將基于Huber M估計的CKF濾波算法成功應(yīng)用于GPS導(dǎo)航系統(tǒng)[16].在高階CKF[17-18]的基礎(chǔ)上,張文杰等提出一種基于Huber方法的高階容積卡爾曼跟蹤算法,實現(xiàn)了高階CKF的魯棒化[19].秦康等針對廣義高階容積卡爾曼濾波算法,采用Huber方法提高了非高斯分布條件下算法的魯棒性[20].吳昊等基于廣義M估計,建立改進的3段Huber權(quán)函數(shù),能夠有效處理觀測值中的野值,提高了CKF算法的魯棒性[21].

然而,上述魯棒化濾波算法可能存在一些問題:1)在多維系統(tǒng)中,基于Huber方法的魯棒濾波器存在數(shù)值穩(wěn)定性較差的問題;2)當觀測噪聲的污染率較高時,濾波器的性能會嚴重退化;3)當觀測噪聲的污染率較高時,含有舍棄段的3段Huber權(quán)函數(shù)的閾值選取較為困難.針對以上問題,本文提出一種基于魯棒M估計的自適應(yīng)CKF算法(robust M-estimates based adaptive CKF,RMA-CKF),在觀測噪聲污染率較高的情況下能夠自適應(yīng)地抑制野值的影響,具有較強的抗差能力.

2 目標跟蹤系統(tǒng)模型和CKF算法

2.1 目標跟蹤系統(tǒng)模型

假定目標做勻速直線運動,其在k時刻的狀態(tài)向量為,包括位置向量[xk yk zk]T和速度向量觀測向量zk=[lk ak bk]T,其中l(wèi)k,ak和bk分別為距離測量值、方位角測量值以及高低角測量值.狀態(tài)空間形式的離散非線性目標跟蹤系統(tǒng)可以表示為

式中:xk ∈Rn,zk ∈Rm,n和m分別為各自的維數(shù),這里n=6,m=3;F和Γ分別為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣和過程噪聲轉(zhuǎn)移矩陣;h(·)為非線性觀測函數(shù);矩陣F,Γ以及觀測函數(shù)h(xk)分別表示為

過程噪聲wk和觀測噪聲vk為互不相關(guān)的高斯白噪聲,其均值和協(xié)方差矩陣分別為

δkj為Kronecker-delta函數(shù).

采用混合高斯分布來表示含有污染分布時觀測噪聲的概率分布

式中:N(0,)和N(0,)分別為觀測噪聲的主體分布和干擾分布;α表示干擾部分的數(shù)據(jù)占所有數(shù)據(jù)的比例,也就是野值噪聲出現(xiàn)的概率,即噪聲污染率;σ1和σ2分別為主體分布和干擾分布的均方差,且有?

2.2 CKF算法

CKF算法利用球面徑向容積準則來近似高維積分,主要包括時間更新和量測更新兩個部分.其濾波過程如下.

時間更新:

其中:In為單位矩陣;[·]j表示矩陣[·]的第j列,j=1,2,···,2n.取Sigma采樣點為

對采樣點進行外推預(yù)測,有

量測更新:

濾波增益、狀態(tài)估計以及協(xié)方差矩陣可由下式求得:

2.3 Huber-CKF算法

當觀測噪聲服從正態(tài)分布時,CKF算法具有良好的性能.然而,CKF算法不具魯棒性,當觀測值中出現(xiàn)野值的概率高于0.002時,包括CKF算法在內(nèi)的非魯棒估計算法的性能將嚴重退化[10].Huber-CKF 算法通過引入Huber等價權(quán)函數(shù),降低受野值干擾的觀測值的權(quán)重,能夠有效{地抑制野值的影響.

3 基于魯棒M估計的自適應(yīng)CKF

3.1 算法推導(dǎo)

本文利用文獻[22]提出的光滑近似函數(shù)fμ=(μ為一個很小的正實數(shù)),設(shè)計了一個修正因子,能夠?qū)τ^測值的所有維度進行綜合評判,以自適應(yīng)地抑制野值的影響.

構(gòu)建非線性回歸模型

其中

定義損失函數(shù)為

式中:ξk=yk?g(xk),為標準化殘差.函數(shù)ρ(ξk)的表達式為

式(28)中的β需要合理選擇,以達到所需的估計效率.在文獻[23]中,β的建議取值為1.345.

定義Φk=則式(28)可以表示為

因此,非線性回歸問題能夠通過使損失函數(shù)最小化解決

對損失函數(shù)求導(dǎo),令φ(ξk)=有

其中

令θk=相應(yīng)地,有

進一步觀察Φk以及ξk的定義,將Φk的表達式展開:

實際上由于真實狀態(tài)值xk不可知,代入回歸計算的是外推估計值k/k?1,從而在式中δxk/k?1=0.同時,觀測噪聲也可視為觀測殘差,為了與觀測殘差的經(jīng)典表示方法一致,令ek=vk.因此,有

由式(36)可知,Φk是基于觀測殘差的一個無量綱數(shù).構(gòu)造修正因子?:

通過修正因子對觀測噪聲協(xié)方差矩陣進行重構(gòu),修正后的協(xié)方差矩陣為

結(jié)合式(37)和(38)可知,修正因子只對協(xié)方差矩陣進行修正,這也與Φk表達式的含義一致.也可表示為

易知,基于魯棒M估計的自適應(yīng)CKF算法的實質(zhì)在于修正觀測向量的誤差協(xié)方差.將損失函數(shù)最小化,即可得到狀態(tài)的最佳估計.

現(xiàn)將RMA-CKF的步驟總結(jié)如下:

狀態(tài)更新階段與式(8)-(11)相同.在量測更新階段,觀測預(yù)測值的計算與式(12)-(14)相同,Pxz,k/k?1的計算與式(16)相同,Pzz,k/k?1以及濾波更新的公式為

由式(20)和(28)可知,本文提出的RMA-CKF算法借鑒了Huber等價權(quán)函數(shù)的思想,但又與之有明顯的不同:Huber方法是對觀測值的每一個維度分別進行檢測,當某一維度出現(xiàn)野值時,通過權(quán)函數(shù)降低其權(quán)重,抑制野值對狀態(tài)估計造成的影響;而RMA-CKF算法則是對觀測殘差的所有維度進行綜合評判,再利用修正因子對協(xié)方差矩陣Rk進行修正.

3.2 算法分析

表1給出了CKF算法、基于Huber M估計的CKF算法(Huber-CKF)、文獻[21]提出的基于廣義M估計的魯棒CKF 算法(M-estimates based robust cubature Kalman filter,MR-CKF)以及本文提出的RMA-CKF算法的觀測噪聲協(xié)方差矩陣.

表1 算法的協(xié)方差矩陣Table 1 Covariance matrix of algorithms

Huber-CKF算法的穩(wěn)健因子見式(20).對于MR-CKF算法,其權(quán)重因子wk根據(jù)3段Huber權(quán)函數(shù)選取

表1比較的幾種算法中,除CKF算法外,其余幾種算法的觀測噪聲協(xié)方差矩陣Rk均與殘差有關(guān),且均能夠根據(jù)殘差自適應(yīng)地調(diào)整大小,以抑制野值對狀態(tài)估計的影響.MR-CKF算法和RMA-CKF算法的穩(wěn)健因子不會改變各自觀測噪聲協(xié)方差矩陣的條件數(shù),而Huber-CKF算法會因為某單一維度出現(xiàn)野值,導(dǎo)致Ψ相應(yīng)維度的數(shù)值較小,從而導(dǎo)致矩陣的條件數(shù)增大.

式(17)可以寫成以下形式:

假設(shè)Kk有微小擾動δKk,Pzz,k/k?1的微小擾動為δPzz,k/k?1,Pxz,k/k?1的微小擾動為δPxz,k/k?1.記 式(46)的擾動方程為

由式(47)推導(dǎo)可得

當δPzz,k/k?1較小時,式(48)可近似為

因此,Huber-CKF算法的觀測噪聲協(xié)方差矩陣條件數(shù)增大將導(dǎo)致cond(Pzz,k/k?1)的增大,進而影響Kk的估計精度.而增益Kk的偏差將影響濾波估計及其協(xié)方差矩陣Pk/k的狀態(tài)估計精度,導(dǎo)致算法性能的退化.本文將在后續(xù)的仿真實驗中對此進行驗證.

4 仿真與分析

4.1 仿真條件

采用蒙特卡洛方法對CKF算法、UKF算法、EKF算法、Huber-CKF算法、MR-CKF算法以及本文提出的RMA-CKF算法進行了對比實驗.目標的起始位置在笛卡爾坐標系下的坐標為[8000,11000,2000],其速度向量為[?50,?100,0].

為了保證仿真實驗的合理性,兩個算例的初始條件相同.假定真實狀態(tài)量的過程噪聲強度為q=0.1 m/s2,測量時間為100 s,采樣的時間間隔?t=0.2 s,即采樣次數(shù)為500次.在仿真實驗中,觀測雷達的斜距離噪聲標準差為50 m,方位角和高低角測量噪聲標準差均為0.5?.

定義位置均方根誤差RMSE為

算例1量測過程中傳感器出現(xiàn)觀測野值的概率為10%,野值觀測噪聲的標準差擴大為正常值的100倍.

算例2量測過程中傳感器出現(xiàn)觀測野值的概率為40%,野值觀測噪聲的標準差擴大為正常值的100倍.

4.2 結(jié)果分析

本文選取了文獻[6]中的EKF算法、文獻[7]中的UKF 算法、文獻[8]中的CKF 算法、文獻[16]中的Huber-CKF算法、文獻[21]中的MR-CKF 算法與本文提出的RMA-CKF算法進行比較.

表2給出了MR-CKF算法中權(quán)函數(shù)閾值的幾種匹配方案,用以選取合適的閾值.圖1和圖2表示幾種閾值匹配方案分別在與算例1和算例2相對應(yīng)的實驗條件下的RMSE.圖1表明,在噪聲污染率為0.1的情況下,方案1-6都無法收斂,只有方案7和8能夠收斂;圖2表明,在噪聲污染率為0.4的情況下,方案1-7都無法收斂,只有方案8能夠收斂.因此,為了與該算法進行比較,在接下來的仿真實驗中,權(quán)函數(shù)的閾值將按照方案8來選取,即χα1=8.5,χα2=20000.

表2 MR-CKF算法閾值的選取Table 2 Threshold selection of MR-CKF

圖1 MR-CKF算法閾值的選取(污染率為0.1)Fig.1 Threshold selection of MR-CKF(α=0.1)

圖2 MR-CKF算法閾值的選取(污染率為0.4)Fig.2 Threshold selection of MR-CKF(α=0.4)

從以上仿真實驗可以看出:

1)如圖1和圖2所示,只有當權(quán)函數(shù)的閾值依據(jù)方案8來選取時,MR-CKF算法才能在噪聲污染率分別為0.1和0.4的條件下均收斂.這是因為在該算法的濾波過程中,觀測野值被舍棄后,用外推預(yù)測值作為這一時刻的估計值,導(dǎo)致外推預(yù)測值與真實狀態(tài)值之間的誤差不斷累積,進而造成算法發(fā)散.因此,在實際的工程應(yīng)用中,MR-CKF算法權(quán)函數(shù)的閾值需要反復(fù)調(diào)試,以避免算法發(fā)散.

2)如圖3和圖4所示,在觀測噪聲中含有不同比例污染噪聲的情況下,各算法都能隨著采樣次數(shù)的增加趨于收斂,但經(jīng)過魯棒處理的算法的估計精度明顯比未經(jīng)過魯棒處理的算法高.在圖3 中,Huber-CKF 算法、MR-CKF算法以及RMA-CKF算法的估計精度大致接近,但仍可以看出Huber-CKF算法比其余兩者更差;RMA-CKF算法和MR-CKF算法都是對殘差進行綜合評價,能夠充分利用觀測野值中的有利信息,從而估計精度比較接近,且比Huber-CKF算法略高.在圖4中,當觀測噪聲中污染噪聲的比例進一步提高時,MR-CKF算法的性能退化,與Huber-CKF算法接近.這是因為當野值數(shù)量增多時,MR-CKF算法中舍棄的觀測值增多,導(dǎo)致外推值與真實值之間的誤差持續(xù)累積.圖5和圖6給出了3種魯棒化算法在不同條件下的濾波軌跡,可以看出RMA-CKF算法的濾波軌跡更接近真實軌跡.表3給出了不同算例中所有采樣點的平均RMSE.由表3可以看出,RMA-CKF算法的估計誤差更小,性能更優(yōu).

圖3 各算法的RMSE比較(算例1)Fig.3 RMSE comparison of algorithms(Example 1)

圖4 各算法的RMSE比較(算例2)Fig.4 RMSE comparison of algorithms(Example 2)

圖5 各算法的濾波軌跡(算例1)Fig.5 Filtered tracks of algorithms(Example 1)

圖6 各算法的濾波軌跡(算例2)Fig.6 Filtered tracks of algorithms(Example 2)

表3 各算法的平均RMSETable 3 Average RMSE of algorithms

3)圖7 和圖8 給出了不同噪聲污染率條件下Huber-CKF算法與本文算法協(xié)方差矩陣條件數(shù)的比較.圖中的縱坐標為兩種算法協(xié)方差矩陣條件數(shù)比值的對數(shù),即

可以看出,Huber-CKF算法的協(xié)方差矩陣的條件數(shù)在不同噪聲污染率條件下均更大.由式(49)可知,野值的出現(xiàn)將導(dǎo)致Huber-CKF算法的協(xié)方差矩陣的條件數(shù)增大,影響增益的估計精度,進而導(dǎo)致算法的性能下降.這也表明Huber-CKF算法存在數(shù)值穩(wěn)定性問題.

圖7 條件數(shù)比較(算例1)Fig.7 Comparison of condition number(Example 1)

圖8 條件數(shù)比較(算例2)Fig.8 Comparison of condition number(Example 2)

4)表4給出了兩個算例中各算法的相對運行時間.從時間復(fù)雜度來看,EKF算法用時最少,CKF算法次之,UKF算法用時比CKF算法略多.而在3種魯棒算法中,RMA-CKF算法用時最少,MR-CKF算法用時最多,但三者的運行相對時間差別較小.這是因為3種魯棒算法都是在CKF算法的框架內(nèi)計算魯棒因子,從而計算代價大致相近.

表4 算法相對運行時間比較Table 4 Comparison of relative runtime of algorithms

5 結(jié)論

基于平方根平滑逼近函數(shù),本文構(gòu)造了一個針對觀測野值的修正因子,進而提出一種基于魯棒M估計的自適應(yīng)CKF算法.該算法充分利用了觀測野值中的有用信息,克服了MR-CKF算法遇到高污染率噪聲時3段Huber權(quán)函數(shù)閾值難以選取的問題,同時也具有更好的數(shù)值穩(wěn)定性.仿真實驗證明,RMA-CKF算法在含有高污染率噪聲的情況下能夠自適應(yīng)地抑制野值的影響.與其他算法相比,在不大幅增加計算代價的前提下,RMA-CKF算法獲得了較強的魯棒性,具有一定的工程應(yīng)用價值.