平方根遞推更新GMP-PHDF

梁志兵, 劉付顯, 高嘉樂

(空軍工程大學防空反導學院, 陜西 西安 710051)

0 引 言

多目標跟蹤(multi-target tracking,MTT)的目的是在每一時刻,從存在漏檢、虛警和數據關聯不確定的量測集中,聯合估計目標狀態及數目。傳統的MTT方法,如聯合概率數據關聯[1](joint probabilistic data association,JPDA)和多假設跟蹤[2-3](multiple hypothesis tracking,MHT),由于考慮目標與量測之間復雜的數據關聯,在計算上難以實現。近年來,基于隨機有限集的MTT方法受到廣泛關注。其中,概率假設密度濾波器[4](probability hypothesis density filter,PHDF)是一種有效的貝葉斯MTT方法,其能夠避免數據關聯難題,實現目標狀態及數目的快速估計。

目前,PHDF主要有序貫蒙特卡羅(sequential Monte Carlo, SMC)PHDF[5-6](SMC-PHDF)和高斯混合(Gaussian mixture,GM)PHDF[7](GM-PHDF)兩種實現技術。SMC-PHDF能夠處理非線性非高斯情況下的MTT問題,但其需要重采樣和粒子聚類[5],且聚類技術容易導致目標狀態及數目的不正確估計。而GM-PHDF可得到PHDF的閉式解,且高斯量的均值代表目標狀態,狀態估計較為準確且計算量較小,但其只適用于線性高斯場景。對于非線性場景,文獻[7]提出擴展卡爾曼(extended Kalman,EK)PHDF和無跡卡爾曼(unscented Kalman,UK)PHDF,但在強非線性模型下,二者均會出現濾波發散問題[8]。對此,Clark等[9]提出GMP-PHDF,其結合蒙特卡羅方法和高斯混合實現技術的優勢,不需要聚類技術和重采樣,可有效處理強非線性模型下的MTT問題。

但現有的GMP-PHDF均采用狀態轉移概率密度作為重要性密度函數,當量測處于其尾部或量測精度較高時,會出現粒子退化現象。而最優重要性密度函數應與當前測量值條件相關,形狀接近真實后驗分布[10]。事實上,近年來出現很多針對重要性密度函數選取的粒子濾波改進方法,如擴展卡爾曼粒子濾波(extended Kalman filter, EKF)[11],無跡卡爾曼粒子濾波(unscented Kalman filter,UKF)[12],容積粒子濾波[13]等,這些方法均利用非線性濾波方法結合最新測量值得到重要性密度函數,可使狀態轉移概率密度朝著高似然區域移動,從而提高濾波精度。但是,這些方法本質上均基于卡爾曼濾波框架,而卡爾曼濾波僅在線性最小方差準則下是最優的,這使得上述非線性濾波方法并不能準確得到后驗概率密度[14-15]。文獻[15]提出一種基于EKF的遞推更新方法,依據測量函數的梯度遞推式地進行狀態更新,可有效克服卡爾曼濾波框架的限制,實現量測對狀態的非線性更新,但EKF的線性化計算使其狀態估計并不準確。對此,文獻[16]將遞推更新方法推廣應用于非線性高斯濾波中,得到遞推更新高斯濾波器(recursive update Gaussian filter,RUGF),并利用其為高斯粒子濾波選擇重要性密度函數,可獲得更加接近于真實分布的狀態后驗估計。但在遞推過程中,利用數值方法(如UKF，容積卡爾曼濾波(cubature Kalman filter,CKF)等)近似高斯積分時,由于計算誤差及噪聲的影響,極易出現協方差矩陣非正定的情況,而導致遞推過程中斷。

為了提高GMP-PHDF的濾波精度,本文首先分析(平方根RUGF(square-root RUGF),SR-RUGF)的實現思路,并給出基于CKF的SR-RUGF實現步驟,避免了協方差矩陣非正定引起遞推中斷的問題;其次,利用SR-RUGF為GMP-PHDF選取重要性密度函數,從而得到基于平方根遞推更新的GMP-PHDF改進算法。

本文首先給出GMP-PHDF的算法流程;其次,給出RUGF的實現步驟,在此基礎上分析SR-RUGF的實現思路,并給出基于CKF的SR-RUGF實現步驟;最后提出平方根遞推更新GMP-PHDF(square-root recursive update GMP-PHDF,SRRU-GMP-PHDF)濾波算法,并對SR-RUGF的算法復雜度進行分析。仿真實驗表明,SRRU-GMP-PHDF算法可以很好地利用量測信息,獲得更高精度的估計結果。

1 高斯混合粒子概率假設密度濾波

GMP-PHDF有效結合GM-PHDF和蒙特卡羅方法的優勢,其PHD由一組高斯量的和近似,目標狀態和數目可通過各高斯量及其權值獲得,不需要重采樣和聚類技術,但其可處理動態方程和測量方程均為非線性的MTT問題。

