劉　華, 吳　文, 王世元

(1. 南京理工大學(xué)近程高速目標(biāo)探測(cè)技術(shù)國(guó)防重點(diǎn)學(xué)科實(shí)驗(yàn)室, 江蘇 南京 210094;

2. 西南大學(xué)電子信息工程學(xué)院, 重慶 400715)

?

基于平方根CKF的多傳感器序貫式融合跟蹤算法

劉華1, 吳文1, 王世元2

(1. 南京理工大學(xué)近程高速目標(biāo)探測(cè)技術(shù)國(guó)防重點(diǎn)學(xué)科實(shí)驗(yàn)室, 江蘇 南京 210094;

2. 西南大學(xué)電子信息工程學(xué)院, 重慶 400715)

摘要：為了提高非線性序貫式融合跟蹤算法的精度,提出了基于平方根容積卡爾曼濾波器的多傳感器序貫式融合跟蹤算法。該算法使用三階容積數(shù)值積分原則計(jì)算非線性過(guò)程的均值和方差,克服了擴(kuò)展卡爾曼濾波器存在的濾波精度低及平方根無(wú)跡卡爾曼濾波器存在的參數(shù)復(fù)雜的缺點(diǎn)。同時(shí)，在濾波遞歸過(guò)程中以協(xié)方差平方根矩陣代替協(xié)方差矩陣,這樣確保了濾波算法的數(shù)值穩(wěn)定性,提高了跟蹤的精度。最后用已知彈道系數(shù)的再入段彈道跟蹤模型對(duì)所提算法的性能進(jìn)行仿真測(cè)試。仿真結(jié)果表明,所提算法具有很好的跟蹤性能,是一種有效的非線性序貫式融合跟蹤算法。

關(guān)鍵詞：多傳感器; 序貫式融合; 平方根容積卡爾曼濾波器; 再入段

0引言

多傳感器非線性融合跟蹤算法是一種將多個(gè)相同或者不同類型傳感器所提供的觀測(cè)信息加以融合形成統(tǒng)一的狀態(tài)估計(jì)的算法,它能提高單傳感器目標(biāo)跟蹤算法的準(zhǔn)確性,因此得到了學(xué)者的廣泛關(guān)注。目前已有許多非線性融合算法,主要有集中式融合算法[1-2]和分布式融合算法[3-4]。但這兩種算法都是在獲取所有傳感器的測(cè)量值后才能對(duì)系統(tǒng)狀態(tài)進(jìn)行融合估計(jì),因此對(duì)于實(shí)時(shí)性要求很高的系統(tǒng)而言,難以滿足其應(yīng)用要求。

針對(duì)上述兩種融合算法存在的實(shí)時(shí)性問(wèn)題,文獻(xiàn)[5]提出了多傳感器非線性序貫式融合算法。該算法先用第一個(gè)傳感器的測(cè)量值更新系統(tǒng)預(yù)測(cè)狀態(tài)及對(duì)應(yīng)的預(yù)測(cè)協(xié)方差,接著依次用下一個(gè)傳感器的測(cè)量值更新前一個(gè)傳感器更新所得的狀態(tài)估計(jì)及對(duì)應(yīng)的協(xié)方差,最后得到該時(shí)刻目標(biāo)狀態(tài)的全局融合估計(jì)值及其相應(yīng)的協(xié)方差。文獻(xiàn)[5]所提出的算法不僅在融合精度上與集中式融合算法等價(jià),而且能做到測(cè)量值即到達(dá)即濾波,具有很好的實(shí)時(shí)性。但由于文獻(xiàn)[5]的融合算法是基于擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)算法的,因此存在EKF算法的一些固有缺點(diǎn):①需要計(jì)算非線性系統(tǒng)的雅克比矩陣,這將導(dǎo)致算法的穩(wěn)定性降低,甚至?xí)霈F(xiàn)發(fā)散;②處理強(qiáng)非線性系統(tǒng)時(shí)精度會(huì)降低。

針對(duì)文獻(xiàn)[5]存在的不足,文獻(xiàn)[6-7]分別提出了基于平方根無(wú)跡卡爾曼濾波器(square-root unscented Kalman filter, SRUKF)的序貫式融合算法和基于SRUKF的序貫融合粒子濾波算法。由于SRUKF算法能精確地捕獲任何非線性高斯系態(tài)的二階矩,因此,與文獻(xiàn)[5]的算法比,基于SRUKF的序貫融合算法有更高的融合精度。但SRUKF算法存在3個(gè)可調(diào)參數(shù),需選擇適當(dāng)才能達(dá)到良好的濾波效果,而且需要2n+1(n是系統(tǒng)狀態(tài)維數(shù))個(gè)采樣點(diǎn),這給工程實(shí)踐帶來(lái)了一定限制。

