張星星 劉樂文

（蘭州理工大學(xué)電氣工程與信息工程學(xué)院 蘭州 730050）

1 引言

純角度跟蹤（Bearing-only Tracking，BOT）和純距離跟蹤（Range-only Tracking，ROT）在軍事領(lǐng)域有著極為廣泛的應(yīng)用，如使用紅外傳感器等進行的電子作戰(zhàn)、水底目標跟蹤、空中目標監(jiān)視等，因此近十幾年來引起了許多學(xué)者的關(guān)注和研究。由于具有高度非線性和量測信息的不完整性，使得BOT和ROT在實際應(yīng)用中具有較大的困難。結(jié)合擴展卡爾曼濾波（Extended Kalman Filtering，EKF）解決BOT和ROT問題是最基本和應(yīng)用最廣泛的方法［1～3］。EKF的實質(zhì)是將非線性函數(shù)局部線性化，雖然處理方法簡單，但由于高階項的省略，當系統(tǒng)的非線性程度較強時其濾波的效果變差。進一步研究發(fā)現(xiàn)，相較于近似非線性函數(shù)而言，近似概率分布更加容易，即通過利用不同的采樣策略進行多點采樣逼近非線性分布，典型的有無跡卡爾曼濾波（Unscented kalman filtering，UKF），容積卡爾曼濾波（Cubature kalman filter，CKF）等［4～6］。UKF計算復(fù)雜度與EKF相同，但濾波精度高于EKF的同時不會受系統(tǒng)非線性強度的影響，然而UKF存在可能出現(xiàn)的協(xié)方差非正定、高維系統(tǒng)濾波精度降低等問題［7］。由此，針對高維非線性系統(tǒng)以及UKF協(xié)方差非正定的情況，提出了基于三階球面—向徑容積規(guī)則的容積卡爾曼濾波，其權(quán)值均為正解決了協(xié)方差不正定的問題，而且當系統(tǒng)狀態(tài)維度大于三維時，CKF的濾波精度高于UKF，因而得到了廣泛的應(yīng)用［8～11］。

本文主要針對純角度和純距離跟蹤模型的部分可觀測性和高度非線性問題，在CKF的基礎(chǔ)上研究了最新的濾波算法球面單純形—徑向容積卡爾曼濾波算法（Spherical simplex-radial cubature kalman filtering，SSRCKF）［12～14］。由于在高維非線性系統(tǒng)中，SSRCKF不僅濾波精度高于傳統(tǒng)的CKF，而且相對高階的CKF計算復(fù)雜度降低。所以本文主要結(jié)合SSRCKF解決純角度和純距離跟蹤問題，提出了球面單純形—徑向容積卡爾曼純角度和純距離跟蹤濾波器。將SSRCKF與純角度跟蹤和純距離跟蹤結(jié)合對提高BOT和ROT問題的濾波精度和降低其復(fù)雜性具有重要的實際應(yīng)用意義。

2 球面單純形—徑向容積卡爾曼濾波

非線性高斯系統(tǒng)建模如下所示。

狀態(tài)方程：

量測方程：

其中，xk∈Rn為系統(tǒng)狀態(tài)向量，n為系統(tǒng)狀態(tài)維度；zk∈Rm為量測向量，m為系統(tǒng)量測維度；f(·)和h(·)為非線性方程，vk和nk分別為系統(tǒng)噪聲和量測噪聲，兩者互不相關(guān)且均為均值為零的高斯白噪聲，方差分別為Qk和Rk。

根據(jù)文獻［11］和［12］可得，高斯前提下貝葉斯濾波器遞推過程中的多維向量積分可以統(tǒng)一表示為

其中，g(x)表示任意非線性函數(shù)，N(x;，p)表示以x為變量，均值為，協(xié)方差為p的高斯分布。若令x=ry，則有xTx=r2yTy。其中，r≥0為球體的半徑；Un={y∈Rn|yyT=1}表示單位球體的表面，y為方向矢量，σ(y)表示Un上的元素。式（3）可以轉(zhuǎn)化為

分解可得球面積分：

