高階容積卡爾曼濾波及其在目標(biāo)跟蹤中的應(yīng)用

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

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

?

高階容積卡爾曼濾波及其在目標(biāo)跟蹤中的應(yīng)用

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

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

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

關(guān)鍵詞：高階容積卡爾曼濾波；目標(biāo)跟蹤；非線性系統(tǒng)；貝葉斯估計；球面相徑規(guī)則；容積規(guī)則

網(wǎng)絡(luò)出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20160127.1137.034.html

本文針對CKF估計精度有限的問題提出了高階容積卡爾曼濾波，推導(dǎo)了五階球面-相徑容積規(guī)則來近似高斯權(quán)值積分，在五階容積規(guī)則的基礎(chǔ)上建立高階容積卡爾曼濾波算法模型和計算流程，并應(yīng)用于三維空間中的機(jī)動目標(biāo)跟蹤系統(tǒng)。

1高階球面-相徑容積規(guī)則

1.1高斯權(quán)值積分近似方法

考慮如下非線性離散時間動力學(xué)系統(tǒng)：

(1)

(2)

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

在容積規(guī)則中，考慮如下積分：

(3)

令x=rs，則式(3)可轉(zhuǎn)換到球面-相徑的坐標(biāo)系統(tǒng)中，有

(4)

(5)

式中：ri和wr,i為計算相徑積分的點和對應(yīng)的權(quán)值，sj和ws,j為計算球面積分的點和對應(yīng)的權(quán)值，Nr和Ns分別是相徑積分和球面積分的點數(shù)。則高斯權(quán)值積分式可轉(zhuǎn)換為

(6)

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

1.2五階球面-相徑容積規(guī)則

文獻(xiàn)[15-16]對球面規(guī)則進(jìn)行了詳細(xì)推導(dǎo)。由Genz積分方法[18-19]，五階球面規(guī)則滿足:

(7)

(8)

(9)

(10)

(11)

(12)

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

(13)

(14)

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

(15)

(16)

2高階容積卡爾曼濾波

1)計算容積點

(17)

(18)

(19)

2)計算經(jīng)狀態(tài)方程傳遞后的容積點

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

(20)

3)計算k+1時刻的狀態(tài)預(yù)測值

(21)

其中權(quán)值wi分別為

(22)

4)估計k+1時刻的狀態(tài)誤差協(xié)方差陣

(23)

5)計算更新后的狀態(tài)容積點

(24)

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

(25)

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

(26)

(27)

(28)

(29)

10)計算k+1時刻的狀態(tài)估計值

(30)

11)估計k+1時刻的狀態(tài)誤差協(xié)方差陣

(31)

3數(shù)值仿真

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

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

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

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

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

式中σr、σθ、σβ是雷達(dá)測量噪聲的標(biāo)準(zhǔn)差。

RMSEpos(k)=

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

表1　仿真參數(shù)設(shè)置

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

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

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

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

表2給出了三種濾波方法對目標(biāo)位置、速度、轉(zhuǎn)速的均方根誤差均值以及算法的容積點數(shù)和平均執(zhí)行時間。

圖4　目標(biāo)轉(zhuǎn)速的估計均方根誤差Fig.4　The RMSE of the target turn rate estimation

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

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

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

4結(jié)論

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

1)對于三維空間中的機(jī)動目標(biāo)跟蹤問題，高階CKF可獲得優(yōu)于UKF和傳統(tǒng)CKF的估計精度和數(shù)值穩(wěn)定性，表明高階球面-相徑容積規(guī)則可以較大提高高斯權(quán)值積分的計算精度，進(jìn)而提高濾波精度，體現(xiàn)了高階球面-相徑容積規(guī)則在估計精度方面的優(yōu)越性。

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

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

參考文獻(xiàn)：

[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]王宏建, 徐金龍, 么洪飛, 等. 基于二階差分濾波器的水下目標(biāo)純方位角跟蹤[J]. 哈爾濱工程大學(xué)學(xué)報, 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.

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

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

doi:10.11990/jheu.201412079

中圖分類號：V557

文獻(xiàn)標(biāo)志碼：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

網(wǎng)絡(luò)出版日期：2016-01-27.

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