引入STF算法的自適應(yīng)SICKF及其在目標(biāo)跟蹤中的應(yīng)用*

沈翔鴻 ，徐曉楓 ，劉 寬 ，張 磊

（1.空軍工程大學(xué)等離子體動力學(xué)重點實驗室，西安 710038；2.中國航發(fā)四川燃?xì)鉁u輪研究院，四川 綿陽 621000；3.西安應(yīng)用光學(xué)研究所，西安 710065）

0 引言

非線性濾波主要解決非線性隨機動態(tài)系統(tǒng)在噪聲觀測干擾下的狀態(tài)估計問題，近年來基于Ito等提出的高斯非線性濾波理論［1］，出現(xiàn)了很多次優(yōu)的非線性濾波方法。Arasaratnam于2009年基于球面-徑向容積準(zhǔn)則提出了容積卡爾曼濾波（Cubature Kalman Filter，CKF）［2］的概念。然而 CKF 不易于高階擴展，難以推導(dǎo)估計精度較高的高階CKF算法。此外，球面容積準(zhǔn)則的固有缺陷也使得CKF的估計精度受到限制。張鑫春提出了三階嵌入式容積卡爾曼濾波（Imbedded Cubature Filter，ICKF）方法及其均方根形式（Square-root ICKF，SICKF）［3-5］。其中，SICKF采用嵌入式容積準(zhǔn)則，克服了球面-徑向容積準(zhǔn)則的諸多缺陷；結(jié)合均方根濾波技術(shù)，減小了算法的估計誤差，數(shù)值穩(wěn)定性得到提高，某些情況下可以在計算量小于五階濾波時取得接近五階的估計精度；可以方便推導(dǎo)出高階ICKF算法［6］，易于進行高階擴展。

另一方面，當(dāng)系統(tǒng)狀態(tài)突變或模型包含不確定性項時，無論是CKF還是SICKF算法，其增益矩陣滯后于預(yù)測狀態(tài)殘差的突變反應(yīng)，造成估計精度降級。為解決這一問題，可以采用強跟蹤濾波（Strong Tracking Filter，STF）方法對該缺陷進行一定補償，該方法能夠通過自適應(yīng)漸消因子對增益矩陣實時調(diào)整，魯棒性及應(yīng)對系統(tǒng)狀態(tài)突變等不確定因素的能力較強［7］。文獻(xiàn)［8］通過嚴(yán)格的理論推導(dǎo)得到STF的等價表述，提出了強跟蹤無跡卡爾曼濾波（Unscented Kalman Filter，UKF）算法。文獻(xiàn)［9］將 STF 與中心差分卡爾曼濾波方法（Central Difference Kalman Filter，CDKF）結(jié)合，類似地提出了一種強跟蹤CDKF算法。文獻(xiàn)［10］結(jié)合STF和CKF優(yōu)點，建立了一種新的強跟蹤CKF（ACKF）算法。文獻(xiàn)［11］將STF中的EKF用均方根容積濾波（Square-Root CKF，SRCKF）替代，進而提出一種強跟蹤SRCKF。文獻(xiàn)［12］在高階容積卡爾曼濾波（High-Degree Cubature Kalman Filter，HCKF）基礎(chǔ)上，通過引入自適應(yīng)漸消因子，建立了自適應(yīng)HCKF。但上述算法僅僅是把UKF，CKF，CDKF，和HCKF的協(xié)方差矩陣引入漸消因子，不能從根本上證明算法的強跟蹤濾波性能，并且每次濾波都必須進行3次積分點的變換，計算速度較慢。針對上述問題，文獻(xiàn)［13］根據(jù)正交性原理，推算得到確保強跟蹤UKF有效的條件，并提出對于突變狀態(tài)具有更強跟蹤能力的改進的強跟蹤UKF算法，在SINS大方位失準(zhǔn)角初始對準(zhǔn)問題中取得良好的估計效果。