徑向積分為

1）球面單純形準則

球面單純形準則是利用n-單純形求解式（5）球面積分的一種方法。n-單純形是指在n維及以上歐氏空間中的n+1個與仿射無關(guān)的點集凸包［12］。從幾何角度可看作一個n維的幾何體。若設(shè)n-單純形的n+1個頂點構(gòu)成的向量由ai=[ai，1，ai，2，...，ai，n]T表示，其中i=1，2，...，n+1。

則向量的各個構(gòu)成元素定義為

{ai}滿足中心對稱性，可選作積分點，再結(jié)合球面容積規(guī)則推導(dǎo)如下。

由單項式球面積分的閉式解原理［15］可得

同時，球面積分可由數(shù)值積分近似

其中，yi和ws分別表示采樣點及其對應(yīng)的權(quán)值。由于采樣點滿足中心對稱性，所以三階單純形球面準則只需考慮零階單項式g(y)=1和二階單項式g(y)=y12，分別代入式（8）可得

將式（14）代入式（12）得y2=1。

由此，2(n+1)個點的三階球面單純形準則可表示為

所以，球面積分的點球面單純形準則可以推廣為

徑向準則是一種借助數(shù)值積分求解球面積分的方法，即

令S(r)=rl，則徑向積分可以轉(zhuǎn)換為

若球面-徑向容積準則對應(yīng)的積分點完全對稱，那么徑向積分只要滿足偶數(shù)階代數(shù)精度即可。通過不同的偶數(shù)階單項式的匹配，可確定不同的點及其對應(yīng)的權(quán)重［15～16］。

利用矩匹配法就可得到僅包含一個點的三階徑向準則，零階矩方程和二階矩方程分別為

所以，三階徑向準則為

整理球面單純形準則（16）和徑向準則（17），再將式（15）和式（19）代入可得三階單純形球面徑向容積規(guī)則（SSR）：

嘉靖五年七月，費宏為少師兼太子太師吏部尚書華蓋殿大學(xué)士，楊一清為少師兼太子太師吏部尚書謹身殿大學(xué)士。華蓋殿大學(xué)士高于謹身殿大學(xué)士，故費宏地位高于楊一清。嘉靖二十四年十二月，夏言為少師兼太子太師吏部尚書華蓋殿大學(xué)士，嚴嵩為少師兼太子太師吏部尚書謹身殿大學(xué)士。夏言取代嚴嵩成為首輔。

進一步，結(jié)合SSR與貝葉斯濾波框架，得到球面單純形-徑向容積卡爾曼濾波器。表1總結(jié)了CKF和SSRCKF采樣點數(shù)與系統(tǒng)狀態(tài)維度之間的關(guān)系。

表1 SR和SSR采樣點數(shù)的比較

3 球面單純形—徑向容積卡爾曼純角度跟蹤和純距離跟蹤濾波器

純角度跟蹤是利用被傳感器獲得被跟蹤目標的角度信息，但不能得到目標的距離信息，這種目標跟蹤的結(jié)果具有唯一性［17～19］。

標系下建立目標純角度跟蹤系統(tǒng)的數(shù)學(xué)模型：狀態(tài)方程與式（1）相同。

量測方程：

其中(x，y，z) 是目標的直角坐標，θk，φk分別表示雷達所測的方位角和高低角，系統(tǒng)噪聲wk-1和量測噪聲vk均為相互獨立的零均值高斯白噪聲。

根據(jù)式（23）確定球面單純形—徑向容積卡爾曼純角度跟蹤算法在k時刻的容積采樣點及相應(yīng)權(quán)值為

i=1，2，…，n，n為系統(tǒng)的狀態(tài)維度，ai為由式（7）確定的n-單純形的n+1個頂點構(gòu)成的向量，和P分別為k-1時刻的后驗均值和協(xié)方差。則具體的濾波算法如下。

預(yù)測：

采樣點的預(yù)測和系統(tǒng)狀態(tài)的預(yù)測分別為

根據(jù)一步預(yù)測，再次利用三階SSR產(chǎn)生新的容積點集。

