基于迭代積分粒子濾波的目標跟蹤算法*

毛少鋒,馮新喜,鹿傳國,危 璋

(1 空軍工程大學信息與導航學院,西安 710077;2 95806部隊,北京 100000)

基于迭代積分粒子濾波的目標跟蹤算法*

毛少鋒1,馮新喜1,鹿傳國2,危 璋1

(1 空軍工程大學信息與導航學院,西安 710077;2 95806部隊,北京 100000)

針對粒子濾波在非線性目標跟蹤中存在粒子退化的問題,提出一種迭代積分粒子濾波的目標跟蹤算法。該算法從改進重要性函數的角度入手,在積分卡爾曼濾波的基礎上,通過高斯牛頓迭代的方法進行量測更新,并對粒子集合中的粒子進行迭代積分卡爾曼濾波,使得構造的重要性函數更加貼近真實后驗分布。仿真結果表明,與粒子濾波算法、積分粒子濾波算法相比,該算法在有效改善非線性目標跟蹤中粒子退化的同時,提高了跟蹤精度。

高斯牛頓迭代;積分卡爾曼濾波;重要性函數;非線性目標跟蹤

0 引言

現代戰場環境中,通過角度獲得的目標數據信息大都是非線性量測,其跟蹤實質就是利用角度信息來估計目標的運動狀態[1]。而粒子濾波(particle filter,PF)[2]可以較好的解決這一非線性濾波估計問題。

然而粒子退化問題一直是影響粒子濾波精度的一個重要方面。粒子濾波使用先驗概率密度作為重要性函數,沒有考慮最新的量測信息,使得狀態估計存在較大誤差[2]。因此,文獻[3]采用擴展卡爾曼濾波(extended kalman filter,EKF)產生重要性函數,但是EKF需要進行求導運算,容易引入截斷誤差;文獻[4]采用的是無跡卡爾曼濾波(unscented kalman filter,UKF),UKF需要設計合理的Sigma點,否則可能導致濾波發散;文獻[5]利用迭代擴展卡爾曼濾波(iterated extended kalman filter,IEKF),運用高斯-牛頓迭代方法進行線性化時誤差較大,精度要求較高時也難以滿足需求;文獻[6]采用容積卡爾曼濾波(cubature kalman filter,CKF),CKF一般在高維情況下有較好的實時性,維數較低時效果不是很好。

針對以上問題,文中利用高斯牛頓迭代方法[7]對積分卡爾曼濾波(quadrature kalman filter,QKF)[8]的量測進行迭代更新,構造重要性函數,提出一種迭代積分粒子濾波(iterated quadrature particle filter,IQPF)算法。該算法通過高斯牛頓迭代對QKF算法進行迭代處理,使得迭代QKF產生的重要性密度函數更加接近真實后驗概率分布。仿真結果表明,該算法有效的提高了目標的跟蹤精度。

1 非線性目標跟蹤模型

文中以二維無源傳感器目標跟蹤系統為例。此時,無源傳感器只提供角度信息,具體的幾何關系如圖1所示。

圖1 目標與傳感器之間的幾何關系

對于勻速運動目標,運動模型可以寫為:

X(k)=FX(k-1)+W(k-1)

(1)

式中:F為狀態轉移矩陣;W(k)為過程高斯白噪聲,服從N(0,Q),Q為協方差矩陣。

目標跟蹤模型的量測方程為:

Z(k)=h(X(k))+V(k)= [α(k)β(k)]T+V(k)

(2)

(3)

(4)

其中：α(k)、β(k)分別為傳感器1和2的方位量測值。X=(x(k),y(k))為目標在k時刻的位置坐標,S1=(x1(k),y1(k))為傳感器1在k時刻的位置坐標,S2=(x2(k),y2(k))為傳感器2在k時刻的位置坐標。V(k)為觀測噪聲,服從N(0,R),而R為量測噪聲協方差矩陣。

從量測方程中可以看出,這是一個典型的非線性問題,需要通過非線性濾波技術來獲得優估計,下面給出迭代積分粒子濾波跟蹤算法。

2 基于迭代的積分粒子濾波算法

2.1 QKF算法

2007年A.Ienkaran[8]等提出的QKF濾波算法通過使用求積分的原則計算非線性隨機變量的均值和協方差,避免了求導運算,并且在低維情況下有較好的濾波效果。同時它除了具有UKF的優點以外,采樣點數還可以根據不同的系統要求而改變,具體的算法流程見文獻[9]。