為進一步提高CKF應(yīng)對系統(tǒng)狀態(tài)突變等不確定因素的能力和算法的濾波精度，本文提出自適應(yīng)均方根嵌入式容積卡爾曼濾波（ASICKF）方法。首先從理論上證明嵌入式容積準(zhǔn)則相對于球面-徑向容積準(zhǔn)則具有更高的逼近精度。然后將文獻(xiàn)［13］中的強跟蹤濾波方法引入SICKF，提出滿足成為強跟蹤濾波器充分條件的ASICKF方法，同時研究出了明確的算法流程。最后，將本文提出算法在一類機動目標(biāo)跟蹤問題中進行了運行，開展了數(shù)值仿真。結(jié)果表明，本文提出的算法可以有效地抵抗?fàn)顟B(tài)突變未知的不良影響，驗證了該算法具有良好的估計能力。

1 均方根嵌入式容積卡爾曼濾波

1.1 三階球面-徑向容積準(zhǔn)則

考慮如下形式的離散非線性系統(tǒng):

令x=rs，且sTs=1，將式（3）變換至球面-徑向坐標(biāo)系統(tǒng)，有

則三階球面-徑向容積準(zhǔn)則概括為:

其中ω1為權(quán)值，且

2）球面容積準(zhǔn)則由式（6）逼近:

將式（5）和式（6）代入式（4），得到三階球面 -徑向容積準(zhǔn)則:

應(yīng)該指出的是式（7）中正權(quán)值會使得容積點超出定義區(qū)域，或產(chǎn)生復(fù)數(shù)容積點［14］；另外，n充分大時，容積點ξi甚至可能超出積分區(qū)域，此外，如果要對其進行行高階擴展，必須提高容積準(zhǔn)則和高斯-拉蓋爾準(zhǔn)則的階數(shù)，還需計算n維超球體面積分，由于這些過程過于冗繁復(fù)雜，致使CKF算法高階擴展性不好。

1.2 嵌入式容積準(zhǔn)則

這里依據(jù)嵌入式容積準(zhǔn)則給出一組集合族R，集合族R完全對稱，用以逼近積分I（g）。這里設(shè)定，作為集合R中多項式擬合階數(shù)為2m+1的元素，即

式（14）說明嵌入式容積準(zhǔn)則對所有奇次多項式的積分都是精確的（積分值為0）。

取m=1，便可得到三階嵌入式容積準(zhǔn)則，此時僅需考慮g在偶次多項式集合上的積分

同理，在式（14）中，取 m=2，便可推導(dǎo)出五階ICKF，詳細(xì)過程參考文獻(xiàn)［6］。

下面從逼近誤差的角度，證明嵌入式容積準(zhǔn)則較球面-徑向容積準(zhǔn)則具有更優(yōu)的估計精度。

1）球面-徑向容積準(zhǔn)則的估計誤差為:

其中:c1和c2分別是四階項的系數(shù)。

2）嵌入式容積準(zhǔn)則的自由變量δ設(shè)定為δ2=1，同式（25）～式（27）的推導(dǎo)過程，得到上述算法中總的逼近誤差為

由此可見，狀態(tài)變量的維數(shù)n與前者的逼近誤差r相關(guān)度很大。變量維數(shù)n增大到一定程度時，逼近誤差r與n成正相關(guān)。這里在系統(tǒng)維數(shù)較高時，三階嵌入式容積準(zhǔn)則的逼近誤差受n的影響為零。

1.3 均方根嵌入式容積卡爾曼濾波

計算過程中，由于矩陣求逆和開方運算都將引入舍入誤差，這會致使濾波過程中狀態(tài)估計誤差協(xié)方差陣變成非正定矩陣。由于協(xié)方差矩陣的均方根形式包含協(xié)方差矩陣的特征空間信息，可加強協(xié)方差矩陣傳遞的準(zhǔn)確性，為提高濾波算法的數(shù)值穩(wěn)定性，結(jié)合均方根濾波技術(shù)，給出SICKF算法。

給定初始條件P0|0和，由式（22）～ 式（24），提出三階SICKF如下:

2）量測更新

由此可見，運用嵌入式容積準(zhǔn)則和均方根濾波方法的SICKF相比CKF，有更小的誤差以及更好的數(shù)值穩(wěn)定性，并且能夠?qū)С龈唠ASICKF算法。

2 自適應(yīng)均方根嵌入式CKF

2.1 強跟蹤濾波器的充分條件