文獻(xiàn)[8-9]提出一種基于3自由度球面-徑向容積準(zhǔn)則的非線性濾波器,即容積卡爾曼濾波器 (cubature Kalman filter, CKF)。該算法使用一組等權(quán)值的容積點(diǎn)集來(lái)計(jì)算非線性變換后的隨機(jī)變量的均值和協(xié)方差。與EKF算法相比,該算法具有更優(yōu)的非線性逼近性能(三階矩)、數(shù)值精度以及濾波穩(wěn)定性。與UKF算法相比,該算法具有實(shí)現(xiàn)簡(jiǎn)單、運(yùn)算時(shí)間短的特點(diǎn)。該算法一經(jīng)提出就在導(dǎo)航[10]、目標(biāo)跟蹤[11-13]、數(shù)據(jù)融合[14-15]等領(lǐng)域得到廣泛應(yīng)用。

由于CKF算法在數(shù)值計(jì)算中存在著噪聲信號(hào)和計(jì)算誤差等因素,有時(shí)會(huì)得到負(fù)的協(xié)方差陣,從而導(dǎo)致CKF出現(xiàn)發(fā)散問(wèn)題。平方根CKF (square-root cubature Kalman filter, SRCKF)[9]算法采用了平方根濾波的思想,確保了協(xié)方差陣的非負(fù)定性,因此可有效提高CKF濾波的精度和穩(wěn)定性。本文在結(jié)合SRCKF和多傳感器序貫式融合算法優(yōu)點(diǎn)的基礎(chǔ)上,提出了基于SRCKF的多傳感器非線性序貫式融合跟蹤算法。

本文首先給出了多傳感器非線性系統(tǒng)的系統(tǒng)描述,然后詳細(xì)闡述了基于SRCKF算法的多傳感器序貫式融合算法,最后用再入段彈道目標(biāo)運(yùn)動(dòng)模型仿真對(duì)比分析了基于EKF、SRUKF、CKF與SRCKF的序貫式融合跟蹤算法的性能。

1系統(tǒng)描述

考慮一個(gè)具有加性高斯白噪聲并帶有個(gè)傳感器的非線性動(dòng)態(tài)系統(tǒng)

(1)

(2)

式中,E代表均值符號(hào);δtk=1 (t=k;t,k>0); δtk=0(t≠k);Qw與Ri為正定矩陣。

2基于SRCKF的多傳感器序貫式融合算系統(tǒng)描述

2.1SRCKF算法介紹

CKF算法在迭代過(guò)程中,由于計(jì)算機(jī)字長(zhǎng)限制而產(chǎn)生的舍入誤差可能導(dǎo)致誤差協(xié)方差陣失去對(duì)稱性和正定性,致使CKF算法中求平方根操作無(wú)法進(jìn)行,進(jìn)而導(dǎo)致濾波中斷,影響濾波器的穩(wěn)定性,而且在每次迭代時(shí)都要計(jì)算協(xié)方差陣的平方根,這需要很大的計(jì)算量。為了增加計(jì)算過(guò)程中的數(shù)值穩(wěn)定性并提高計(jì)算效率,Arasaratnam和Haykin在CKF算法的基礎(chǔ)上進(jìn)一步提出了SRCKF算法。

SRCKF算法的具體流程參見文獻(xiàn)[8]。與CKF算法相比,SRCKF算法具有以下兩個(gè)優(yōu)點(diǎn):

(1) 在濾波過(guò)程中直接以協(xié)方差陣的三角分解因子進(jìn)行遞推更新,而不是協(xié)方差陣本身,這樣可降低計(jì)算負(fù)擔(dān),提高了算法的計(jì)算效率;

(2) 保證了協(xié)方差陣的非負(fù)性,避免了濾波器的發(fā)散問(wèn)題,提高了濾波器的數(shù)值穩(wěn)定性。

2.2基于SRCKF的多傳感器序貫融合算法流程

步驟 1初始化,使用三階容積準(zhǔn)則產(chǎn)生2n個(gè)基本容積點(diǎn)及對(duì)應(yīng)的權(quán)值

(3)

式中,n為系統(tǒng)狀態(tài)變量的維數(shù),記n維單位向量為e=[1,0,…,0]T,符號(hào)[l]表示對(duì)e中元素進(jìn)行全排列和取反所生成的完全對(duì)稱點(diǎn)集,[l]j表示[l]中第j列向量。當(dāng)n=3時(shí),[l]可表示為

(4)

(5)

(6)

(7)

