基于SSRCKF的純角度和純距離跟蹤濾波器*

張星星 劉樂文

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

1 引言

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

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

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

非線性高斯系統建模如下所示。

狀態方程：

量測方程：

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

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

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

分解可得球面積分：

徑向積分為

1）球面單純形準則

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

則向量的各個構成元素定義為

{ai}滿足中心對稱性，可選作積分點，再結合球面容積規則推導如下。

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

同時，球面積分可由數值積分近似

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

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

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

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

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

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

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

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

所以，三階徑向準則為

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

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

進一步，結合SSR與貝葉斯濾波框架，得到球面單純形-徑向容積卡爾曼濾波器。表1總結了CKF和SSRCKF采樣點數與系統狀態維度之間的關系。

表1 SR和SSR采樣點數的比較

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

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

標系下建立目標純角度跟蹤系統的數學模型：狀態方程與式（1）相同。

量測方程：

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

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

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

預測：

采樣點的預測和系統狀態的預測分別為

根據一步預測，再次利用三階SSR產生新的容積點集。

更新：

純角度觀測向量預測為

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

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

在直角坐標系下建立目標純角度跟蹤系統的數學模型：狀態方程與式（1）相同。

量測方程為

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

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

4 仿真分析

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

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

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

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

UKF算法中的參數設定分別為系統狀態維度L=9，自由調節參數k=0，控制采樣點的分布狀態α=1，非負權系數β=2，縮放比例參數λ=3-L。

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

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

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

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

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

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

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

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

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

其中，UKF算法中的參數設定為系統狀態維度L=6，自由調節參數k=0，控制采樣點的分布狀態α=1，非負權系數β=2，縮放比例參數λ=3-L。

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

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

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

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

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

5 結語

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