基于方差平方根CKF算法在魚雷跟蹤中的應用

劉清慧，　高　江，　鄧南明

（中國人民解放軍91388部隊， 廣東 湛江， 524022）

基于方差平方根CKF算法在魚雷跟蹤中的應用

劉清慧，高江，鄧南明

（中國人民解放軍91388部隊， 廣東 湛江， 524022）

針對現有無跡卡爾曼濾波（UKF）算法在高維系統中易出現協方差非正定導致濾波不穩定甚至發散的問題，探討了基于方差平方根的容積卡爾曼濾波（CKF）算法在魚雷目標跟蹤中的應用。該算法首先基于Cubature準則，獲得一組具有相同權重的Cubature點， 然后經過非線性系統方程將該點集進行轉換產生新的點， 以此預測下一時刻系統的狀態， 并在濾波更新過程中通過傳播狀態的方差平方根， 確保了方差矩陣的對稱性和正定性。仿真結果表明， CKF的濾波精度要高于UKF。

魚雷； 目標跟蹤； 方差平方根； 無跡卡爾曼濾波； 容積卡爾曼濾波

0　引言

魚雷作為一種現代化的水中武器， 是水面艦艇和潛艇面臨的主要水下威脅。對來襲魚雷目標進行跟蹤識別， 是反魚雷技術需解決的首要難題，現有技術僅靠魚雷報警聲吶測得的純方位信息難以實現對魚雷目標的精確定位跟蹤［1-2］。

為此， 很多學者提出了基于魚雷的運動學模型以及魚雷報警聲吶測得的信息， 構造魚雷目標跟蹤的狀態空間模型， 并建立可應用于該模型的濾波算法， 以提高魚雷目標跟蹤精度， 實現對目標狀態準確估計。擴展卡爾曼濾波（extend Kalman filter， EKF）是最早在工程中實現運用的非線性濾波算法［3］， 其主要思想是利用1階泰勒級數將非線性系統方程模型在濾波狀態預測值處進行展開，從而使得線性化系統模型仍能應用于線性卡爾曼濾波中， 但是在強非線性模型下高階項的忽略會使得EKF濾波估計精度不高， 同時非線性函數泰勒級數展開中雅克比矩陣的求取， 使得EKF算法計算量增大， 復雜度提高［4］。

基于此， Julier和Uhlman基于無跡變換（unscented transformation， UT）思想， 提出了一種新的濾波算法——無跡卡爾曼濾波（unscented Kalman filter， UKF）［5］。它通過具有不同權值的2n+1個確定性Sigma采樣點， 并利用上述Sigma點經過非線性系統方程轉換后產生新的點來估計下一時刻系統狀態的均值和方差， 理論推導證明UKF濾波估計精度能夠達到非線性系統泰勒級數展開的3階項， 因此UKF精度高于EKF， 計算量卻與EKF相同。但是， UKF中Sigma點集的選取缺乏數學推導依據， 且在高維數系統中UKF的Sigma點權值容易出現負值， 導致濾波過程中的協方差矩陣非正定， 出現濾波數值不穩定性，致使算法中斷運行。

為了克服UKF算法中存在的問題， 很多學者開始研究數學推導更為嚴謹的容積卡爾曼濾波（cubature Kalman filter， CKF）［6］。CKF算法依據理論依據更為嚴格的Cubature準則獲得2n個同等權值的Cubature點， 同樣利用這些點來估計下一時刻系統狀態。

文中針對魚雷目標跟蹤問題， 在CKF的基礎上通過利用狀態的方差平方根替代方差在濾波過程中計算， 確保方差矩陣的對稱性和正定性， 解決了UKF在非線性估計中易出現協方差非正定導致濾波不穩定甚至發散的問題。最后利用CKF算法與UKF算法進行仿真對比分析， 結果表明CKF算法的濾波精度要高于UKF。

1　高斯濾波器的一般形式

假設系統離散非線性系統模型［7］

式中： F和H為已知任意函數； x為系統狀態向量； z為量測向量； v和w分別為系統噪聲和量測噪聲， 均滿足零均值高斯分布， 即

貝葉斯濾波是應用廣泛的估計算法， 根據系統的觀測序列矢量Zk-1=｛z，…，z｝迭代計算系

1） 時間更新

其中， d為積分符號。

2） 觀測更新

2　UKF算法

UKF算法的核心思想是UT變換。該算法首先進行UT變換將系統狀態離散成一組Simga點集，然后利用此點集計算式（2）～式（5）得到濾波算法， 每個濾波周期內進行時間更新和量測更新。根據上述分析， 得UKF算法的簡要步驟如下。

