高階容積卡爾曼濾波及其在目標跟蹤中的應用

張龍, 崔乃剛, 楊峰，路菲，盧寶剛

(1.哈爾濱工業大學 航天工程系, 黑龍江 哈爾濱 150001; 2.北京航天長征飛行器研究所, 北京 100076)

?

高階容積卡爾曼濾波及其在目標跟蹤中的應用

張龍1, 崔乃剛1, 楊峰1，路菲1，盧寶剛2

(1.哈爾濱工業大學 航天工程系, 黑龍江 哈爾濱 150001; 2.北京航天長征飛行器研究所, 北京 100076)

摘要：針對傳統的容積卡爾曼濾波(CKF)估計精度有限的問題，提出了一種基于任意階容積規則的高階容積卡爾曼濾波(HCKF)方法并應用于機動目標跟蹤問題。傳統的CKF采用三階球面-相徑容積規則，可獲得優于其他非線性濾波如不敏卡爾曼濾波(UKF)的估計精度和數值穩定性 。為了進一步提高CKF的估計精度，在基于點的高斯近似濾波框架下，分別使用Genz積分方法和矩匹配法推導出任意階的球面規則和相徑規則，以此構造高階球面-相徑容積規則來計算高斯型積分，并建立高階容積卡爾曼濾波算法。將提出的HCKF算法應用于機動目標跟蹤問題中并進行數值仿真。仿真結果表明，相對于傳統容積卡爾曼濾波，高階容積卡爾曼濾波對目標位置和速度估計的精度分別提高了11%和24%，可獲得更高的估計精度。

關鍵詞：高階容積卡爾曼濾波；目標跟蹤；非線性系統；貝葉斯估計；球面相徑規則；容積規則

網絡出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20160127.1137.034.html

本文針對CKF估計精度有限的問題提出了高階容積卡爾曼濾波，推導了五階球面-相徑容積規則來近似高斯權值積分，在五階容積規則的基礎上建立高階容積卡爾曼濾波算法模型和計算流程，并應用于三維空間中的機動目標跟蹤系統。

1高階球面-相徑容積規則

1.1高斯權值積分近似方法

考慮如下非線性離散時間動力學系統：

(1)

(2)

式中：狀態向量xk∈Rn，測量向量yk∈Rm；vk-1和wk是相互獨立的零均值系統高斯白噪聲和測量高斯白噪聲，方差分別為Qk-1和Rk。

在容積規則中，考慮如下積分：

(3)

令x=rs，則式(3)可轉換到球面-相徑的坐標系統中，有

(4)

(5)

式中：ri和wr,i為計算相徑積分的點和對應的權值，sj和ws,j為計算球面積分的點和對應的權值，Nr和Ns分別是相徑積分和球面積分的點數。則高斯權值積分式可轉換為

(6)

采用不同階數的容積規則，并根據該規則選取不同容積點和權值，式(6)可得到不同的積分結果。目前，三階球面-相徑容積規則已經得到廣泛的關注，下面給出五階球面-相徑容積規則。

1.2五階球面-相徑容積規則

文獻[15-16]對球面規則進行了詳細推導。由Genz積分方法[18-19]，五階球面規則滿足:

(7)

(8)

(9)

(10)

(11)

(12)

選擇r1為自由變量并令r1=0，解式(12)，可得五階相徑規則的點和權值為

(13)

(14)

結合式(6)、(7)、(13)和(14)，可以得到滿足分布為x,～N(x;0,I)的五階球面-相徑容積規則

(15)

(16)

2高階容積卡爾曼濾波

1)計算容積點

(17)

(18)

(19)

2)計算經狀態方程傳遞后的容積點

建設單位作為投資主體，在組織招投標活動中，采取一系列手段來保護自身利益，有些明招暗定虛假招標，給參與投標活動的單位帶來極大損失，主要表現在以下幾個方面：

(20)

3)計算k+1時刻的狀態預測值

(21)

其中權值wi分別為

(22)

4)估計k+1時刻的狀態誤差協方差陣

(23)

5)計算更新后的狀態容積點

(24)

6)計算經過測量方程傳遞的容積點

(25)

7)計算k+1時刻的測量預測值

(26)

(27)

(28)

(29)

10)計算k+1時刻的狀態估計值

(30)

11)估計k+1時刻的狀態誤差協方差陣

(31)

3數值仿真

為了表明高階容積卡爾曼濾波算法的優越性能，將高階容積濾波(五階CKF)應用到三維空間中的目標跟蹤系統中，并與不敏卡爾曼濾波(UKF)和傳統容積濾波(三階CKF)進行對比。

定義地面坐標系o-xyz的原點位于地面，ox指向東向，oy指向北向，oz與ox和oy成右手系，指向天向。假設目標在坐標系o-xyz中等高度機動飛行，目標跟蹤系統動力學模型如下[9,16-17]：

Q=diag(M1,M1,M1,M2)

假設測量雷達固定在地面坐標系原點，獲取對目標的相對距離r、高低角η和方位角θ信息。假設目標的測量向量為y=[rθη]T，滿足：

式中：w為測量噪聲，其協方差陣R為