設定非線性高斯動態模型和測量模型為

fk+1|k(x|ξ)=N(x;φk(ξ),Qk)

(1)

gk+1(z|x)=N(z;hk+1(x),Rk+1)

(2)

式中,N(·;m,P)表示均值為m,方差為P的高斯分布;φk和hk+1分別表示非線性狀態轉移函數和測量函數;Qk和Rk+1分別為過程噪聲協方差和測量噪聲協方差。

假設k時刻目標PHD和k+1時刻新生目標PHD為高斯混合形式:

(3)

(4)

(1) 預測

首先,將目標PHDvk、狀態轉移密度fk+1|k及新生目標PHDγk+1代入PHD預測方程[4](這里不考慮目標衍生問題),可得

vk+1|k(x)=

(5)

進一步遞推得

vk+1|k(x)=

(6)

(7)

(8)

(9)

由此可以得到預測PHD:

vk+1|k(x)=

(10)

(2) 更新

假設k+1時刻的預測PHD為

(11)

將其與測量密度gk+1代入PHD更新方程[4],得

vk+1(x)=(1-PD,k+1)vk+1|k(x)+

(12)

式中

(13)

(14)

對此,k+1時刻更新PHD可表示為

vk+1(x)=(1-PD,k+1)vk+1|k(x)+

(15)

式中

(16)

(17)

(18)

2 基于遞推更新的GMP-PHDF

2.1 遞推更新高斯濾波器[16]

RUGF依據測量函數的梯度遞推式地進行狀態更新,可有效克服卡爾曼濾波框架的限制并很好地利用量測信息,實現量測對狀態的非線性更新。

首先定義

(19)

(20)

其中,N為遞推次數,R為測量噪聲協方差。高斯積分可利用多種數值計算方法,如UKF,CKF等進行求解。從表1可看出,遞推更新過程可以視作為將當前量測按照遞推次數分為N等份,每次對狀態進行漸進式的更新。

2.2 平方根遞推更新高斯濾波器

通過分析表1的實現步驟可以發現,RUGF本質是:①xk+1、zk+1與測量噪聲之間的協方差,即Ck+1和Dk+1均不假設為0,參與遞推更新過程;②利用量測對狀態進行漸進式的更新。

表2 CKF-SR-RUGF實現步驟

其中,m=2nx,nx為狀態向量的維數,且Vk+1為測量噪聲。chol(A)返回矩陣A的Cholesky上三角矩陣;qr{A}表示先對矩陣AT求QR分解得到矩陣Q′和R′,然后只返回矩陣R′的上三角部分的轉置矩陣。另外,ηj為CKF容積點,其表達式為

(21)

式中,[1]j表示容積點集中的第j個元素,以n=4為例,容積點集可表示為

2.3 平方根遞推更新GMP-PHDF

最優的重要性密度函數應與當前測量值條件相關,形狀接近真實后驗分布。遞推更新可以更好地利用量測信息,獲得的狀態后驗估計更為接近真實分布,而其平方根實現又可有效解決因協方差矩陣非正定而引起的遞推中斷問題。對此,本文利用SR-RUGF為GMP-PHDF產生重要性密度函數,得到SRRU-GMP-PHDF算法,具體實現如表3所示。

表3 SRRU-GMP-PHDF實現步驟

3 SR-RUGF復雜度分析

一個算法的復雜度分析對其工程實用至關重要。對此,以容積求積準則為例,利用CKF、CKF-RUGF和CKF-SR-RUGF選取重要性密度函數,這里對它們的計算復雜度進行分析比較。CKF-SR-RUGF的單次計算量分析如表4所示。

其中,i表示遞推更新的次數,m為量測維數,且n為狀態維數。對于維數為l×p的矩陣,QR分解的計算量為2lp2,Cholesky分解的計算量為lp2+p3/3。

根據文獻[20],CKF選取重要性密度函數的計算量為

m3+3m2+2mn+m

(22)

m3+4m2+4mn+m)

(23)

根據表4,本文CKF-SR-RUGF的計算量為

3m3+12n2m+6m2n-m2+12mn+m)

(24)

從上述分析不難看出,單次CKF-RUGF的計算量略大于CKF的計算量,而由于QR分解的引入,CKF-SR-RUGF的計算量要大于CKF- RUGF的計算量。但相對于CKF,CKF-SR-RUGF計算量的增加主要還是來自于遞推次數N。因此,在實際應用中,應選擇合適的N以權衡計算復雜度和所需精度。

4 仿真分析

假設傳感器的測量范圍是半徑為2 000 m的半圓,共存在6個目標,它們具體進入和離開測量區域的時間以及真實的運動軌跡如圖1所示。

圖1 目標真實軌跡Fig.1 True target trajectories