(8)

(9)

式中,qr{·}表示矩陣的QR分解。

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

(30)

(31)

綜上所述,可以得到基于SRCKF的多傳感器序貫式融合濾波算法如圖1所示。

圖1　基于SRCKF的序貫濾波框圖

3再入段彈道目標(biāo)運(yùn)動(dòng)模型和目標(biāo)測(cè)量模型

3.1目標(biāo)運(yùn)動(dòng)模型

再入段導(dǎo)彈距離地面較近,飛行時(shí)間較短,可假定地球?yàn)槠矫?并可忽略地球公轉(zhuǎn)與自轉(zhuǎn)的影響,且假定目標(biāo)在再入段時(shí)只受到空氣阻力和地球引力的作用,因此可建立再入段彈道的目標(biāo)運(yùn)動(dòng)方程為

(32)

式中,v為運(yùn)動(dòng)速度;ρ(z)為目標(biāo)周圍的大氣密度;β為彈道系數(shù);g是地球引力加速度。大氣密度可建模為與高度z成指數(shù)關(guān)系的函數(shù),ρ(z)=c1e-c2z,c1與c2均為常數(shù)。

對(duì)于目標(biāo)運(yùn)動(dòng)方程(32),其相應(yīng)的離散時(shí)間目標(biāo)運(yùn)動(dòng)狀態(tài)模型通過(guò)歐拉近似得到

(33)

非線性函數(shù)D(Sk,β)表達(dá)式為

(34)

式中,g=9.81m/s2。矩陣Φ與G分別表示為

fd(Sk-1,β)為空氣阻力對(duì)再入目標(biāo)產(chǎn)生的影響,表達(dá)式為

(35)

假設(shè)過(guò)程噪聲wk是零均值的高斯白噪聲,其協(xié)方差矩陣為

(36)

式中,q是過(guò)程噪聲強(qiáng)度參數(shù)。

3.2目標(biāo)測(cè)量模型

雷達(dá)觀測(cè)量有兩個(gè),彈道的距離r與俯仰角φ,根據(jù)幾何關(guān)系,可建立測(cè)量方程為

(37)

式中,Mk=[rk,φk]T,rk是目標(biāo)距離,φk是目標(biāo)俯仰角,計(jì)算公式為

(38)

φk=arctan(zk/xk)+vφ

(39)

雷達(dá)測(cè)量噪聲vk=[vr,vφ]T為零均值的高斯白噪聲,即vk~N(0,Rk),其協(xié)方差矩陣為

(40)

式中,diag{·}表示對(duì)角矩陣;σr為雷達(dá)的測(cè)距誤差標(biāo)準(zhǔn)差;σφ為雷達(dá)的俯仰角測(cè)量誤差標(biāo)準(zhǔn)差。

4仿真及結(jié)果

4.1仿真場(chǎng)景

圖2　目標(biāo)運(yùn)動(dòng)軌跡與測(cè)量軌跡

設(shè)濾波初始方差為P0=diag{[5002, 202, 5002, 202]},濾波初始位置為X0=[232 500,-2 255.2, 88 100,-397.6]T,采樣周期為0.5s,仿真時(shí)間為100s,并假定各傳感器均無(wú)虛警及漏檢發(fā)生,進(jìn)行200次蒙特卡羅仿真。仿真設(shè)備的處理器為AMDAthlon(tm) 64X2DualCoreProcessor4000+,內(nèi)存為2G。

4.2實(shí)驗(yàn)結(jié)果

