加性噪聲下增廣容積卡爾曼濾波及其目標(biāo)跟蹤應(yīng)用

劉 江 葉松慶

(中國科學(xué)院重慶綠色智能技術(shù)研究院 重慶 400714)

加性噪聲下增廣容積卡爾曼濾波及其目標(biāo)跟蹤應(yīng)用

劉 江 葉松慶

(中國科學(xué)院重慶綠色智能技術(shù)研究院 重慶 400714)

傳統(tǒng)容積卡爾曼濾波(CKF)有良好的濾波精度和較低的計(jì)算復(fù)雜度，使其廣泛被應(yīng)用于目標(biāo)跟蹤系統(tǒng)。但在高維非線性和波動(dòng)性大的目標(biāo)跟蹤系統(tǒng)中，3階和高階CKF分別存在濾波精度不足和穩(wěn)定性低的問題。為提高CKF的濾波精度并保證穩(wěn)定性，討論和給出加性噪聲下的增廣容積卡爾曼濾波(ACKF)。在仿真中，將CKF、UKF和ACKF應(yīng)用于5維高非線性目標(biāo)跟蹤，并分析比較三者的目標(biāo)跟蹤性能。研究結(jié)果表明，在高維非線性目標(biāo)跟蹤系統(tǒng)中，3階ACKF可以獲得更好目標(biāo)跟蹤精度和穩(wěn)定性，以及可接受的計(jì)算復(fù)雜度。

目標(biāo)跟蹤 增廣容積卡爾曼濾波 非線性濾波 加性噪聲

0 引 言

因?yàn)榭柭鼮V波(KF)具有良好的魯棒性和最優(yōu)特性[1-2]，它被廣泛地應(yīng)用于線性系統(tǒng)的狀態(tài)估計(jì)和目標(biāo)跟蹤領(lǐng)域[3-4]。但對于非線性系統(tǒng)，經(jīng)典卡爾曼濾波存在局限性。對此研究人員提出擴(kuò)展卡爾曼濾波(EKF)，使其較好地適用于低維和低非線目標(biāo)跟蹤性系統(tǒng)[5]。但對于高維和高非線性系統(tǒng)，EKF會導(dǎo)致嚴(yán)重的系統(tǒng)失真和濾波誤差。針對這類目標(biāo)跟蹤系統(tǒng)，研究人員對粒子濾波(PF)在移動(dòng)物體跟蹤系統(tǒng)中的應(yīng)用進(jìn)行了研究，并做了相關(guān)性能分析[6-8]。粒子濾波通過大量的隨機(jī)樣本和歸一化權(quán)重對非線性系統(tǒng)的狀態(tài)分布進(jìn)行逼近，故粒子濾波存在計(jì)算復(fù)雜度高，濾波精度低和數(shù)值發(fā)散等缺點(diǎn)[9]。為解決這些問題，Julier等人[10]提出基于無跡變換(UT)的無跡卡爾曼濾波(UKF)。UKF通過UT變換和矩匹配獲得確定性樣本和權(quán)重，具有良好的濾波精度和較低的計(jì)算復(fù)雜度。相比于粒子濾波，UKF在非線性目標(biāo)跟蹤系統(tǒng)中有一定的性能優(yōu)勢[11-12]。UKF的濾波精度取決于UT變換所計(jì)算的均值和協(xié)方差。為提高UKF的濾波精度，人們提出了高階UT變換[13-14]、新型Sigma集合[15]以及線性擴(kuò)展形式的UKF[16]。以上提高UKF濾波精度的方法，均以修改樣本集合或樣本數(shù)量為目標(biāo)。但Wu等人[17]從系統(tǒng)本身出發(fā)，討論了UKF在加性噪聲下的增廣UKF(AUKF)，并對比分析AUKF和UKF的性能。研究結(jié)果表明，AUKF能比UKF更好地匹配奇數(shù)階矩信息，并在非線性濾波中獲得更好的濾波精度和穩(wěn)定性。但為使樣本和權(quán)重能匹配狀態(tài)的4階矩，UKF參數(shù)可能為負(fù)數(shù)[18]。當(dāng)系統(tǒng)維數(shù)時(shí)，UKF在均值點(diǎn)上樣本權(quán)重為 。該負(fù)權(quán)重可能致UKF的不穩(wěn)定性[19]。在AUKF中，系統(tǒng)狀態(tài)的維數(shù)增廣后使得該不穩(wěn)定性問題更加明顯。