1） 初始化

2） 計算Sigma點集（k=1，2，…）

繼續求解上述Sigma點各自相應的權值為

式中， n和κ必須滿足

3） 時間更新

4） 再次計算Sigma點集（k=1，2，…）

5） 觀測更新

從而可得狀態估計值和狀態誤差協方差估計值分別為

以上即為UKF算法的計算步驟。從上述分析可知， UKF中Sigma點集的選取缺乏嚴格數學推導依據， 且從式（9）可知， 當系統維數（維數n≥4）較高時， UKF中的自由調節參數κ將會小于零， 又據式（4）可知， 某些Sigma點的權值也將小于零， UKF濾波過程中的協方差非正定， 從而導致濾波數值不穩定性， 出現算法中斷運行情況發生。

3　基于方差平方根的CKF算法

針對UKF算法缺乏嚴格數學推導的不足， 文中提出把CKF算法應用在魚雷目標跟蹤中。該算法根據理論依據更為嚴格的Cubature準則， 通過2n個同等權值的Cubature點經非線性系統方程轉換后產生新的點來給出下一時刻系統狀態的預測， 與UKF相同， CKF算法也不需對非線性模型線性化， 但是CKF算法比UKF算法使用的采用點更少。同時， 文中在濾波過程中利用狀態的方差平方根來替代方差完成算法的迭代運算， 確保了方差矩陣的對稱性和正定性， 解決了UKF在非線性估計中協方差出現非正定的問題。

根據上述分析， 可得基于方差平方根的CKF算法的簡要步驟如下。

文奇的“奇點”理論提出后，并沒有立即獲得人們的廣泛認同，只是得到一部分人的支持，如庫茲韋爾。庫茲韋爾于1990年出版《智能機器時代》(The Age of Intelligent Machines)一書，認為隨著計算機性能的不斷提升，未來經過足夠多的時間，人類將會創造比他自身更聰明的實體[27]。庫茲威爾的推斷是基于計算能力的指數級增長及技術的加速循環規則。

3） 基于Spherical-Radial Cubature準則， 計算Cubature點集ξi（i=1，2，…）及其權值

其中， ［1］i表示單位矩陣的第i列。

4） 時間更新

5） 利用Cholesky分解誤差協方差

6） 再次計算Cubature點集ξi（i=1，2，…）及其權值

根據以上式子， 可得基于方差平方根的CKF算法的增益

從而可得狀態估計值和狀態誤差協方差估計值分別為

以上即為CKF算法的計算步驟。從上述分析可知， CKF與UKF的濾波過程類似， 都是通過一組具有權重的點集經過非線性轉換來計算系統狀態的預測， 其優點是不僅避免了線性化處理非線性模型， 而且不局限于系統模型的具體非線性方程。同時， CKF具有比UKF更優越的濾波性能。首先， CKF因其經過嚴格的Cubature準則數學推導， 所以在理論上有一定的保證； 其次， 由式（23）可知， Cubature點集的權值均為1/m， 該值始終大于0， 提高了濾波的數值穩定性和濾波精度； 最后CKF再利用式（22）和式（28）， 確保了CKF算法中協方差的正定性， 解決了UKF在高維系統中存在的問題， 實現了濾波器建立的優化設計。

4　仿真結果與性能分析

利用Matlab軟件平臺對上述方差平方根的CKF算法以及UKF算法進行仿真驗證， 假設魚雷目標的非線性運動學方程［8］

同時， 仿真試驗中假設魚雷報警聲吶對魚雷目標進行測量， 設兩者斜距為r， 方位角為θ， 則魚雷跟蹤模型的量測方程［8］

同時， 設置系統的真實初始值

仿真時間200 s， 進行50次Monte Carlo仿真。采用均方根誤差（root mean square error， RMSE）來評估UKF和EKF算法的估計效果方法。定義k時刻位置、速度和角速度的均方根誤差

其中， N為Monte Carlo仿真次數。

圖1～圖3分別給出了50次Monte Carlo位置、速度和角速度的仿真結果。從圖中可知， UKF誤差曲線在開始的0～50 s中沒有結果輸出， 這是由于系統狀態維數n=5， 根據式（9）可知， UKF的自由調節參數κ=-2， 從而導致某些Sigma點權值出現負值， 進一步導致協方差矩陣的非正當，造成算法中斷運行， UKF濾波估計無結果輸出。同時可知， 基于方差平方根的CKF算法由于利用狀態的方差平方根來替代方差完成算法的迭代運算， 確保了方差矩陣的對稱性和正定性， 故CKF濾波估計結果曲線沒有出現算法中斷現象， 雖然由于系統噪聲和觀測噪聲的存在， 使得基于方差平方根的CKF誤差曲線存在振蕩情況， 但是都能穩定在一定范圍內， 達到了濾波估計效果。

