引入STF算法的自適應SICKF及其在目標跟蹤中的應用*

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

（1.空軍工程大學等離子體動力學重點實驗室，西安 710038；2.中國航發四川燃氣渦輪研究院，四川 綿陽 621000；3.西安應用光學研究所，西安 710065）

0 引言

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

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

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

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

1.1 三階球面-徑向容積準則

考慮如下形式的離散非線性系統:

令x=rs，且sTs=1，將式（3）變換至球面-徑向坐標系統，有

則三階球面-徑向容積準則概括為:

其中ω1為權值，且

2）球面容積準則由式（6）逼近:

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

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

1.2 嵌入式容積準則

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

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

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

同理，在式（14）中，取 m=2，便可推導出五階ICKF，詳細過程參考文獻［6］。

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

1）球面-徑向容積準則的估計誤差為:

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

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

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

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

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

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

2）量測更新

由此可見，運用嵌入式容積準則和均方根濾波方法的SICKF相比CKF，有更小的誤差以及更好的數值穩定性，并且能夠導出高階SICKF算法。

2 自適應均方根嵌入式CKF

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

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

濾波算法只有同時滿足條件式（45）和式（46）時，才能稱為強跟蹤濾波算法，文獻［13］指出了文獻［8］中的強跟蹤UKF存在的缺陷，通過嚴謹的理論推導，得到強跟蹤UKF成立的充分條件。

計算過程中發現，CKF算法是通過將基于對稱采樣的UKF算法中可調參數κ置零得到的，也證明CKF是基于對稱采樣的UKF算法可調參數κ置零的特例。此外，不難發現在CKF和SRCKF的基礎上，采用更加精確的容積準則便得到ICKF和SICKF。根據上述情況，本文將文獻［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，狀態估計和估計誤差協方差陣Pk|k的均方根Sk|k，從而整個濾波過程結束。

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

3 數值仿真

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

設定測量雷達自地面坐標系原點處獲取目標斜距η和目標方位角θ等信息。測量方程為，滿足

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

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

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

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

圖1～圖3為4種濾波方法對假設未知飛行目標位置、速度和轉速的均方根誤差情況對比。

圖2 飛行目標速度的估計均方根誤差

圖3 飛行目標轉速的估計均方根誤差

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

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

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

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

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

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

4 結論

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