對比UKF和CKF，兩者具有很高的相似性[21,28]。尤其是當(dāng)UKF尺度因子κ=0，此時(shí)UKF與3階CKF具有相同的樣本點(diǎn)和樣本權(quán)重[29]。根據(jù)AUKF的思想，Li等人[30]給出了加性噪聲下增廣CKF(ACKF)。3階ACKF的樣本點(diǎn)具有相同的正權(quán)重，且具有更好的濾波性能。

本文中，先對基于樣本的卡爾曼濾波框架和球面-徑向容積法則進(jìn)行了總結(jié)，并給出3階CKF的樣本及其權(quán)重。然后給出一般形式下的增廣球面-徑向法則?；诖?，討論了增廣形式的動(dòng)態(tài)系統(tǒng)和測量方程，并給出ACKF的濾波流程。在目標(biāo)跟蹤仿真中，將UKF(κ=3-n)、3階CKF、5階CKF與3階ACKF應(yīng)用于5維的跟蹤模型中，分析它們在目標(biāo)的位置、速度以及角速度分量上的濾波性能。結(jié)果表明，3階ACKF有更好的濾波性能，尤其是在非線性程度高的位置和速度分量上明顯好于其他三種算法。因此3階ACKF更適合于高非線性目標(biāo)跟蹤系統(tǒng)。

1 容積卡爾曼濾波算法

1.1 基于樣本的卡爾曼濾波框架

卡爾曼濾波框架被廣泛地應(yīng)用于基于樣本點(diǎn)的非線性濾波算法中。總結(jié)UKF和CKF，可知兩者的區(qū)別主要在于樣本選擇策略和權(quán)重確定方法，但使用相同的卡爾曼濾波框架。因此，本節(jié)將對非線性卡爾曼濾波框架進(jìn)行總結(jié)。

考慮如下非線性動(dòng)態(tài)系

xk+1=f(xk)+vk

(1)

zk+1=h(xk+1)+wk+1

(2)

其中，xk∈n，zk∈m；vk和wk分別為互相獨(dú)立的過程和測量高斯白噪聲，協(xié)方差分別為Qk和Rk+1。

(3)

(4)

此時(shí)，基于卡爾曼濾波框架的非線性濾波過程如下：

(1) 時(shí)間更新

Si,k+1|k=f(Si,k|k)

(5)

(6)

(7)

(2) 測量更新

(8)

(9)

其中，yk+1是測量值，以及：

(10)

(11)

(12)

(13)

(14)

1.2 球面-徑向容積法則

在CKF中，核心是通過球面-徑向法則選擇樣本和權(quán)重集合，對一類積分I(f)=∫f(x)×exp(-xTx)dx的逼近[21,22,31]。通過坐標(biāo)轉(zhuǎn)換，將該積分轉(zhuǎn)換到球面-徑向坐標(biāo)系中：

(15)

其中，Un={y∈n|yTy=1}為超球面。

(16)

從式可知，3階CKF具有完全對稱的樣本點(diǎn)，以及相同的非負(fù)權(quán)重。這使得3階CKF比UKF具有更好的穩(wěn)定性。在高維和高非線性的系統(tǒng)中，3階CKF的濾波精度可能無法滿足要求。Jia等人[22]給出了5階CKF，并在仿真中對5階CKF進(jìn)行了分析和討論。

2 增廣容積卡爾曼濾波算法

2.1 增廣球面-徑向容積法則

本小節(jié)將給出增廣容積卡爾曼濾波(ACKF)算法的具體形式。在此之前，為了不失一般性與方便討論，考慮如下一步非線性變換函數(shù)：

u=g(x)+e

(17)

其中，x∈n，均值與協(xié)方差分別為和Px；e∈m，均值與協(xié)方差分別為0和E。

將式改寫為增廣形式：

u=ga(xa)=g(x)+e

(18)

其中，xa=[xT,eT]T，均值和協(xié)方差分別為：

(19)

(20)

以3階球面徑向容積法則為例，增廣形式的樣本及其權(quán)重如下所示：

(21)

其中，i=1,2,…,na，na=n+m。