針對式（1），文獻(xiàn)［7］提出了 STF，有效地解決了模型不確定時EKF魯棒性變差，出現(xiàn)濾波發(fā)散的問題。濾波器成為STF的充分條件便是通過實時調(diào)整增益矩陣使得下面兩個條件同時滿足［13］:

濾波算法只有同時滿足條件式（45）和式（46）時，才能稱為強跟蹤濾波算法，文獻(xiàn)［13］指出了文獻(xiàn)［8］中的強跟蹤UKF存在的缺陷，通過嚴(yán)謹(jǐn)?shù)睦碚撏茖?dǎo)，得到強跟蹤UKF成立的充分條件。

計算過程中發(fā)現(xiàn)，CKF算法是通過將基于對稱采樣的UKF算法中可調(diào)參數(shù)κ置零得到的，也證明CKF是基于對稱采樣的UKF算法可調(diào)參數(shù)κ置零的特例。此外，不難發(fā)現(xiàn)在CKF和SRCKF的基礎(chǔ)上，采用更加精確的容積準(zhǔn)則便得到ICKF和SICKF。根據(jù)上述情況，本文將文獻(xiàn)［13］中的強跟蹤濾波方法嘗試引入SICKF，從而提出了ASICKF算法。

2.2 ASICKF算法的流程

1）進行第 1次 Cubature Transform（CT）變換計算容積點

其中，SQ，k-1為 Qk-1的均方根。

式（56）中，ρ為遺忘因子，一般取 0＜ρ≤1。

8）定義中間過程變量Nk和Mk

式中，β為弱化因子，定義與原始強跟蹤濾波器相同。

11）進行量測更新。本文按照式（64）～式（65）可以求出增益矩陣Wk，狀態(tài)估計和估計誤差協(xié)方差陣Pk|k的均方根Sk|k，從而整個濾波過程結(jié)束。

基于正交性原理，STF通過調(diào)整增益矩陣Wk，能夠使?fàn)顟B(tài)估計實時跟蹤殘差的變化。上述ASICKF算法與SICKF算法在時間更新過程上相同；創(chuàng)新點是，做了強跟蹤理論與SICKF算法的結(jié)合，即在量測更新過程中，根據(jù)殘差γk得到漸消因子，通過和反饋于增益矩陣Wk。這與文獻(xiàn)中提到的強跟蹤濾波算法相比，ASICKF算法僅需進行兩次CT變換，使算法的復(fù)雜度和計算量都大大降低，相比應(yīng)用起來更為簡便。

3 數(shù)值仿真

這里，還將ASICKF算法嘗試用于解決包含未知機動的目標(biāo)跟蹤問題中，并將ASICKF算法與SICKF、CKF和ACKF等算法進行仿真對比研究。這里把地面坐標(biāo)系o-xyz的原點定于地面，對3個坐標(biāo)軸方向分別設(shè)定，ox方向為東，oy方向為北，oz方向為天向。對目標(biāo)假設(shè)以未知角速度Ω等高度飛行，假設(shè)高度h=11 000 m。計算過程中，地球扁率、自轉(zhuǎn)的影響不作考慮。根據(jù)上述假設(shè)目標(biāo)的運動特點，可采用如下模型進行描述:

設(shè)定測量雷達(dá)自地面坐標(biāo)系原點處獲取目標(biāo)斜距η和目標(biāo)方位角θ等信息。測量方程為，滿足

這里設(shè)定初始時刻t0=1 s，期間飛行目標(biāo)進行了2次加強機動，分別在t=25 s時刻和t=75 s時刻，持續(xù)時間均為2 s:

這里進行蒙特卡洛打靶實驗200次，打靶時間為100 s。為目標(biāo)真實狀態(tài)初始值，真實狀態(tài)的協(xié)方差矩陣可以表示為如下矩陣:。這里采用滿足均值為、協(xié)方差為的高斯正態(tài)分布隨機生成每次實驗的初始狀態(tài)估計。分別采用SICKF，CKF，ACKF和ASICKF對假設(shè)未知飛行目標(biāo)進行跟蹤。

定義位置均方根誤差和位置的均方根誤差均值分別為

同樣的，可以定義速度均方根誤差RMSEvel和均方根誤差均值MRMSEvel以及轉(zhuǎn)速均方根誤差RMSEomg和均方根誤差均值MRMSEomg。