圖3給出了基于EKF、SRUKF、CKF與SRCKF 4種算法的序貫式融合估計(jì)的位置均方根誤差(root mean square error, RMSE)變化曲線,圖4是這4種算法的速度均方根誤差變化曲線。從圖3和圖4中可以看出,基于EKF的序貫式融合算法誤差最大。這是由于EKF算法采用了一階泰勒級(jí)數(shù)展開,丟棄了高階項(xiàng),引入了很大的高階截?cái)嗾`差,降低了估計(jì)精度。從圖中還可得到基于CKF與SRCKF算法的融合估計(jì)精度比SRUKF算法的融合估計(jì)精度要高,這是由于SRUKF算法僅捕獲非線性高斯系統(tǒng)的二階矩,而CKF與SRCKF算法使用容積數(shù)值積分原則對(duì)非線性系統(tǒng)方程的均值和方差進(jìn)行計(jì)算,通過(guò)此方法能夠達(dá)到泰勒展開式的三階精度,因此具有更高的融合估計(jì)精度。而SRCKF算法采用了平方根濾波的思想,確保協(xié)方差矩陣的正定性,提高了CKF算法的融合估計(jì)精度。因此,采用基于SRCKF的序貫融算法能夠提高目標(biāo)跟蹤精度。

圖3　位置均方根誤差對(duì)比

圖4　速度均方根誤差對(duì)比

比較算法性能的另一種指標(biāo)是平均累加均方根誤差(accumulated root mean square error,ARMSE)。下面分別定義位置平均累加均方根誤差A(yù)RMSEp與速度平均累加均方根誤差A(yù)RMSEv。

(41)

(42)

表1定量地給出了這4種算法的位置與速度的平均累加均方差誤差。從表1可以看出,SRCKF估計(jì)精度最高,CKF算法與SRUKF算法次之,EKF算法精度最低。這與圖3和圖4的結(jié)果一致。

表1　位置與速度的ARMSE比較

表2給出了4種算法的200次蒙特卡羅仿真的平均運(yùn)行時(shí)間。從表2可看出,SRUKF、CKF和SRCKF算法的計(jì)算復(fù)雜度均明顯大于EKF算法,而在SRUKF、CKF與SRCKF這3種算法中,SRCKF算法的計(jì)算復(fù)雜度稍低于另外兩種算法。

表2　算法的運(yùn)行時(shí)間對(duì)比

5結(jié)論

本文將SRCKF算法與多傳感器序貫式數(shù)據(jù)融合算法結(jié)合,提出了基于SRCKF算法的多傳感器序貫式數(shù)據(jù)融合算法,并將該算法用于跟蹤彈道系數(shù)已知的再入段彈道目標(biāo)。仿真實(shí)驗(yàn)就估計(jì)精度、計(jì)算復(fù)雜度對(duì)本文算法與基于EKF、SRUKF及CKF的多傳感器序貫式融合算法進(jìn)行了比較,仿真結(jié)果證實(shí)本文算法的融合精度較高,計(jì)算復(fù)雜度適中,是一種有效的多傳感器融合跟蹤算法。

參考文獻(xiàn)：

[1] Li X R. Comparison of two measurement fusion methods for Kalman-filter-based multisensor data fusion[J].IEEETrans.onAerospaceandElectronicSystems, 2001, 37(1): 273-280.

[2] Lang H. Centralized and distributed multisensor integration with uncertainties in communication networks[J].IEEETrans.onAerospaceandElectronicSystems, 1991, 27(2): 370-379.

[3] Rao B S, Durrant-whyte H F. Fully decentralized algorithm for multi-sensor Kalman filtering[J].IEEProceedings-DControlTheoryandApplications, 1991, 138(5): 413-420.

[4] Si X H, Li X B, Zhang Y, et al. Multi-sensor fusion target tracking of reentry phase based on square-root unscented Kalman filter[J].SystemsEngineeringandElectronics, 2012, 34(2): 303-306. (司學(xué)慧, 李小兵, 張彥, 等. 基于平方根UKF的多傳感器融合再入段目標(biāo)跟蹤研究[J].系統(tǒng)工程與電子技術(shù), 2012, 34(2): 303-306.)

[5] Wen C L, Lü B, Ge Q B. A data fusion algorithm based on filtering step by step[J].ACTAElectronicaSinica, 2004, 32(8): 1264-1267. (文成林, 呂冰, 葛泉波. 一種基于分步式濾波的數(shù)據(jù)融合算法[J].電子學(xué)報(bào), 2004, 32(8): 1264-1267.)

[6] Guo W Y, Han C Z, Lian F. Multiple-sensor fusion tracking based on square-root unscented Kalman filter[J].JournalofSystemSimulation,2008,20(12):3237-3240.(郭文艷,韓崇昭,連峰.基于平方根UKF的多傳感器融合跟蹤[J].系統(tǒng)仿真學(xué)報(bào),2008,20(12):3237-3240.)

[7] Pan B, Feng J F, Li Q, et al. Tracking algorithm for MMW/IR multi-sensors fusion[J].JournalofInfraredandMillimeterWaves,2010,29(3):231-235.(潘勃,馮金富,李騫,等.毫米波/紅外多傳感器融合跟蹤算法研究[J].紅外與毫米波學(xué)報(bào),2010,29(3):231-235.)

[8] Arasaratnam I, Haykin S. Cubature Kalman filters[J].IEEETrans.onAutomaticControl, 2009, 54(6): 1254-1269.

[9] Arasaratnam I, Haykin S. Cubature Kalman smoothers[J].Automatica, 2010, 47(10): 2245-2250.

[10] Sun F, Tang L J. INS/GPS integrated navigation filter algorithm based on cubature Kalman filter[J].ControlandDecision, 2012, 27(7): 1032-1036. (孫楓, 唐李軍. 基于cubature Kalman filter的INS/GPS組合導(dǎo)航濾波算法[J].控制與決策, 2012, 27(7): 1032-1036.)

[11] Mu J, Cai Y L. Iterated cubature Kalman filter and its application[J].SystemsEngineeringandElectronics, 2011, 33(7): 1454-1459,1509. (穆靜, 蔡遠(yuǎn)利. 迭代容積卡爾曼濾波算法及其應(yīng)用[J].系統(tǒng)工程與電子技術(shù), 2011, 33(7): 1454-1459,1509.)

[12] Yuan Z J, Zheng N N, Jia X C. The Gauss-Hermit particle filter[J].ActaElectronicSinca,2003,31(7):970-973.(袁澤劍,鄭南寧,賈新春.高斯-厄米特粒子濾波器[J].電子學(xué)報(bào),2003,31(7):970-973.)

[13] Lu C G, Feng X X, Zhang D. Pure bearing tracking based on improved cubature Kalman filter[J].SystemsEngineeringandElectronics, 2012, 34(1): 28-33. (鹿傳國(guó), 馮新喜, 張迪. 基于改進(jìn)容積卡爾曼濾波的純方位目標(biāo)跟蹤[J].系統(tǒng)工程與電子技術(shù), 2012, 34(1): 28-33.)

[14] Ao Z G, Tang C C, Fu C Q, et al. Multi-sensor adaptive cubature Kalman filter data fusion algorithm[J].ApplicationResearchofComputers,2014,31(5):1312-1315,1331.(敖志剛,唐長(zhǎng)春, 付成群, 等. 多傳感器自適應(yīng)容積卡爾曼濾波融合算法[J].計(jì)算機(jī)應(yīng)用研究, 2014, 31(5): 1312-1315,1331.)

[15] Hu Z T,Cao Z W,Li S,et al.Heterogeneous multi-sensor fusion algorithm based on cubature Kalman filter[J].JournalofOptoelectronics·Laser,2014,25(4):697-703.(胡振濤,曹志偉,李松,等.基于容積卡爾曼濾波的異質(zhì)多傳感器融合算法[J].光電子·激光,2014,25(4):697-703.)

劉華(1980-),男,博士研究生,主要研究方向?yàn)榉蔷€性濾波、目標(biāo)跟蹤、多源信息融合。

E-mail: peterliuh@126.com

吳文(1968-),男,研究員,博士,主要研究方向?yàn)楹撩撞ㄖ茖?dǎo)技術(shù)和多模復(fù)合制導(dǎo)技術(shù)。

E-mail: wuwen@njust.edu.cn

王世元(1980-),男,副教授,博士,主要研究方向?yàn)榉蔷€性信號(hào)處理、自適應(yīng)濾波器設(shè)計(jì)、數(shù)字通信系統(tǒng)及生物信息學(xué)。

E-mail: wsy@swu.edu.cn

網(wǎng)絡(luò)優(yōu)先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20150109.0953.001.html

Multi-sensor sequential fusion tracking algorithm based on

square-root cubature Kalman filter

LIU Hua1, WU Wen1, WANG Shi-yuan2

(1.MinisterialKeyLaboratoryofJGMT,NanjingUniversityofScienceandTechnology,Nanjing210094,China;

2.SchoolofElectronicandInformationEngineering,SouthwestUniversity,Chongqing400715,China)

Abstract:In order to improve the accuracy of the nonlinear sequential fusion algorithm, a new multi-sensor sequential fusion algorithm based on square-root cubature Kalman filter (SRCKF) is proposed. The proposed algorithm uses the third degree spherical-radial cubature rule to calculate the mean and covariance of the nonlinear process, and hence, overcomes the shortcomings of low performance in extended Kalman filter and complex parameters in square-root unscented Kalman filter. Meanwhile, the square-root covariance matrix replaces the covariance matrix in filtering recursion. In this way, the numerical stability of the algorithm is guaranteed and the tracking accuracy is improved. The performance of the proposed algorithm is tested by the reentry trajectory tracking model with known ballistic coefficients. Simulation results show that the proposed algorithm has good tracking performance, and is therefore an effective nonlinear sequential fusion tracking algorithm.

Keywords:multiple sensors; sequential fusion; square-root cubature Kalman filter (SRCKF); reentry phase

作者簡(jiǎn)介：

中圖分類號(hào)：TP 911

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

DOI:10.3969/j.issn.1001-506X.2015.07.05

收稿日期：2014-07-25；修回日期：2014-12-03；網(wǎng)絡(luò)優(yōu)先出版日期：2015-01-09。