將式(19)、式(20)代入式(21)，得到：

(22)

其中，j=1,2,…,n，l=1,2,…,m；以上所有樣本點(diǎn)都具有相同的權(quán)重1/2na。

2.2 增廣容積卡爾曼濾波算法

為了將增廣卡爾曼濾波算法適用于動(dòng)態(tài)系統(tǒng)式(1)、式(2)中，先將其寫成增廣形式：

(23)

(24)

(25)

(26)

(27)

在增廣形式下，式(7)中不再包括過程噪聲項(xiàng)Qk。

3 目標(biāo)跟蹤仿真

在本節(jié)中，將考慮一個(gè)空中交通管制系統(tǒng)[21]。在該系統(tǒng)中，空中飛行器按照指定角速度在水平面上做轉(zhuǎn)彎動(dòng)作。飛行器在空中水平面的軌跡如圖1所示[21]。

圖1 飛行器軌跡(I-起點(diǎn)，F(xiàn)-終點(diǎn)，★-雷達(dá)站)

該轉(zhuǎn)彎動(dòng)作可以通過如下動(dòng)態(tài)系統(tǒng)模型表征[21]：

xk-1+vk-1

(28)

其中：

在測量站，雷達(dá)只能測量目標(biāo)飛行器與自身的相對距離和夾角。因此，測量方程為：

(29)

其中，nk是均值和協(xié)方差分別為0和Rk=diag[1 000,100]的高斯白噪聲。

本次仿真實(shí)驗(yàn)中，系統(tǒng)狀態(tài)的初始真實(shí)值為：

x0=[1 000m,300ms-1,1 000m,0ms-1,-3°s-1]T

P0=diag[1 000m2,10m2/s2,100m2,10m2/s2,100mrad2/s2]

在k=0，系統(tǒng)狀態(tài)的均值和協(xié)方差最優(yōu)估計(jì)分別為x0|0=x0和P0|0=P0。仿真時(shí)間長度為200s，采樣間隔Δt=1。

為了對CKF和ACKF的濾波性能進(jìn)行評價(jià)，引入評價(jià)標(biāo)準(zhǔn)根均方誤差(RMSE)。本仿真中，位置信息的RMSE的定義如下[22]：

在目標(biāo)跟蹤測試中，系統(tǒng)狀態(tài)的RMSEs時(shí)間平均值統(tǒng)計(jì)結(jié)果如表1所示。3階ACKF、3階CKF、5階CKF以及UKF(κ=3-m=-2)的計(jì)算復(fù)雜度或計(jì)算時(shí)間開銷如表2所示。系統(tǒng)狀態(tài)的仿真結(jié)果如圖2-圖4所示。

表1 各算法的RMSEs

表2 各算法的樣本數(shù)與計(jì)算時(shí)間

圖2 位置RMSE

圖3 速度RMSE

圖4 角速度RMSE

從圖2-圖4可知，3階ACKF在位置、速度和角度上的RMSE明顯優(yōu)于3階CKF和UKF(κ=-2)，尤其是在位置和速度分量。這可能是由位置和速度分量的高非線性度，以及低測量精度導(dǎo)致。相比于5階CKF，3階ACKF依然具有更好的濾波精度。同時(shí)，3階CKF3和UKF(κ=-2)的RMSE曲線波動(dòng)幅度明顯大于3階ACKF。這表明3階ACKF的濾波性能比3階CKF更加穩(wěn)定。同時(shí)，κ=-2使得UKF中出現(xiàn)負(fù)權(quán)重，這可能導(dǎo)致UKF的出現(xiàn)協(xié)方差非正定性以及目標(biāo)跟蹤失敗問題。根據(jù)表1可知，3階ACKF的統(tǒng)計(jì)RMSEs明顯好于3階CKF和UKF(κ=-2)；略為好于5階CKF。表2顯示，3階ACKF的計(jì)算時(shí)間高于3階CKF和UKF，且近似地正比于樣本數(shù)。與5階CKF相比，3階ACKF的樣本數(shù)只有其近似一半，卻獲得更好的濾波性能。從數(shù)值積分的角度分析，3階ACKF比3階CKF采用更多的樣本數(shù)，更好地逼近非線性系統(tǒng)的分布；利用系統(tǒng)噪聲和測量噪聲，更好地匹配狀態(tài)的奇數(shù)階矩信息，獲得更好的濾波精度和穩(wěn)定性；而且其正權(quán)重保證了濾波的穩(wěn)定。

