劉暢, 楊鎖昌,*, 汪連棟, 張寬橋
(1. 軍械工程學院 導彈工程系, 石家莊 050000; 2. 電子信息系統(tǒng)復雜電磁環(huán)境效應國家重點實驗室, 洛陽 471003)
目標跟蹤是一類典型的非線性濾波問題,貝葉斯估計理論為非線性濾波提供了嚴謹?shù)慕鉀Q框架。對于線性高斯系統(tǒng),貝葉斯估計的最優(yōu)解為卡爾曼濾波[1]。但對于非線性高斯系統(tǒng),很難得到高斯加權(quán)積分的解析解,因此科研人員提出了許多次優(yōu)非線性濾波方法。其中,擴展卡爾曼濾波(Extented Kalman Filter,EKF)[2]由于簡單高效得到廣泛應用,但EKF采用非線性函數(shù)的一階泰勒展開近似非線性函數(shù),對于強非線性系統(tǒng)會產(chǎn)生較大的近似誤差,且其需要計算雅可比矩陣,這既增加了計算復雜度也要求非線性函數(shù)連續(xù)可微。
為了克服EKF的缺點,隨機采樣型濾波器和確定采樣型濾波器分別被提出,其核心是用一組隨機或確定加權(quán)采樣點逼近狀態(tài)的后驗分布,以采樣點的加權(quán)和近似“非線性函數(shù)×高斯函數(shù)”的積分。隨機采樣型濾波器的主要應用形式——粒子濾波(Particle Filter,PF)[3],以蒙特卡羅隨機采樣得到的粒子近似狀態(tài)后驗分布,理論上適用于解決任意非線性濾波問題,但存在權(quán)值退化、粒子多樣性匱乏、實時性差等嚴重缺陷。依據(jù)采樣點選取策略的不同,確定采樣型濾波器主要分為中心差分卡爾曼濾波(Central Difference Kalman Filter,CDKF)[4]、無跡卡爾曼濾波(Unscented Kal-man Filter,UKF)[5]和容積卡爾曼濾波(Cubature Kalman Filter,CKF)[6]等。CDKF以多項式插值擬合逼近狀態(tài)后驗分布,計算簡單但濾波精度較低且易受參數(shù)取值影響。UKF以UT(Unsented Transformation)變換逼近狀態(tài)后驗分布,能夠以二階泰勒精度逼近非線性狀態(tài)而計算量與EKF同階,但系統(tǒng)狀態(tài)維數(shù)大于3時,中心采樣點權(quán)值為負,從而引起協(xié)方差矩陣的非正定和濾波發(fā)散。CKF以三階球面-徑向容積準則逼近狀態(tài)后驗分布,其采樣點權(quán)值始終為正,且有嚴格的數(shù)學證明,與UKF相比濾波精度更高而計算量相近,因此,近年來得到了廣泛的應用。
為進一步提高CKF的精度,容積積分卡爾曼濾波(Cubature Quadrature Kalman Filter,CQKF)[7]以容積準則和高斯-拉蓋爾積分準則近似高斯加權(quán)積分,濾波精度與切比雪夫-拉蓋爾多項式的階數(shù)m成正相關關系。CQKF是CKF的拓展形式,當m≥2時,其精度高于CKF。但CQKF要求系統(tǒng)模型和噪聲精確已知,而現(xiàn)代戰(zhàn)場復雜對抗環(huán)境使得系統(tǒng)模型具有很大的不確定性,系統(tǒng)狀態(tài)突變,過程噪聲及量測噪聲未知時變,從而造成CQKF濾波精度下降甚至發(fā)散。為解決噪聲統(tǒng)計特性未知情況下的濾波問題,貝葉斯法[8]、相關法[9-11]、協(xié)方差匹配法[12]和極大似然估計法[13]等多種噪聲估計算法分別被提出。貝葉斯法涉及到多重積分,計算量大且未必能得到最優(yōu)封閉解,多限于理論研究。相關法主要應用于線性系統(tǒng),其在非線性系統(tǒng)中的拓展尚未無偏性證明[14]。協(xié)方差匹配法是一種有偏估計方法,存在穩(wěn)態(tài)估計誤差且估計精度相對較低。極大似然估計法由于能夠直接構(gòu)造含有待估計參數(shù)的概率密度函數(shù),且計算量適中而得到廣泛關注,其中以基于極大后驗估計(Maximum A Posterior, MAP)準則提出的Sage-Husa時變噪聲統(tǒng)計估值器[15]應用最為廣泛。
基于上述考慮,本文提出了一種自適應強跟蹤CQKF(AST-CQKF) 算法。AST-CQKF借鑒強跟蹤濾波(Strong Tracking Filter, STF)的思想,引入時變漸消因子對預測協(xié)方差矩陣在線調(diào)整,同時利用Sage-Husa時變噪聲統(tǒng)計估值器對噪聲統(tǒng)計特性實時估計,并將估計結(jié)果引入濾波迭代過程中,從而增加了CQKF算法的魯棒性和自適應性。通過仿真實驗驗證了本文算法的有效性和可行性。
考慮如下非線性系統(tǒng):
(1)
式中:xk∈Rn為k時刻的狀態(tài)向量;zk∈Rp為k時刻的量測向量;f:Rn→Rn和h:Rn→Rp為已知非線性函數(shù);vk-1∈Rn和ηk∈Rp為不相關的高斯白噪聲,且vk-1~N(0,Qk-1),ηk~N(0,Rk);Qk-1和Rk為噪聲協(xié)方差矩陣。
假設k-1時刻的后驗概率密度函數(shù)為高斯分布,即
(2)
(3)
其中狀態(tài)一步預測及協(xié)方差為
(4)
(5)
輸出預測、自協(xié)方差及互協(xié)方差分別為
(6)
(7)
(8)
非線性系統(tǒng)式(1)在最小方差估計準則下的最優(yōu)高斯濾波器為[16]
(9)
式中:
(10)
因此,貝葉斯濾波的核心問題轉(zhuǎn)化為計算形式如式(11)的積分表達式:
(11)
式中:f(x)為任意非線性函數(shù);g(x)為高斯密度函數(shù)。通常式(11)難以得到解析表達式,主要采取一系列采樣點ξj及其權(quán)值wj的加權(quán)和進行數(shù)值近似,即
(12)
對于任意函數(shù)f(x),X∈Rn,式(11)的一般形式為
(13)
可以在球面坐標系中表示為[7]
μ)ds(Z)]rn-1e-r2/2dr
(14)
式中:X=CrZ+μ,且X~N(μ,Σ),μ為高斯分布的均值,C為協(xié)方差矩陣Σ的Cholesky分解,即Σ=CCT;r和Z為積分變量;Un={Z∈RnZZT=1}為單位超球面;ds(·)為Un上的面積元素。
若μ=0且Σ為單位矩陣,則式(14)中的積分為
(15)
由三階球面-徑向容積準則可以近似為
(16)
式中:Γ(·)為伽馬函數(shù);[ui](i=1,2,…,2n)為位于單位超球面與坐標軸交點的容積點,即完全對稱點集[1]的第i個列向量
[1]=
(17)
將式(16)代入式(14),令λ=r2/2,則
λ(n/2-1)e-λdλ
(18)
根據(jù)高斯-拉蓋爾積分準則,積分可以近似為
(19)
式中:積分點λj(j=1,2,…,m)為m階切比雪夫-拉蓋爾多項式的m個根,即λj滿足
(m+α)(m+α-1)λm-2-…=0
(20)
式中:α=n/2-1。
對應的權(quán)值Aj為
(21)
將式(19)~式(21)代入式(18),則得到容積積分準則為
(22)
由式(22)可知,對于如式(1)所示的n維狀態(tài)估計問題,需要計算滿足
(23)
的2mn個采樣點ξl(本文稱其為CQ點)及對應權(quán)值wl,其計算方式如下:
i=1,2,…,2n;j=1,2,…,m;l=1,2,…,2mn
(24)
(25)
步驟1初始化。
1) 令
2) 根據(jù)式(24)和式(25)計算CQ點ξl及對應權(quán)值wl。
步驟2預測更新。
2) 計算CQ點:
3) 更新CQ點隨狀態(tài)方程的轉(zhuǎn)移:
4) 計算一步狀態(tài)預測及預測誤差協(xié)方差矩陣:
步驟3量測更新。
1) 分解協(xié)方差矩陣:
2) 計算CQ點:
3) 更新CQ點隨量測方程的轉(zhuǎn)移:
Zl,kk-1=h(χl,kk-1)
4) 計算量測預測值:
5) 計算自協(xié)方差矩陣及互協(xié)方差矩陣:
6) 計算卡爾曼增益:
7) 估計狀態(tài):
8) 估計狀態(tài)預測誤差協(xié)方差矩陣:
為了提高EKF對于系統(tǒng)模型不確定性及狀態(tài)突變的魯棒性,周東華等[17]提出的STF利用衰減記憶濾波思想,在計算預測誤差協(xié)方差矩陣時引入漸消因子,強迫殘差序列正交,從而保證濾波器對系統(tǒng)實際狀態(tài)的跟蹤效果。
非線性系統(tǒng)式(1)的STF方程為[18]
(26)
式中:
In×n為n階單位矩陣;λk為漸消因子,其計算式為
(27)
式中:
(28)
(29)
(30)
(31)
其中:tr(·)為矩陣求跡運算;β≥1為弱化因子;0<ρ≤1為遺忘因子,通常取ρ=0.95。由于STF是EKF的改進形式,在計算漸消因子時仍需要求解雅可比矩陣,因此不能直接將其引入CQKF,需要研究不利用Hk計算漸消因子的等價表述方式。文獻[19]給出了Hk、Nk和Mk的等效表述形式,即
(32)
(33)
(34)
(35)
漸消因子的具體引入方式見第4節(jié)。
考慮噪聲的一般形式,即vk-1∈Rn和ηk∈Rp是帶時變均值和協(xié)方差且線性無關的高斯白噪聲,且vk-1~N(qk-1,Qk-1),ηk~N(rk,Rk)。針對qk、Qk、rk和Rk等噪聲參數(shù)的估計問題,文獻[15]基于MAP準則得到了Sage-Husa噪聲統(tǒng)計估值器,并給出了最優(yōu)MAP估值器、次優(yōu)MAP估值器、次優(yōu)無偏MAP估值器和時變噪聲統(tǒng)計估值器等多種形式。文獻[20]和文獻[21]分別將其拓展到UKF和CKF,給出了適用于UKF和CKF的遞推算法。由于UKF、CKF和CQKF同屬確定采樣型濾波器,借鑒UKF和CKF的遞推形式,得到如下適用于CQKF的次優(yōu)無偏MAP常值噪聲統(tǒng)計估計器:
(36)
(37)
(38)
(39)
對于時變噪聲統(tǒng)計而言,應當強調(diào)新近數(shù)據(jù)的作用,逐漸遺忘陳舊數(shù)據(jù)。選取加權(quán)系數(shù){γi},使之滿足
(40)
于是有
(41)
式中:b為遺忘因子。將次優(yōu)無偏MAP常值噪聲統(tǒng)計估計器中的加權(quán)和系數(shù)1/k替換為{γi},即得到適用于CQKF的時變噪聲統(tǒng)計估值器:
(42)
(43)
(44)
(45)
將時變漸消因子和時變噪聲統(tǒng)計估值器嵌入標準CQKF,并將其拓展到噪聲均值非零的情形,即可得到AST-CQKF算法,流程如圖1所示。
圖1 AST-CQKF算法流程圖Fig.1 AST-CQKF algorithm flowchart
算法具體流程如下:
步驟1初始化。
1) 令
2) 根據(jù)式(24)和式(25)計算CQ點ξl及對應權(quán)值wl。
步驟2預測更新。
2) 計算CQ點:
3) 計算CQ點隨狀態(tài)方程的轉(zhuǎn)移:
4) 計算一步狀態(tài)預測及預測誤差協(xié)方差矩陣:
步驟3量測更新。
1) 分解協(xié)方差矩陣:
2) 計算CQ點:
3) 計算CQ點隨量測方程的轉(zhuǎn)移:
4) 計算量測預測值:
5) 計算自協(xié)方差矩陣和互協(xié)方差矩陣:
步驟4一步狀態(tài)預測協(xié)方差修正。
1) 根據(jù)式(35)計算漸消因子λk。
2) 利用λk修正一步狀態(tài)預測協(xié)方差矩陣:
3) 分解修正后的協(xié)方差矩陣:
4) 計算CQ點:
5) 計算CQ點經(jīng)量測方程的轉(zhuǎn)移:
6) 計算量測預測值:
7) 計算自協(xié)方差矩陣和互協(xié)方差矩陣:
步驟5狀態(tài)更新。
1) 計算卡爾曼增益:
2) 估計狀態(tài):
3) 估計狀態(tài)預測誤差協(xié)方差矩陣:
步驟6噪聲估計。
根據(jù)式(42)~式(45)對噪聲統(tǒng)計特性遞推估計。
考慮一個典型的二維平面雷達跟蹤問題[7],目標以固定未知轉(zhuǎn)彎速率Ω做圓周運動,其狀態(tài)方程為
Xk-1+vk-1
(46)
(47)
雷達固定于坐標原點,對目標的距離rk及方位角θk進行測量,則量測方程為
(48)
初始狀態(tài)真實值為
對應的協(xié)方差矩陣為
P00= diag[100 m210 m2/s2100 m210 m2/s2
100 m·rad2/s2]T
(49)
(50)
(51)
為驗證算法在系統(tǒng)狀態(tài)模型不準確及噪聲統(tǒng)計特性不準確下的有效性,設置以下2種仿真情形。
由圖2可知,通過AST-CQKF算法得到的位置、速度均方根誤差明顯小于CQKF算法,而轉(zhuǎn)彎率均方根誤差基本相同,可見AST-CQKF算法能夠有效減小噪聲統(tǒng)計特性不準確帶來的估計誤差,提高估計精度。此外,噪聲統(tǒng)計特性的改變對位置、速度、轉(zhuǎn)彎率的影響程度不同,對位置影響最大,速度次之,轉(zhuǎn)彎率幾乎無影響。
圖2 跟蹤均方根誤差(情形1)Fig.2 RMSE tracking(Case 1)
為設置系統(tǒng)模型的不確定性,將式(46)變形得
圖3 跟蹤均方根誤差(情形2)Fig.3 RMSE tracking(Case 2)
Xk=
Xk-1+vk-1
(52)
式中:a和c為可調(diào)節(jié)的參數(shù),模型正常時兩者均為1,通過設置不同的系數(shù)以模擬系統(tǒng)狀態(tài)模型的不確定性,本文設定a=1.1,c=1.2。仿真過程中,狀態(tài)真值由精確的狀態(tài)方程得到,而濾波器使用不精確的狀態(tài)方程進行狀態(tài)估計,以此比較AST-CQKF與CQKF算法在系統(tǒng)狀態(tài)模型不準確時的濾波精度。經(jīng)過200次獨立的蒙特卡羅仿真,得到位置、速度、轉(zhuǎn)彎率的RMSE結(jié)果分別如圖3所示(情形2)。
由圖3可知,系統(tǒng)狀態(tài)模型不準確時,CQKF在仿真開始后迅速發(fā)散,無法對目標保持跟蹤,而AST-CQKF算法雖然不能完全消除模型不確定性的影響,但能夠防止濾波發(fā)散,保持濾波收斂性,對于模型不確定型具有魯棒性。
設置不同的仿真步數(shù)與仿真次數(shù),得到CQKF算法與AST-CQKF算法的仿真計算時間如表1所示。由表1可以看出,與前者相比,后者的仿真計算時間增加了一倍左右,尤其是仿真次數(shù)較多時,計算時間的增加更為明顯,這是由于AST-CQKF算法步驟更多、復雜度更高引起的。但單次仿真時,2種算法的時間均小于0.1 s,因此與濾波精度的提高相比,AST-CQKF算法的時間增加處于可接受的范圍內(nèi)。
表1 CQKF與AST-CQKF算法蒙特卡羅仿真計算時間Table 1 Calculation time of Monte Carlo simulation in CQKF and AST-CQKF algorithm
CQKF算法在復雜對抗環(huán)境下的目標跟蹤主要存在2方面問題:①系統(tǒng)狀態(tài)空間模型發(fā)生突變導致濾波發(fā)散;②噪聲統(tǒng)計特性發(fā)生改變或不精確造成濾波精度下降。針對該問題,本文提出了一種新的自適應強跟蹤CQKF(AST-CQKF)算法應用到目標跟蹤:
1) 將強跟蹤濾波器與CQKF相結(jié)合,給出了適用于CQKF的漸消因子計算方法,利用漸消因子修正一步狀態(tài)預測協(xié)方差矩陣,進而用于狀態(tài)估計,克服了模型不準確影響濾波精度和濾波穩(wěn)定性的問題,改善了CQKF的跟蹤性能。
2) 利用Sage-Husa時變噪聲統(tǒng)計估值器對噪聲實時估計,得到了AST-CQKF算法,增強了濾波器對于噪聲統(tǒng)計特性變化的自適應能力,有效地提高了濾波精度。
3) 仿真結(jié)果表明,在系統(tǒng)量測噪聲統(tǒng)計特性不準確或狀態(tài)空間模型不準確的情況下,CQKF算法的濾波精度急劇下降甚至濾波發(fā)散, AST-CQKF算法則能夠?qū)崿F(xiàn)系統(tǒng)狀態(tài)的快速準確跟蹤,有效地克服了CQKF算法的局限性。
參考文獻 (References)
[1] KALMAN R E.A new approach to linear filtering and prediction theory[J].Transactions on ASME Journal of Basic Engineering,1960,82(D):35-46.
[2] JAZWINSKI A H.Stochastic processes and filtering theory[M].New York:Academic Press,1970:235-237.
[3] 李天成,范紅旗.孫樹棟.粒子濾波理論、方法及其在多目標跟蹤中的應用[J].自動化學報,2015,41(12):1981-2002.
LI T C,FAN H Q,SUN S D.Particle filtering:Theory,approach,and application for multitar-get tracking[J].Acta Automatica Sinica,2015,41(12):1981-2002(in Chinese).
[4] 韓萍,干浩亮,何煒琨,等.基于迭代中心差分卡爾曼濾波的飛機姿態(tài)估計[J].儀器儀表學報,2015,36(1):187-193.
HAN P,GAN H L,HE W K,et al.Iterated central difference Kalman filter based aircraft attitude estimation[J].Chinese Journal of Scientific Instrument,2015,36(1):187-193(in Chinese).
[5] 王寶寶,吳盤龍. 基于平方根無跡卡爾曼濾波平滑算法的水下純方位目標跟蹤[J].中國慣性技術學報,2016,24(2):180-184.
WANG B B,WU P L.Underwater bearing-only tracking based on square-root unscented Kalman filter smoothing algorithm[J].Journal of Chinese Inertial Technology,2016,24(2):180-184(in Chinese).
[6] 張龍,崔乃剛,楊峰,等.高階容積卡爾曼濾波及其在目標跟蹤中的應用[J].哈爾濱工程大學學報,2016,37(4):573-578.
ZHANG L,CUI N G,YANG F,et al.High-degree cubature Kalman filter and its application in target tracking[J].Journal of Harbin Engineering University,2016,37(4):573-578(in Chinese).
[7] BHAUMIK S,WATI S.Cubature quarature Kalman filter[J]. IET Signal Processing,2013,7(7):533-541.
[8] LAINIOTIS D G.Optimal adaptive estimation:Structure and parameters adaption[J].IEEE Transactions on Automatic Control,1971,16(2):160-170.
[9] MEHRA R K. On the identification of variances and adaptive filtering[J].IEEE Transactions on Automatic Control,1970,15(2):175-184.
[10] ODELSON B J,RAJAMANI M R,RAWLINGS J B.A new autocovariance least-squares method for estimating noise covariances[J].Automatica, 2006,42(2):303-308.
[11] AKESSON B M,JORGENSON J B,POULSEN N K,et al.A generalized autocovariance least-squares method for Kalman filter tuning[J].Journal of Process Control,2008,18(7-8):769-779.
[12] MYERS K A,TAPLEY B D.Adaptive sequential estimation with unknown noise statistics[J].IEEE Transactions on Automatic Control,1976,21(8):520-523.
[13] KASHYAP R L.Maximum likelihood identification of stochastic linear systems[J].IEEE Transactions on Automatic Control,1970,15(1):25-34.
[14] LIMA F V, RAJAMANI M R, SODERSTROM T A,et al.Covariance and state estimation of weakly observable systems:Application to polymerization processes[J].IEEE Transactions on Control Systems Technology,2013,21(4):1249-1257.
[15] 李寧,祝瑞輝,張勇剛.基于Sage-Husa算法的自適應平方根CKF目標跟蹤方法[J].系統(tǒng)工程與電子技術,2014,36(10):1899-1905.
LI N,ZHU R H,ZHANG Y G.Adaptive square CKF method for target tracking based on Sage-Husa algorithm[J].Systems Engineering and Electronics,2014,36(10):1899-1905(in Chinese).
[16] 王小旭,潘泉,黃鶴,等.非線性系統(tǒng)確定采樣型濾波算法綜述[J].控制與決策,2012,27(6):801-812.
WANG X X,PAN Q,HUANG H,et al.Overview of deterministic sampling filtering algorithms for nonlinear system[J].Control and Decision,2012,27(6):801-812(in Chinese).
[17] 周東華,席裕庚,張鐘俊.一種帶多重次優(yōu)漸消因子的擴展卡爾曼濾波器[J].自動化學報,1991,17(6):689-695.
ZHANG D H,XI Y G,ZHANG Z J.A suboptimal multiple fading extended Kalman filter[J].Acta Automatica Sinica, 1991,17(6):689-695(in Chinese).
[18] 方君,戴邵武,許文明,等.基于ST-SRCKF的超高速強機動目標跟蹤算法[J].北京航空航天大學學報,2016,42(8):1698-1708.
FANG J,DAI S W,XU W M,et al.Highly maneuvering hypervelocity-target tracking algorithm based on ST-SRCKF[J].Journal of Beijing University of Aeronautics and Astronautics,2016,42(8):1698-1708(in Chinese).
[19] 張龍,崔乃剛,王小剛,等.強跟蹤-容積卡爾曼濾波在彈道式再入目標跟蹤中的應用[J].中國慣性技術學報,2015,23(2):211-218.
ZHANG L,CUI N G,WANG X G,et al.Strong tracking-cubature Kalman filter for tracking ballistic reentry target[J].Journal of Chinese Inertial Technology,2015,23(2):211-218(in Chinese).
[20] 趙琳,王小旭,孫明,等.基于極大后驗估計和指數(shù)加權(quán)的自適應UKF濾波算法[J].自動化學報,2010,36(7):1007-1019.
ZHAO L,WANG X X,SUN M,et al.Adaptive UKF filteing algorithm based on maximum a posterior estimation and exponential weighting[J].Acta Automatica Sinica,2010,36(7):1007-1019(in Chinese).
[21] 丁家琳,肖建,趙濤.自適應CKF強跟蹤濾波器及其應用[J].電機與控制學報,2015,19(11):111-120.
DING J L,XIAO J,ZHAO T.Adaptive CKF strong tracking filter and application[J].Electric Machines and Control,2015,19(11):111-120(in Chinese).