式中σr、σθ、σβ是雷達測量噪聲的標準差。

RMSEpos(k)=

圖1為目標飛行的平面軌跡和雷達位置，“I”表示軌跡起點，“F”表示軌跡終點，“★”為雷達位置。圖2～4分別給出了三種濾波方法對目標位置估計均方差、速度估計均方差和轉速估計均方差的對比結果。由于0～40 s內三種濾波方法得到估計結果差別不大，故沒有顯示在圖2～4中。由仿真結果可知，三階CKF與UKF的估計精度較為接近并且三階CKF精度稍高于UKF。五階CKF的估計精度均高于UKF和三階CKF，且數值穩定性更好。說明高階球面-相徑容積規則可以較大提高高斯權值積分的計算精度，進而提高濾波精度，體現了高階球面-相徑容積規則在估計精度方面的優越性。

表1　仿真參數設置

圖1　目標飛行的平面軌跡和雷達位置Fig.1　The plane trajectory of the target and the radar's position

圖2　目標位置的估計均方根誤差Fig.2　The RMSE of the target position estimation

圖3　目標速度的估計均方根誤差Fig.3　The RMSE of the target velocity estimation

圖5對比了10次隨機打靶中各濾波方法計算所消耗的CPU時間。其中三階CKF采用的容積點數最少，為14個容積點，因此算法計算量最小、運行時間最短。而基于高階球面-相徑容積規則的五階CKF由于在時間更新和測量更新計算中采用99個容積點，提高了計算復雜度，因此算法運算時間最長。

表2給出了三種濾波方法對目標位置、速度、轉速的均方根誤差均值以及算法的容積點數和平均執行時間。

圖4　目標轉速的估計均方根誤差Fig.4　The RMSE of the target turn rate estimation

圖5　3種濾波方法的執行時間Fig.5　Execution time of the three methods

參數項UKF三階CKF五階CKF位置精度/m17.3816.6614.82速度精度/(m·s-1)8.557.956.04轉速精度/((°)·s-1)0.990.920.86積分點數151499平均執行時間/s0.0420.0360.242

可以清楚看到，五階CKF的估計精度明顯優于其他兩種濾波方法。但是相對于UKF和三階CKF，五階CKF采用的容積點數較多，運算時間較長。由此可知，五階CKF在取得較高估計精度的同時，計算量的代價相對較大，特別是在系統狀態向量維數n較高的情況下，其使用的容積點數幾乎是三階CKF的n倍。因此，五階CKF適用于濾波時間間隔較長或對濾波實時性要求不高的高精度濾波系統中。此次仿真中五階CKF算法的運算時間與濾波時間間隔相差較大，可以滿足實時性需求。

4結論

本文針對傳統容積卡爾曼濾波估計精度有限的問題，在基于點的高斯近似濾波框架下，提出高階容積卡爾曼濾波算法HCKF。該算法采用Genz積分方法和矩匹配法分別推導出任意階的球面規則和相徑規則，以此構造高階球面-相徑容積規則。通過數學仿真可以得出以下結論：

1)對于三維空間中的機動目標跟蹤問題，高階CKF可獲得優于UKF和傳統CKF的估計精度和數值穩定性，表明高階球面-相徑容積規則可以較大提高高斯權值積分的計算精度，進而提高濾波精度，體現了高階球面-相徑容積規則在估計精度方面的優越性。

2)由于HCKF采用的容積點數量為2n2+1(n為狀態向量維數)，可知當狀態向量維數較大時，采用的容積點較多，計算量相對CKF和UKF較大。因此，作者的下一個研究思路是通過降低狀態向量維數或觀測向量維數的方法在保持HCKF估計精度的同時明顯減小算法計算量。

3)本文研究的HCKF是基于五階球面-相徑容積規則。實際上，采用不同階數的球面規則和相徑規則，就可以得到不同精度的容積規則。因此，一個可行的研究思路是設計更高階的球面-相徑容積濾波算法并對其分析。

參考文獻：

[1]ITOK,XIONGKaiqi.Gaussianfiltersfornonlinearfilteringproblems[J].IEEEtransactionsonautomaticcontrol, 2000, 45(5): 910-927.

[2]FORBESJR.ExtendedKalmanfilterandsigmapointfilterapproachestoadaptivefiltering[C]//AIAAGuidance,Navigation,andControlConference.Toronto,Canada, 2010: 7748-7763.

[3]JULIERSJ,UHLMANNJK.Unscentedfilteringandnonlinearestimation[J].ProceedingsoftheIEEE, 2004, 92(3): 401-422.

[4]LIUXing,JIANGShoushan.ResearchontargettrackingbasedonunscentedKalmanfilter[J].Sensors&transducersjournal, 2013, 153(6): 13-21.

[5]王宏建, 徐金龍, 么洪飛, 等. 基于二階差分濾波器的水下目標純方位角跟蹤[J]. 哈爾濱工程大學學報, 2014, 35(1): 87-92.

WANGHongjian,XUJinlong,YAOHongfei,etal.Researchonsecondorderdivideddifferencefilteralgorithmforunderwatertargetbearing-onlytracking[J].JournalofHarbinEngineeringUniversity, 2014, 35(1): 87-92.