(25)

式中

傳感器測量模型為

(26)

假設目標新生PHD為

(27)

式中

Pγ=diag([2 500 2 500 2 500 2 500 (6×π/180)2]T)

圖2 新生PHD初始化Fig.2 Initialization of birth PHD

假設雜波服從泊松分布,每次測量雜波數目平均值為λ=20,且均勻分布在區域[-π/2,π/2]rad×[0,2 000]m內。目標存活概率和檢測概率分別為PS=0.99,PD=0.98,粒子數M=300,遞推更新次數N=20,蒙特卡羅仿真次數為MC=50。

另外,為消除高斯量的指數增長,設定權重剪枝門限T=10-5,合并門限U=4 m,高斯量最大數目Jmax=100,具體設置方法見文獻[7]。

圖3、圖4分別給出傳統GMP-PHDF[9]和本文SRRU-GMP-PHDF算法(利用CKF實現)在某一時刻的狀態估計圖,可以看出,相對于GMP-PHDF,SRRU-GMP-PHDF具有更優的狀態估計性能。

圖3 GMP-PHDF狀態估計Fig.3 State estimates of GMP-PHDF

圖4 SRRU-GMP-PHDF狀態估計Fig.4 State estimates of SRRU-GMP-PHDF

此外,本文將SRRU-GMP-PHDF算法(利用CKF實現)與傳統GMP-PHDF(以狀態轉移密度為重要性密度函數),CKF-GMP-PHDF(利用CKF產生重要性密度函數[13]),EKRU-GMP-PHDF(利用文獻[15]遞推更新方法產生重要性密度函數)進行性能對比。

圖5給出各算法的最優分配模式(optimal subpattern assignment,OSPA)[21]性能對比圖。不難發現,本文SRRU-GMP-PHDF算法和EKRU- GMP-PHDF算法的估計性能要優于其他兩種算法,這表明遞推更新思想的優越性及本文平方根遞推更新方法的有效性。而EKRU-GMP-PHDF性能略差于SRRU-GMP-PHDF,可能的原因是EKF的線性化計算引入較大的濾波誤差。傳統GMP-PHDF的性能最差,且發散現象較嚴重,原因在于似然函數相對狀態轉移密度函數呈尖峰狀態,導致其出現粒子退化問題,而CKF-GMP-PHDF性能優于GMP-PHDF,原因是其采用CKF結合最新量測產生重要性密度函數,有效緩解了粒子退化問題,但其在多目標臨近情形時容易出現發散現象。

圖5 OSPA性能對比Fig.5 Performance comparison of OSPA

圖6給出各算法的勢估計性能對比圖,從中可以看出,在本文跟蹤場景下,SRRU-GMP-PHDF算法的勢估計值與真實值最為接近,說明本文算法勢估計性能的有效性。EKRU-GMP-PHDF勢估計性能略優于CKF-GMP-PHDF,而傳統GMP-PHDF的勢估計值與真實值相差最大,無法有效估計目標數目。

圖6 勢估計性能對比Fig.6 Performance comparison of cardinality estimates

表5給出各算法的單次運行時間對比,其中遞推更新次數N=20。SRRU-GMP-PHDF 、EKRU-GMP-PHDF和CKF-GMP-PHDF的運行時間要明顯高于GMP-PHDF兩個數量級,原因是對于每一個重要性密度函數的選取,CKF-GMP-PHDF要利用CKF結合最新量測計算得到,而其他兩種算法均進行N次的遞推計算。SRRU-GMP-PHDF的運行時間最長,但其估計性能也最好。

表5 單次運行時間比較

圖7和表6分別給出遞推更新次數N=3,5,10,20時,SRRU-GMP-PHDF算法的OSPA及其均值對比。不難發現,隨著N的增大,SRRU-GMP-PHDF算法性能逐漸提升。但是當N增加到一定程度時,SRRU-GMP-PHDF算法性能幾乎不再提升。因此,實際應用時,應選取合適的遞推更新次數,以權衡計算復雜度與估計精度。

SRRU-GMP-PHDFOSPA均值N=320.745N=515.871N=1015.437N=2015.428

5 結 論

傳統GMP-PHDF采用先驗狀態轉移概率密度作為重要性密度函數,會出現粒子退化問題。而RUGF可獲得更為接近于真實分布的后驗估計,但其協方差矩陣易非正定而導致遞推中斷。對此,本文首先分析SR-RUGF的實現思路,并利用SR-RUGF為GMP-PHDF構建重要性密度函數,進而提出SRRU-GMP-PHDF算法。仿真結果驗證了本文算法的有效性。

參考文獻：

[1] BA H X, CAO L, HE X Y, et al. Modified joint probabilistic data association with classification-aided for multi-target tracking[J]. Journal of Systems Engineering and Electronics, 2008, 19(3):434-439.