更新：

純角度觀測向量預(yù)測為

上述式（1）和式（23）～（35）構(gòu)成了完整的球面單純形—徑向容積卡爾曼純角度跟蹤濾波算法。

在純距離目標跟蹤模型下，量測系統(tǒng)能獲取的量測信息只有距離，相較于純角度跟蹤而言可觀測程度更低，對濾波器的性能要更高［20］，利用SSRCKF解決純距離目標跟蹤問題有更長遠的意義。

在直角坐標系下建立目標純角度跟蹤系統(tǒng)的數(shù)學(xué)模型：狀態(tài)方程與式（1）相同。

量測方程為

其中(x0，y0)和(xk，yk)分布為二維空間中觀測雷達的位置和k時刻目標所在的位置。

由于在球面單純形—徑向容積卡爾曼純角度跟蹤算法的基礎(chǔ)上，只要注意量測方程的預(yù)測和更新，球面單純形—徑向容積卡爾曼純角度跟蹤算法簡單明了，這里不再進行詳細的推導(dǎo)說明。

4 仿真分析

本部分主要給出兩種不同場景下的仿真從而驗證上文中提出濾波器的正確性。首先，比較二維系統(tǒng)中EKF純距離跟蹤、UKF純距離跟蹤、CKF純距離跟蹤、SSRCKF純距離跟蹤等不同濾波算法對CA運動目標的跟蹤效果；其次，在三維系統(tǒng)中比較以上不同的濾波算法對CA運動目標的純角度跟蹤的效果。

4.1 球面單純形—徑向容積卡爾曼純角度跟蹤

仿真場景一：在三維空間中，目標運動模型為CA模型，掃描周期T=1s總的采樣次數(shù)是100/T，用向量描述系統(tǒng)狀態(tài)，其中x、y和z分別表示目標在x軸、y軸和z軸上的位置分量，、和分別表示目標在x軸、y軸和z軸上的速度分量，、和分別表示目標在x軸、y軸和z軸上的加速度分量，觀測雷達的位置在原點，該雷達只能接收到目標的角度信息（即方位角和高低角），則運動目標在直角坐標系中的數(shù)學(xué)模型可以表示為

其中，系統(tǒng)噪聲wk-1和量測噪聲vk均為相互獨立的零均值的高斯白噪聲，其對應(yīng)的方差分別為Qk=diag([0.01，0.02，0.02]T)，Rk=diag([0.002，0.002]T)，目標的初始狀態(tài)設(shè)置為X0=[10，10，3，1000，10，-3，100，50，-5]T，G(k)為噪聲驅(qū)動矩陣，F(xiàn)為狀態(tài)轉(zhuǎn)移矩陣。

UKF算法中的參數(shù)設(shè)定分別為系統(tǒng)狀態(tài)維度L=9，自由調(diào)節(jié)參數(shù)k=0，控制采樣點的分布狀態(tài)α=1，非負權(quán)系數(shù)β=2，縮放比例參數(shù)λ=3-L。

在該仿真場景下分別對非線性濾波算法EKF、UKF、CKF以及SSRCKF進行仿真實驗比較，仿真過程中，誤差分析公式為

其中，xij和ijk分別表示xi和i第j次采樣值。

圖1給出了該仿真場景下目標的實際運動軌跡以及分別采用EKF、UKF、CKF、SSRCKF四種純角度濾波算法得到的目標運動軌跡。圖2為該場景四種算法濾波后得到的目標位置的誤差分析曲線。結(jié)合圖1和圖2，可以很明顯地看出在三維空間對目標的純角度跟蹤中球面單純形—徑向容積卡爾曼純角度跟蹤具有很大的優(yōu)勢。由于EKF的局部線性化，不適用與強非線性系統(tǒng)，所以EKF純角度濾波器發(fā)散，其濾波效果最差。相較于EKF純角度濾波器而言，UKF純角度濾波器的濾波精度收斂，但由于UKF在高維系統(tǒng)（此時的系統(tǒng)維度為n=9）中濾波精度會下降，所以UKF純角度濾波器的誤差曲線在CKF純角度濾波器之上，這證明了CKF在高維非線性系統(tǒng)的優(yōu)勢。誤差最小的算法是本文提出的SSRCKF純角度濾波算法，且此時估計軌跡與真實軌跡幾乎重合，這就驗證了在系統(tǒng)狀態(tài)維度更高，非線性更強的系統(tǒng)，該濾波器較其他純角度濾波器在濾波精度上具有更大的優(yōu)勢，同時更進一步說明了SSRCKF算法的優(yōu)越性。