4 結(jié) 語

本文對UKF、AUKF、和CKF在非線性目標(biāo)跟蹤系統(tǒng)中的性能做了分析比較，并給出了ACKF以及3階ACKF的樣本點(diǎn)和權(quán)重。在5維的高非線性目標(biāo)跟蹤模型中，3階ACKF比3階CKF、5階CKF和UKF(κ=-2)具有更好的濾波精度和穩(wěn)定性，以及可接受的計(jì)算復(fù)雜度。因此，對于高維非線性目標(biāo)跟蹤系統(tǒng)，ACKF是一個(gè)更好的選擇。

[1] 孔軍,湯心溢,蔣敏,等.基于多尺度特征提取的Kalman濾波跟蹤[J].紅外與毫米波學(xué)報(bào),2011(5):446-450.

[2]GroverR,HwangP.IntroductiontorandomsignalsandappliedKalmanfiltering[J].InternationalJournalofAdaptiveControl&SignalProcessing,1992,6(5):516-518.

[3]UhlmannJK.Algorithmsformultiple-targettracking[J].AmericanScientist,1992,80(2):128-141.

[4]FrühwirthR.ApplicationofKalmanfilteringtotrackandvertexfitting[J].NuclearInstrumentsandMethodsinPhysicsResearchSectionA:Accelerators,Spectrometers,DetectorsandAssociatedEquipment,1987,262(2):444-450.

[5]EinickeGA,WhiteLB.RobustextendedKalmanfiltering[J].IEEETransactionsonSignalProcessing,1999,47(9):2596-2599.

[6]YangJ,SchonfeldD,MohamedM.Robustvideostabilizationbasedonparticlefiltertrackingofprojectedcameramotion[J].CircuitsandSystemsforVideoTechnology,IEEETransactionson,2009,19(7):945-954.

[7]ChangC,AnsariR,KhokharA.Multipleobjecttrackingwithkernelparticlefilter[C]//2005IEEEComputerSocietyConferenceonComputerVisionandPatternRecognition,Vol1,Proceedings,2005:566-573.

[8]OkumaK,TaleghaniA,DeFreitasN,etal.Aboostedparticlefilter:Multitargetdetectionandtracking[C]//ComputerVision-ECCV2004.City:Springer,2004:28-39.

[9]DoucetA,JohansenAM.Atutorialonparticlefilteringandsmoothing:Fifteenyearslater[J].HandbookofNonlinearFiltering,2009,12:656-704.

[10]JulierSJ,UhlmannJK.Ageneralmethodforapproximatingnonlineartransformationsofprobabilitydistributions[R].Technicalreport,RoboticsResearchGroup,DepartmentofEngineeringScience,UniversityofOxford,1996.

[11]ZhaoM,YuXL,CuiML,etal.SquareRootUnscentedKalmanFilterBasedonStrongTracking[C]//TheProceedingsoftheThirdInternationalConferenceonCommunications,SignalProcessing,andSystems,Springer,2015:797-804.

[12]JulierSJ,UhlmannJK.Unscentedfilteringandnonlinearestimation[J].ProceedingsoftheIEEE,2004,92(3):401-422.

[13] 張勇剛,黃玉龍,武哲民,等.一種高階無跡卡爾曼濾波方法[J].自動(dòng)化學(xué)報(bào),2014,40(5):838-848.

[14] 黃銚,張?zhí)祢U,高清山,等.一種提高無跡卡爾曼濾波精確度的方法[J].計(jì)算機(jī)仿真,2010,27(3):348-352.

[15]WangY,LiuJ,YangW,etal.AheuristicforSigmasetselectionofUKF[C]//SignalProcessing(ICSP),2014 12thInternationalConferenceon,2015:72-77.

[16]LiuJ,WangY,ZhangJ.AlinearextensionofUKFtohigher-ordermoment-matching[C]//DecisionandControl(CDC),2014IEEE53rdAnnualConferenceon,2014:5021-5026.