圖1　基于容積卡爾曼濾波算法和無跡卡爾曼濾波算法得到的位置均方根誤差曲線Fig. 1　Curves of root mean square error(RMSE) of position versus time with cubature Kalman filter (CKF) algorthm and unscented Kalman filter (UKF) algorthm

圖2　基于CKF和UKF算法得到的速度均方根誤差Fig. 2　Curves of RMSE of velocity versus time with CKF algorthm and UKF algorthm

圖3　基于CKF和UKF算法的角速度均方根誤差曲線Fig. 3　Curves of RMSE of angular velocity versus time with CKF algorthm and UKF algorthm

綜上所述， 無論是在濾波穩定性以及收斂精度上CKF都遠遠優于UKF， 因此證實了CKF算法性能上優于UKF。

5　結束語

針對于魚雷目標跟蹤問題， 文中提出了基于方差平方根的CKF算法， 將該算法濾波性能與UKF算法進行比較， 并進行了仿真試驗驗證。通過仿真結果分析可知， CKF算法估計精度優于UKF算法。文中的研究對工程實際應用具有一定的參考價值。

［1］ 陳敬軍. 國外反艦魚雷的現狀與發展趨勢［J］. 聲學技術， 2013， 32（2）： 164-170.

Chen Jing-jun. The Status Quo and Development Trend of Overseas Anti-surface Ship Torpedo［J］. Technical Acoustics， 2013， 32（2）： 164-170.

［2］ 肖碧琴， 袁富宇， 苗艷. 魚雷目標定位跟蹤技術［J］. 火力與指揮控制， 2014， 39（1）： 103-105.

Xiao Bi-qin， Yuan Fu-yu， Miao Yan. Review of Localization Algorithms for Torpedo-Target［J］. Fire Control & Command Control， 2014， 39（1）： 103-105.

［3］ Athans M， Wishner R P， Bertolini A. Suboptimal State Estimation for Continuous Time Nonlinear Systems from Discrete Noisy Measurements［J］. IEEE Transactions on Automatic Control， 1968， 5（13）： 504-514.

［4］ Kalman R E. A New Approach to Linear Filtering and Pediction Poblems［J］. Journal of Basic Engineering， 1960，1（82）： 34-45.

［5］ Julier S J， Uhlman J K. A New Extension of the Kalman Filter to Nonlinear Systems［J］. Proceedings of the Society of Photo-Optical Instrumentation Engineers， 1997， 3068（1）： 182-193.

［6］ Arasaratnam I， Haykin S. Cubature Kalman Filters［J］. IEEE Transactions on Automatic Control， 2009， 54（6）：1254-1269.

［7］ 占榮輝， 張軍. 非線性濾波理論與目標跟蹤應用［M］.北京： 國防工業出版社， 2013： 256-257.

［8］ 高文娟， 李亞安， 陳曉， 等. 基于交互式多模型的水下機動目標跟蹤［J］. 魚雷技術， 2015， 23（3）： 196-201.

Gao Wen-juan， Li Ya-an， Chen Xiao， et al. Application of IMM to Underwater Maneuver Target Tracking［J］. Torpedo Technology， 2015， 23（3）： 196-201.

（責任編輯： 楊力軍）

Application of CKF Algorithm Based on Square Root
of Variance to Torpedo Tracking

LIU Qing-hui，GAO Jiang，DENG Nan-ming
（91388thUnit， The People′s Liberation Army of China， Zhanjiang 524022， China）

The existing unscented Kalman filter（UKF） algorithm results in non-definite covariance easily in high-dimensional system， which leads to instability of filter and even divergence. In this paper， the application of cubature Kalman filter（CKF） algorithm based on square root of variances to underwater target tracking is discussed. This algorithm can achieve a group of cubature points with same weight based on the cubature principle， transform the points set into the new points by non-linear system equation in order to predict the system state at next time， and ensure symmetry and positive definiteness of the covariance matrix by using the square root of the variances of propagation state. Simulation results show that CKF is better than UKF in estimation precision.

torpedo； target tracking； square root of variance； unscented Kalman filter（UKF）； cubature Kalman filter（CKF）

TJ630.34； TN953

A

1673-1948（2015）06-0428-05

10.11993/j.issn.1673-1948.2015.06.007

2015-09-06；

2015-10-30.

劉清慧（1974-）， 男， 碩士， 工程師， 主要從事水中兵器試驗研究.