圖1 場景一中目標的真實軌跡與濾波軌跡

圖2 場景一中目標位置的誤差分析曲線

另一方面，由于此時系統(tǒng)維度n=9，由表1可知當n≥7時，SSRCKF的采樣點會少于CKF，且n越大，兩者采樣點個數(shù)相差越大，由此，SSRCKF純角度濾波器的計算復(fù)雜度得到了有效的降低。

4.2 球面單純形—徑向容積卡爾曼純距離跟蹤

仿真場景二：在二維空間中，選擇CA模型作為目標運動模型，掃描周期T=1s總的采樣次數(shù)是60/T，用向量描述系統(tǒng)狀態(tài)，其中x和y分別表示目標在x軸和y軸上的位置分量，和分別表示目標在x軸和y軸上的速度分量，和分別表示目標在x軸和y軸上的加速度分量，假設(shè)觀測雷達的位置為x0=200，y0=300，該雷達只能接收到目標的距離信息，則運動目標在直角坐標系中的數(shù)學(xué)模型可以表示為

其中，系統(tǒng)噪聲wk-1和量測噪聲vk均為相互獨立的零均值高斯白噪聲，其對應(yīng)的方差分別為Qk=diag([0.001，0.001]T)，Rk=0.01，目標的初始狀態(tài)為，噪聲驅(qū)動矩陣G(k)和狀態(tài)轉(zhuǎn)移矩陣F分別為

其中，UKF算法中的參數(shù)設(shè)定為系統(tǒng)狀態(tài)維度L=6，自由調(diào)節(jié)參數(shù)k=0，控制采樣點的分布狀態(tài)α=1，非負權(quán)系數(shù)β=2，縮放比例參數(shù)λ=3-L。

圖3給出的是場景二下的目標的實際運動軌跡以及分別采用EKF純距離跟蹤、UKF純距離跟蹤、CKF純距離跟蹤、SSRCKF純距離跟蹤四種算法得到的目標運動軌跡。圖4為場景二中四種算法的目標位置誤差分析曲線。

觀察和分析圖3和圖4可得，在高維非線性系統(tǒng)中（n=6），幾種非線性純距離跟蹤算法的濾波性能的排序為EKF

圖3 場景二中的目標真實軌跡與濾波軌跡

圖4 場景二中目標位置的誤差分析曲線

綜合以上兩個仿真，在系統(tǒng)狀態(tài)維度更高，非線性更強的系統(tǒng)中相對于其他的非線性純角度濾波器和純距離濾波器而言，SSRCKF純角度濾波器和SSRCKF純距離濾波器不僅濾波精度更高，估計誤差更小，而且計算復(fù)雜度更低，效率更好。但另一方面，純角度和純距離濾波器存在的一旦系統(tǒng)狀態(tài)發(fā)生突變就無法精確跟蹤的問題和進一步減少計算量的目標也將是未來研究工作的重心所在。

5 結(jié)語

本文針對純角度跟蹤和純距離跟蹤所具有的部分可觀測性和高度非線性，結(jié)合SSRCKF提出了球面單純形—徑向容積卡爾曼純角度和純距離跟蹤濾波器，并在高維非線性系統(tǒng)中與其他非線性純角度和純距離跟蹤濾波器進行了比較，驗證了所提算法的優(yōu)越性。仿真結(jié)果表明，該算法在提高濾波精度的同時還減少了計算量，這一點在只能獲取不完全狀態(tài)信息的軍事領(lǐng)域具有重要的理論意義和應(yīng)用前景。