[6]ARASARATNAMI,HAYKINS,ELLIOTTRJ,Discrete-timenonlinearfilteringalgorithmsusinggauss-hermitequadrature[J].ProceedingsoftheIEEE, 2007, 95(5): 953-977.

[7]JIABin,XINMing,CHENGYang.SparseGauss-Hermitequadraturefilterwithapplicationtospacecraftattitudeestimation[J].Journalofguidance,control,anddynamics, 2011, 34(2): 367-379.

[8]JIABin,XINMing,CHENGYang.Sparse-gridquadraturenonlinearfiltering[J].Automatica, 2012, 48(2): 327-341.

[9]ARASARATNAMI,HAYKINS.CubatureKalmanfilters[J].IEEEtransactionsonautomaticcontrol, 2009, 54(6): 1254-1269.

[10]ARASARATNAMI,HAYKINS,HURD,TR.CubatureKalmanfilteringforcontinuous-discretesystems:theoryandsimulations[J].IEEEtransactionsonsignalprocessing, 2010, 58(10): 4977-4993.

[11]FERNNDEZ-PRADESC,VIL-VALLSJ.Bayesiannonlinearfilteringusingquadratureandcubaturerulesappliedtosensordatafusionforpositioning[C]//IEEEInternationalConferenceonCommunications(ICC).CapeTown,SouthAfrica, 2010: 1-5.

[12]PESONENH,PICHéR,Cubature-basedKalmanfiltersforpositioning[C]//7thWorkshoponPositioningNavigationandCommunication(WPNC).Dresden, 2010: 45-49.

[13]LIQiurong,SUNFeng.StrongtrackingcubatureKalmanfilteralgorithmforGPS/INSintegratednavigationsystem[C]//IEEEInternationalConferenceonMechatronicsandAutomation(ICMA).Takamatsu, 2013: 1113-1117.

[14]LIChao,GEQuanbo.SCKFforMAVattitudeestimation[C]//2011InternationalConferenceonMachineLearningandCybernetics.Guilin,China, 2011: 1313-1318.

[15]TAOSun,MINGXin.Hypersonicentryvehiclestateestimationusinghigh-degreecubatureKalmanfilter[C]//AIAAAtmosphericFlightMechanicsConference.Atlanta, 2014: 2383-2394.

[16]JIABin,XINMing,CHENGYang.Thehigh-degreecubatureKalmanfilter[C]//51stIEEEConferenceonDecisionandControl.Maui,USA, 2012: 4095-4100.

[17]JIABin,XINMing,CHENGYang.High-degreecubatureKalmanfilter[J].Automatica, 2013, 49(2): 510-518.

[18]GENZ,A.Fullysymmetricinterpolatoryrulesformultipleintegralsoverhyper-sphericalsurfaces[J].Journalofcomputationalandappliedmathematics, 2003, 157(1): 187-195.

[19]GENZA,MONAHANJ.AStochasticalgorithmforhigh-dimensionalintegralsoverunboundedregionswithGaussianweight[J].Journalofcomputationalandappliedmathematics, 1999, 112(1/2): 71-81.

收稿日期：2014-12-31.

基金項目：國家自然科學基金項目(61304236).

作者簡介：張龍(1987-), 男,博士研究生; 通信作者：張龍, E-mail: zhanglong_hit@163.com.

doi:10.11990/jheu.201412079

中圖分類號：V557

文獻標志碼：A

文章編號：1006-7043(2016)04-0573-06

High-degree cubature Kalman filter and its application in target tracking

ZHANG Long1，CUI Naigang1，YANG Feng1，LU Fei1，LU Baogang2

(1. Department of Astronautics Engineering, Harbin Institute of Technology, Harbin 150001, China； 2. Beijing Institute of Space Long March Vehicle, Beijing 100191, China)

Abstract：A high-order cubature Kalman filter (HCKF) based on the arbitrary-order cubature rule was proposed and applied to the maneuvering target tracking problem resulting from the limited precision of the conventional cubature Kalman filter (CKF). The conventional CKF, which employs the third-order spherical-radial cubature rule, can achieve better estimation precision and numerical stability than the other nonlinear filters, such as Unscented Kalman Filter(UKF). To further improve the estimation precision of CKF, in the framework of point-based Gaussian approximation filters, Genz's method and the moment-matching method were employed to deduce the arbitrary-order spherical rule and the radial rule, respectively. By combining the two rules, the high-order spherical-radial cubature rule was developed for computing Gaussian weighted integrals, and HCKF was proposed based on the high-order spherical-radial cubature rule. The proposed HCKF algorithm was applied to the maneuvering target tracking to test its performance. The simulation result shows that HCKF can achieve better estimation precision than conventional CKF, with the improvement of position and velocity estimates by 11% and 24% respectively.

Keywords：high-degree cubature Kalman filter; target tracking; nonlinear system; Bayesian filtering; spherical-radial rule; cubature rule

網絡出版日期：2016-01-27.

崔乃剛(1965-), 男,教授,博士生導師.