[2] REID D B. An algorithm for tracking multiple targets[J]. IEEE Trans.on Automatic Control, 1979, 24(6):843-854.

[3] STREIT R L, LUGINBUHL T E. Maximum likelihood method for probabilistic multi-hypothesis tracking[C]∥Proc.of the SPIE Signal and Data Processing of Small Targets, 1994, 2235: 5-7.

[4] MAHLER R. Multi-target Bayes filtering via first-order multi-target moments[J]. IEEE Trans.on Aerospace and Electronic systems, 2003, 39(4):1152-1178.

[5] VO B N, SINGH S, DOUCET A. Sequential Monte Carlo methods for multi-target filtering with random finite sets[J]. IEEE Trans.on Aerospace and Electronic Systems,2005,41(4):1224-1245.

[6] 王利偉,司偉建,曲志昱.增強型SMC-PHD多目標跟蹤算法[J].系統工程與電子技術,2015,37(10):2205-2211.

WANG L W, SI W J, QU Z Y. Improved SMC-PHD algorithm for multiple targets tracking[J]. Systems Engineering and Electronics, 2015,37(10):2205-2211.

[7] VO B N, MA W K. The Gaussian mixture probability hypothesis density filter[J]. IEEE Trans.on Signal Processing, 2006, 54(11):4091-4104.

[8] KOTECHA J H, DJURIC P M. Gaussian particle filtering[J]. IEEE Trans.on Signal Processing, 2003,51(10):2592-2601.

[9] CLARK D, VO B T, VO B N. Gaussian particle implementations of probability hypothesis density filters[C]∥Proc.of the IEEE Aerospace Conference, 2007:1-11.

[10] MERWE R, DOUCET A, FREITAS N, et al. The unscented particle filter[C]∥Proc.of the 13th International Conference on Neural Information Processing Systems, 2000: 563-569.

[11] HOU S Y, HUNG H S, KAO T S. Extended Kalman particle filter angle tracking (EKPF-AT) algorithm for tracking multiple targets[C]∥Proc.of the International Conference on System Science and Engineering, 2010: 216-220.

[12] 康莉, 謝維信, 黃敬雄. 基于unscented粒子濾波的紅外弱小目標跟蹤[J]. 系統工程與電子技術, 2007, 29(1):1-4.

KANG L, XIE W X, HUANG J X. Tracking of infrared small target based on unscented particle filtering[J]. Systems Engineering and Electronics, 2007, 29(1): 1-4.

[13] LU C G, FENG X X, LEI Y, et al. A novel particle filter for nonlinear non-Gaussian estimation[C]∥Proc.of 3rd International Workshop on Intelligent Systems and Applications, 2011:1-5.

[14] HUBER M F, HANEBECK U D. Gaussian filtering for polynomial systems based on moment homotopy[C]∥Proc.of 16th IEEE International Conference on Information Fusion,2013:1080-1087.

[15] ZANETI R. Recursive update filtering for nonlinear estimation[J]. IEEE Trans.on Automatic Control, 2012, 57(6):1481-1490.

[16] 張勇剛,王剛,黃玉龍,等.遞推更新高斯粒子濾波[J].控制理論與應用,2016,33(3): 353-360.

ZHANG Y G, WANG G, HUANG Y L, et al. Recursive update Gaussian particle filter[J]. Control Theory & Application, 2016, 33(3): 353-360.

[17] 何友,修建娟,關欣.雷達數據處理及應用[M].3版.北京:電子工業出版社, 2013.

HE Y,XIU J J, GUAN X. Radar data processing with applications[M]. 3rd ed. Beijing: Publishing House of Electronics Industry, 2013.

[18] VAN DER M R, WAN E A. The square-root unscented Kalman filter for state and parameter-estimation[C]∥Proc.of the IEEE International Conference on Acoustics, Speech, and Signal Processing, 2001: 3461-3464.

[19] 劉華,吳文,王世元.基于平方根CKF的多傳感器序貫式融合跟蹤算法[J].系統工程與電子技術,2015,37(7):1494-1498.

LIU H, WU W, WANG S Y. Multi-sensor sequential fusion tracking algorithm based on square-root cubature Kalman filter[J]. Systems Engineering and Electronics, 2015, 37(7):1494-1498.

[20] 張召友, 郝燕玲, 吳旭. 3種確定性采樣非線性濾波算法的復雜度分析[J].哈爾濱工業大學學報, 2013,45(12):111-115.

ZHANG Z Y, HAO Y L, WU X. Complexity analysis of three deterministic sampling nonlinear filtering algorithms[J]. Journal of Harbin Institute of Technology, 2013, 45(12): 111-115.

[21] VO B N, VO B T, SCHUHMACHER D. A consistent metric for performance evaluation of multi-object filters[J]. IEEE Trans.on Signal Processing, 2008,56(8):3447-3457.