2.2 迭代QKF的量測更新

然而QKF算法對模型具有較強的依賴性,且對濾波的初始條件比較敏感,因此每次迭代相比上一步迭代能否更逼近理想值是不確定的。而迭代QKF利用觀測更新得到的狀態估計,對系統的非線性量測方程進行線性化,這樣每次迭代都采用最新的迭代值,可以有效的減少QKF算法線性化過程中帶來的誤差。

高斯牛頓迭代主要用來求解非線性最小二乘問題。由于QKF屬于一種高斯次優濾波算法,其中所包含的隨機量符合高斯分布,因此,可使用最大后驗估計來計算更新狀態。

利用最小均方誤差估計準則,可得目標狀態的估計值為:

(5)

根據貝葉斯理論,后驗概率為:

預測概率為:

(7)

其中,p(Zk|Xk)為似然概率,由量測方程和噪聲協方差矩陣確定。Bell指出卡爾曼濾波的量測更新可以表示為一個求極大后驗估計的問題。經過上面的分析,也就是求極大似然估計的問題,即極小化的“費用函數”[10],這樣就可以將問題轉化為求下列函數的極小值:

(8)

用觀測量和狀態預測這兩個相互獨立的隨機量構造了一個偽觀測量:

(9)

則此觀測量同樣為高斯隨機變量,均值和方差分別表示為:

E(Xk)=[h(Xk)Xk]T

(10)

V=diag([RP])

(11)

則式(8)可表示為:

(12)

令STS=V-1,L(Xk)=S[Z-E(Xk)],則有:

(13)

根據上式可以看出,QKF的量測更新則轉化為最小二乘問題,按照高斯牛頓方法,對于目標函數JQ(Xk),高斯牛頓迭代可表示為:

(14)

其中i為迭代次數,函數L(Xk)的一階導數為:

(15)

因此,將式(15)代入式(14),整理可得到迭代QKF的量測更新方程為:

同時可得到迭代QKF的協方差矩陣方程為:

(17)

2.3 IQPF算法

通過對粒子集合中的每個粒子進行迭代QKF濾波,來產生粒子濾波的重要性函數,使得粒子的分布更接近狀態的后驗概率分布。

IQPF中粒子采樣的具體步驟如下:

(18)

(19)

則歸一化權值為:

(20)

5)重采樣,根據有效粒子數和預設門限來判斷是否需要重采樣,得到等權值粒子集。

(21)

(22)

3 仿真分析

為了驗證算法的有效性,通過兩組實驗,對IQPF算法和PF算法、QPF算法的性能進行對比分析。

實驗1:通過一組典型的一維非線性模型估計問題驗證IQPF算法的濾波性能,設系統模型的狀態方程和觀測方程為:

Xk=1+sin (0.4πk)+0.5Xk-1+wk-1

式中：過程噪聲服從Gamma分布,wk～Gamma(3,2),觀測噪聲服從正態分布,Vk～N(0,0.000 1),初始狀態X0=1,粒子數為N=200,蒙特卡羅實驗仿真次數為50次,重采樣部分采用殘差采樣法,仿真時間為T=60 s,迭代QKF采用的積分點數為m=3。

圖2 系統狀態估計

圖3 位置均方根誤差

表1 各算法均方根誤差的均值和方差

圖2為各種算法的狀態估計曲線,圖3為它們的均方根誤差曲線圖,表1為各算法均方根誤差的均值和方差以及平均運行時間。從仿真結果可以看出,PF算法在進行系統狀態估計過程中,有時會出現較大的偏離,而IQPF相比PF、QPF能夠較好的估計系統狀態。此外,從表1可以看出,IQPF的均方根誤差較小,有效改善了濾波估計的精度,這主要在于IQPF迭代時選取的是上一次迭代的最新值。

圖4 位置均方根誤差

圖5 速度均方根誤差

表2 各算法均方根誤差的均值

圖4為位置均方根誤差曲線,圖5它們的速度均方根誤差曲線圖,表2為各算法均方根誤差的均值和方差以及平均運行時間。從仿真曲線以及表2可以看出,PF跟蹤目標的誤差較大,難以實現目標的準確跟蹤,主要在于目標濾波的初始狀態與目標真實狀態偏離較大,粒子退化現象嚴重;QPF能夠實現目標的穩定跟蹤,這主要取決于QKF較優的非線性逼近性能、數值精確度以及濾波穩定性,使得利用QKF重要性函數更加貼近于真實后驗;IQPF在QKF的基礎上,通過對量測方程進行迭代處理,每次迭代都采用上一步更新得到的最新的迭代值,使得跟蹤精度相比QPF有了較好的改善,但運行時間也有了些許增加。

