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

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

?

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

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

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

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

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

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

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

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

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

考慮如下非線(xiàn)性離散時(shí)間動(dòng)力學(xué)系統(tǒng)：

(1)

(2)

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

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

(3)

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

(4)

(5)

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

(6)

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

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

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

(7)

(8)

(9)

(10)

(11)

(12)

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

(13)

(14)

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

(15)

(16)

2高階容積卡爾曼濾波

1)計(jì)算容積點(diǎn)

(17)

(18)

(19)

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

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

(20)

3)計(jì)算k+1時(shí)刻的狀態(tài)預(yù)測(cè)值

(21)

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

(22)

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

(23)

5)計(jì)算更新后的狀態(tài)容積點(diǎn)

(24)

6)計(jì)算經(jīng)過(guò)測(cè)量方程傳遞的容積點(diǎn)

(25)

7)計(jì)算k+1時(shí)刻的測(cè)量預(yù)測(cè)值

(26)

(27)

(28)

(29)

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

(30)

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

(31)

3數(shù)值仿真

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

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

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

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

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

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

RMSEpos(k)=

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

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

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

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

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

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

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

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

4結(jié)論

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

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

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

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

參考文獻(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é)報(bào), 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.

基金項(xiàng)目：國(guó)家自然科學(xué)基金項(xiàng)目(61304236).

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

doi:10.11990/jheu.201412079

中圖分類(lèi)號(hào)：V557

文獻(xiàn)標(biāo)志碼：A

文章編號(hào)：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)師.