圖1～圖3為4種濾波方法對假設(shè)未知飛行目標(biāo)位置、速度和轉(zhuǎn)速的均方根誤差情況對比。

圖2 飛行目標(biāo)速度的估計均方根誤差

圖3 飛行目標(biāo)轉(zhuǎn)速的估計均方根誤差

目標(biāo)在第25 s和75 s發(fā)生加強機動后，CKF和SICKF的估計誤差發(fā)生劇烈躍升且開始收斂的情況滯后。分析原因，狀態(tài)改變后，由于飛行目標(biāo)真實運動與模型不符，這時激發(fā)模型的狀態(tài)更新過程引入誤差變大，飛行目標(biāo)真實狀態(tài)通過協(xié)方差陣無法得到準(zhǔn)確反映，也無法根據(jù)測量信息進行調(diào)整，因此，算法的估計性能下降，導(dǎo)致跟蹤精度下降。

ACKF和ASICKF的估計誤差相比其他兩種算法的估計誤差，躍升幅度較小。同樣的，飛行目標(biāo)狀態(tài)的劇烈變化讓系統(tǒng)模型出現(xiàn)短暫失靈，不同的是，在采用這兩種算法濾波過程中，根據(jù)殘差γk的變化，由殘差的方差陣得到的對增益矩陣Wk進行跟蹤調(diào)整，保證了殘差序列中的有效信息能夠被迅速提取出來，不斷使輸出殘差序列正交；相對其他兩種算法，較為準(zhǔn)確的測量數(shù)據(jù)在量測更新過程中的比例變大，從而有效降低了狀態(tài)突變后模型不準(zhǔn)造成的狀態(tài)一步預(yù)測估計誤差的占比，跟蹤精度變高。這里，ASICKF采用了更為精確的容積準(zhǔn)則，因此，其計算精度相比ACKF算法而言，更為精確。

表1 4種濾波方法的均方根誤差均值對比

此外，表1中展示了針對4種濾波方法對未知飛行目標(biāo)位置、速度以及轉(zhuǎn)速的濾波均方根誤差均值MRMSE、使用的容積點個數(shù)進行了分析。很容易發(fā)現(xiàn)，引入自適應(yīng)漸消因子解決狀態(tài)突變的機動目標(biāo)跟蹤問題，可以顯著提高容積濾波的估計精度。這里對比發(fā)現(xiàn)ASICKF算法在整體測量性能上要明顯優(yōu)于ACKF算法。

圖4中，將5次隨機打靶實驗，4種濾波方法運行所需的CPU時間進行對比。這里數(shù)值模擬采用Matlab2014a，計算機配置為 Intel（R）Core（TM）i5-3470 CPU 3.20 GHz。因為ASICKF和SICKF兩種算法在狀態(tài)和量測更新過程中會分解協(xié)方差矩陣這一運算過程，并且其采用容積點多達(dá)33個，這必然會導(dǎo)致其運行時間比CKF方法和ACKF方法要長；采用10個容積點的ACKF方法由于濾波過程中要進行3次CT變換，而ASICKF濾波過程只進行2次CT變換，相比而言，ASICKF方法計算量較小?？偟膩砜?，運行時間上ASICKF和ACKF相差不大；由于引入了，使得ACKF和ASICKF兩種方法運算量增加，運行時間大于上述其他2種方法。

圖4 4種濾波方法的運行時間

4 結(jié)論

本文通過研究引入STF方法中的自適應(yīng)漸消因子，有效減少因系統(tǒng)狀態(tài)突變造成的估計精度下降的問題。文中將該方法在狀態(tài)發(fā)生突變的目標(biāo)跟蹤系統(tǒng)中進行了應(yīng)用，數(shù)值模擬發(fā)現(xiàn)，當(dāng)未知飛行目標(biāo)發(fā)生激烈狀態(tài)變化時，ASICKF法對比CKF法和SICKF法，魯棒性和系統(tǒng)自適應(yīng)能力更強，經(jīng)過對比也發(fā)現(xiàn)，ASICKF法相比ACKF法估計精度和狀態(tài)跟蹤能力更好，證明了ASICKF法表現(xiàn)出良好的濾波性能。