4 結論

文中針對在非線性目標跟蹤中的粒子退化問題,提出了一種迭代的積分粒子濾波目標跟蹤算法。利用高斯牛頓迭代對QKF量測進行迭代更新,使得改進后的重要性函數更加貼近真實后驗分布。仿真實驗表明,文中算法有效地解決了粒子退化以及非線性目標跟蹤中的非線性估計問題,提高了目標跟蹤精度。然而如何將所提算法更好的運用到多目標跟蹤中,將是下一步研究的重要方向。

[1] 張洋, 芮國勝, 苗俊, 等. 擴展容積卡爾曼濾波定位技術研究 [J]. 光電工程, 2012, 39(4): 38-43.

[2] 胡士強, 敬中良. 粒子濾波算法綜述 [J]. 控制與決策, 2005, 20(4): 361-365.

[3] De Freitas N, Niranjan M, Gee A H, et al. Sequential Monte Carlo methods to train neural network models [J]. Neural Computa-tion, 2000, 12(4): 955-993.

[4] Van der Merwe R, Douce A, de Freitas N, et al. The unscented particle filter [R]. Cambridge, UK: Engineering Department, Cambridge University, 2000: 1-45.

[5] 李良群, 姬紅兵, 羅軍輝. 迭代擴展卡爾曼粒子濾波器 [J]. 西安電子科技大學學報, 2007, 34(2): 233-238.

[6] 孫楓, 唐李軍. Cubature粒子濾波 [J]. 系統工程與電子技術, 2011, 31(11): 2554-2557.

[7] BELL B, CATHEY F. The iterated Kalman filter update as a Gauss-Newton method [J]. IEEE Transactions on Automatic Control, 1993, 38(2): 294-297.

[8] Ienkaran A, Simon H, Robert J E. Discr-etetime nonlinear filtering algorithms using Gauss-Hermite quadrature [J]. Proceedings of the IEEE. 2007, 95(5): 953-977.

[9] 巫春玲, 韓崇昭. 求積分卡爾曼粒子濾波算法 [J]. 西安交通大學學報, 2009, 43(2): 25-28.

[10] Freitas J F G, Niranjan M, Gee A H, et al. Sequential Monte Carlo Methods to train neural network models [J]. Neural Computation, 2000, 12(4): 993-995.

[11] 孔云波, 馮新喜, 鹿傳國. 基于改進高斯混合粒子濾波的純方位目標跟蹤算法 [J]. 宇航學報, 2012, 33(7): 971-876.

[12] 郝燕玲, 楊峻巍, 陳亮, 等. 平方根容積卡爾曼濾波器 [J]. 彈箭與制導學報, 2012, 32(2): 169-172.

[13] 鹿傳國, 馮新喜, 張迪, 等. 蒙特卡羅馬爾可夫鏈容積粒子濾波器 [J]. 電子科技大學學報, 2012, 41(6): 859-864.

Target Tracking Algorithm Based on Iterative Quadrature Particle Filter

MAO Shaofeng1, FENG Xinxi1, LU Chuanguo2, WEI Zhang1

(1 Information and Navigation College, Air Force Engineering University, Xi’an 710077, China; 2 No.95806 Unit, Beijing 100000, China)

In view of particle degradation problem of nonlinear target tracking, an iterative Quadrature particle filter (IQPF) target tracking algorithm was proposed based on the improved importance function. The algorithm is the basis of the Quadrature Kalman filter (QKF), the Gauss-Newton iteration method is used to make iterative processing, which can take the measurement into account by means of the iterative Quadrature Kalman filter, to make the importance function be closer to the true posterior. The computer simulation results indicate that compared with the particle filter (PF) algorithm and the Quadrature particle filter (QPF) algorithm, this algorithm has better performance in accuracy and effectively solves the particle degradation problem in nonlinear tracking.

Gauss-Newton iteration; Quadrature Kalman filter; importance function; nonlinear tracking problem

2014-03-25

陜西省自然科學基金(2011JM8023)資助

毛少鋒(1989-),男,陜西澄城人,碩士研究生,研究方向:目標跟蹤。

TN953

A