[17]WuY,HuD,WuM,etal.UnscentedKalmanfilteringforadditivenoisecase:augmentedvs.non-augmented[C]//ProceedingsoftheAmericanControlConference,2015:4051-4055.

[18]JulierSJ,UhlmannJK.AnewextensionoftheKalmanfiltertononlinearsystems[C]//ProceedingsofSpie,1997:182-193.

[19] 胡高歌,高社生,種永民,等.StochasticstabilityofthederivativeunscentedKalmanfilter[J].中國物理B(英文版),2015(7):64-73.

[20]HuberMF,HanebeckUD.Gaussianfilterbasedondeterministicsamplingforhighqualitynonlinearestimation[C]//Proceedingsofthe17thIFACWorldCongress,2008:13527-13532.

[21]ArasaratnamI,HaykinS.CubatureKalmanfilters[J].AutomaticControl,IEEETransactionson,2009,54(6):1254-1269.

[22]JiaB,XinM,ChengY.High-degreecubatureKalmanfilter[J].Automatica,2013,49(2):510-518.

[23]ZhaoY.PerformanceevaluationofcubatureKalmanfilterinaGPS/IMUtightly-couplednavigationsystem[J].SignalProcessing,2016,119:67-79.

[24] 鹿傳國,馮新喜,張迪.基于改進(jìn)容積卡爾曼濾波的純方位目標(biāo)跟蹤[J].系統(tǒng)工程與電子技術(shù),2012,34(1):28-33.

[25]PakkiK,ChandraB,GuDW,etal.Cubatureinformationfilteranditsapplications[C]//2011AmericanControlConference(ACC),IEEE,2011:3609-3614.

[26]PesonenH,PicheéR.Cubature-basedKalmanfiltersforpositioning[C]//PositioningNavigationandCommunication(WPNC),2010 7thWorkshopon,IEEE,2010:45-49.

[27]SinghAK,BhaumikS.Higherdegreecubaturequadraturekalmanfilter[J].InternationalJournalofControl,AutomationandSystems,2015,13(5):1097-1105.

[28] 潘泉,楊峰,葉亮,等.一類非線性濾波器—UKF綜述[J].控制與決策,2005,20(5):481-489.

[29] 王小旭,潘泉,黃鶴,等.非線性系統(tǒng)確定采樣型濾波算法綜述[J].控制與決策,2012,27(6):801-812.

[30]LiP,YuJ,WanM,etal.TheaugmentedformofcubatureKalmanfilterandquadratureKalmanfilterforadditivenoise[C]//Information,ComputingandTelecommunication,2009.YC-ICT’09.IEEEYouthConferenceon,2009:295-298.

[31] 穆靜,蔡遠(yuǎn)利.迭代容積卡爾曼濾波算法及其應(yīng)用[J].系統(tǒng)工程與電子技術(shù),2011,33(7):1454-1457.

AUGMENTED CUBATURE KALMAN FILTER FOR ADDITIVE NOISEAND ITS APPLICATION TO OBJECT TRACKING

Liu Jiang Ye Songqing

(ChongqingInstituteofGreenandIntelligentTechnology,ChineseAcademyofSciences,Chongqing400714,China)

Since the cubature Kalman filter (CKF) provides a good accuracy with low computational complexity, it is wildly applied in estimation and tracking systems. But for a tracking system involving high dimensionality and acute nonlinearity, 3-degree CKF and high-degree CKF encounter low accuracy and instability problems, respectively. To improve the the performance, augmented cubature Kalman filter for additive noise is discussed. In the simulation, CKF, UKF and ACKF are applied to 5-dimensional targets tracking system. Besides, their performances including accuracy, stability and complexity are compared by RMSEs. The results show that 3-degree ACKF can obtain better tracking accuracy and stability with acceptable computational complexity than UKF, 3-degree CKF and 5-degree CKF in highly nonlinear and dimensional systems.

Tracking Augmented CKF Nonlinear filter Additive noise

2016-01-18。國家自然科學(xué)

61202131)；中國科學(xué)院“西部之光”項(xiàng)目以及青年創(chuàng)新促進(jìn)會項(xiàng)目(2015315)。劉江，副研究員，主研領(lǐng)域：計(jì)算機(jī)軟件與理論，可計(jì)算理論。葉松慶，碩士生。

TP3

A

10.3969/j.issn.1000-386x.2017.03.025