穆祥強，王朝英，危璋，孔云波

(空軍工程大學 信息與導航學院，陜西 西安　710011)

?

基于迭代積分卡爾曼PHD濾波多目標跟蹤

穆祥強，王朝英，危璋，孔云波

(空軍工程大學 信息與導航學院，陜西 西安710011)

在多目標跟蹤環(huán)境下，通過對數(shù)值積分卡爾曼概率假設密度濾波器(QK-PHD)的誤差分析，提出一種迭代QK-PHD算法。該算法基本思想是將一步預測作為第1次迭代的初始值，通過量測更新得到狀態(tài)估計，然后將此狀態(tài)估計作為下一步迭代的初始均值和方差。通過多次迭代后濾波輸出精度更高方差更小。Matlab仿真結(jié)果表明，在不明顯增加計算時間的基礎上，迭代QK-PHD比QK-PHD的跟蹤效果更加明顯，濾波輸出精度更高。

迭代；多目標跟蹤；狀態(tài)估計；數(shù)值積分；卡爾曼濾波；概率假設密度濾波

0　引言

非線性濾波問題廣泛存在于導航、目標跟蹤、數(shù)字通信等領(lǐng)域，其基本任務都是從當前時刻目標狀態(tài)估計和量測去估計出下一時刻的目標狀態(tài)。在多目標環(huán)境下，目標的新生、衍生、合并導致目標個數(shù)動態(tài)變化，而且在觀測過程中會引入大量的噪聲，這些噪聲影響著目標的濾波精度。Mahler[1]根據(jù)有限集[2-4]提出概率假設密度濾波器(probability hypothesis density，PHD)。該方法不需要數(shù)據(jù)關(guān)聯(lián)從而得到廣泛應用。

當目標的運動模型為非線性高斯情況時，為了更加精確地跟蹤目標，需要采用非線性濾波。Vo等人分別提出了擴展卡爾曼PHD(extended Kalman filter PHD,EKF-PHD)和無跡卡爾曼PHD(unscented Kalman filter PHD,UKF-PHD)。但是擴展卡爾曼濾波是展開泰勒級數(shù)來線性化模型，當模型非線性化程度較大時其濾波誤差比較大。迭代擴展卡爾曼濾波(iterated EKF，IEKF)[5]是基于量測信息來糾正非線性觀測函數(shù)的泰勒級數(shù)展開使之重新線性化，從而減小濾波的總體誤差。無跡卡爾曼[6-9](UKF)濾波受尺度參數(shù)的影響較大，使得協(xié)方差矩陣非正定，從而影響系統(tǒng)穩(wěn)定。文獻[10]提出了數(shù)值積分卡爾曼概率假設密度濾波算法，這個方法相對于擴展卡爾曼PHD和無跡卡爾曼PHD在強非線性情況下具有更好的精度和跟蹤效果。文獻[11]將迭代理論與容積卡爾曼相結(jié)合使得濾波算法精度更高，本文在對此文獻深入的研究之后將Gauss-Newton[12]的迭代理論與QKF-PHD相結(jié)合提出了迭代數(shù)值卡爾曼概率假設密度濾波方法。在量測過程中通過迭代方法充分利用新的量測對目標狀態(tài)的更新，降低了非線性的影響提高跟蹤精確度。仿真結(jié)果表明，基于迭代QKF-PHD相比于QKF-PHD跟蹤精度更高，更加穩(wěn)定。

1　概率假設密度算法(PHD)

傳統(tǒng)的跟蹤算法如概率數(shù)據(jù)關(guān)聯(lián)等算法都是通過對量測進行分配，將多目標問題轉(zhuǎn)化為并行的單目標跟蹤處理問題，這類算法的核心問題都是數(shù)據(jù)關(guān)聯(lián)。但是當目標數(shù)過多時就會出現(xiàn)計算量指數(shù)增長、組合爆炸問題。概率假設密度基于有限集，利用一階統(tǒng)計距近似方法對目標后驗概率函數(shù)求集合積分運算，避免了直接計算完全后驗概率密度[13]。因此避免了傳統(tǒng)方法需要的數(shù)據(jù)關(guān)聯(lián)步驟，因此得到廣泛應用。

假設用Dk|k和Dk|k-1分別表示pk(Xk|Z1:k)和pk|k-1(Xk|Z1:k-1)的PHD其遞推過程為

Dk|k-1(xk|Z1:k-1)=γk(xk)+

fk|k-1(xk|xk-1)]*Dk-1|k-1(xk-1|Z1:k-1)dxk-1，

(1)

Dk|k(xk|Z1:k)=(1-PD(xk))Dk|k-1(xk|Z1:k-1)+

(2)

式中：D(·)表示目標強度；γk(xk)表示新生目標強度；βk|k-1(xk|xk-1)為前一時刻衍生目標強度；fk|k-1(xk|xk-1)為目標轉(zhuǎn)移函數(shù)；ek|k-1(xk-1)為目標存活概率；PD(xk)為目標檢測概率；φk,z(xk)=PD(xk)gk|k(z|xk)為目標似然函數(shù);gk|k(z|xk)為單目標似然函數(shù);Dk(z)=κk(z)+Ck(z)為量測的強度;κk(z)為雜波

Ck(z)=∫φk,z(xk)Dk|k-1(xk|Z1:k-1)dx.

2　基于迭代QKF-PHD濾波算法

2.1QKF-PHD濾波算法

非線性系統(tǒng)為

(3)

式中：xk∈Rm和zk∈Rm分別為狀態(tài)向量和量測向量；wk-1和vk分別為系統(tǒng)噪聲和量測噪聲，服從零均值方差為Qk-1，Rk的高斯分布；f(·)和h(·)分別為狀態(tài)轉(zhuǎn)移函數(shù)和量測方程。

假設隨機變量x，其概率密度分布服從標準的高斯分布x～N(0,1)，對于任意的關(guān)于x的函數(shù)g(x)，其積分為

(4)

式中：ξj為積分點；ωj為權(quán)值；m為積分點個數(shù)，使用文獻[14]提出了一種計算積分點的方法。構(gòu)造一個三角對稱矩陣J,其對角線的元素全部為0，其他元素為

(5)

QKF-PHD的算法流程為：

設k-1時刻的后驗概率假設密度的高斯形式表示為

(1) 時間預測

1) 矩陣分解

(6)

2) 求取積分點

(7)

3) 傳播積分點

(8)

4) 狀態(tài)一步預測與協(xié)方差

(9)

(10)

(11)

(2) 量測更新

1) 矩陣分解

(12)

2) 求取積分點

(13)

3) 傳播積分點

(14)

4) 量測預測以及協(xié)方差

(15)

(16)

5) 互協(xié)方差

(17)

6) 濾波增益

(18)

7) 計算量測更新與協(xié)方差

(19)

(20)

QKF-PHD的權(quán)值更新為

(21)

后驗PHD為

Dk(x)=(1-Pd(x))Dk|k-1(x)+

(22)

2.2迭代QKF-PHD(IQKF-PHD)算法的實現(xiàn)

因為在QKF的濾波算法中一步預測難免會出現(xiàn)誤差，通過迭代不斷地對量測新息與方差進行修正就可以降低誤差，IQKF-PHD的時間預測方程仍如QKF-PHD，但是其量測步驟更新需改變。

IQKF-PHD算法的步驟如下：

時間更新

(23)

(24)

狀態(tài)預測與協(xié)方差為

(25)

(26)

量測更新

首先計算第t次迭代的積分點與傳遞值為

(27)

(28)

計算量測預測、新息協(xié)方差和協(xié)方差分別為

(29)

(30)

(31)

其次計算第i次迭代的濾波增益、狀態(tài)估計以及協(xié)方差

(32)

(33)

(34)

最后迭代的終止條件。終止條件為

設迭代的終止的迭代次數(shù)為N，則k時刻的狀態(tài)估計與方差為

(35)

(36)

3　仿真與分析

通過試驗，對比本文提出的算法與QKF-PHD，IEKF-PHD算法性能對比分析，證明本文提出算法的有效性。

(1) 試驗仿真環(huán)境

設初始時刻存在2個目標，初始狀態(tài)分別為(1 000,100,-1 000,-100)和(1 000,100,-3 000,100)，第3個目標出現(xiàn)時刻為20 s。目標做以ω=0.05 rad/s作圓周運動，目標1與目標2運動軌跡有交叉，假設目標存活概率為1，檢驗概率為0.95，觀測站位于(0，0)。過程噪聲wk-1為高斯白噪聲，方差為10 m2/s2。vk為量測噪聲其方差角誤差為0.01 rad2，距離誤差為100 m2。蒙特卡羅仿真次數(shù)為35次，選取積分點為2個。采用評價指標為最優(yōu)子模型分配(optimal subpattern assignment，OSPA) 距離，在這次仿真中對勢估計的懲罰程度和對狀態(tài)估計的懲罰程度都取值為1。

目標跟蹤模型如下

Xk=FXk-1+wk-1,

(2) 仿真結(jié)果分析

仿真結(jié)果如圖1～5所示。

圖1　目標運動軌跡Fig.1　Track of motional target

圖2　第1個目標OSPA距離Fig.2　First target OSPA distance

圖3　第2個目標OSPA距離Fig.3　Second target OSPA distance

圖4　第3個目標OSPA距離Fig.4　Third target OSPA distance

圖5　目標總的OSPA距離Fig.5　Three-target OSPA distance

圖1是目標的運動軌跡，其軌跡為曲線。在t=20 s時候出現(xiàn)第3個目標，也就是產(chǎn)生新的目標。從圖2中可以看出IEKF-PHD算法在t=23～24 s時候還能跟蹤上，但是之后算法跟蹤開始發(fā)散無法跟蹤目標運動。其他2種算法QKF-PHD和IQKF-PHD跟蹤上了目標運動而且效果比IEKF-PHD效果要好得多。從后面2種算法的性能比較可以看出，IQKF-PHD算法的跟蹤精度更高，跟蹤效果最好。從圖3～5中可以看出QKF-PHD與IQKF-PHD算法在跟蹤目標1時后一種算法更加平穩(wěn)，對每一個目標的跟蹤其OSPA距離都是最低的。當t=27 s時刻目標3的運動軌跡與目標2的運動軌跡交叉，從圖5可以看出其他2個算法在此時刻都出現(xiàn)了誤差增大，而本文算法則沒有。由此可得，本文提出的IQKF-PHD散發(fā)跟蹤精度要明顯優(yōu)于其他2個跟蹤算法，體現(xiàn)了該算法的優(yōu)越性。

4　結(jié)束語

本文結(jié)合了迭代思想與QKF-PHD算法的優(yōu)點提出了迭代QKF-PHD算法。該算法利用迭代思想對量測更新步驟進行迭代，充分利用了新的量測對目標狀態(tài)的更新，提高了數(shù)值卡爾曼概率假設密度算法對目標跟蹤精度。仿真結(jié)果表明，本文提出的算法相對與其他2種算法具有更好的跟蹤效果，其穩(wěn)定性與魯棒性得到明顯的提升。

[1]MAHLER R P S.Multitarget Bayes Filtering via First-Order Multitarget Moments[J].IEEE Transactions on Aerospace and Electronic Systems,2003,39(4):1152-1178

[2]楊峰,王永齊,梁彥.基于概率假設密度濾波方法的多目標跟蹤技術(shù)綜述[J].自動化學報,2013,39(11):1944-1956.

YANG Feng, WANG Yong-qi, LIANG Yan.A Suivey of PHD Filter Based Multi-Target tracking[J]. Acta Automatic Sinica, 2013,39(11):1944-1956.

[3]鄒其兵.多伯努利濾波器及其在檢測前跟蹤中的應用[D].西安：西安電子科技大學,2012.

ZOU Qi-bing. Study of Multi-Bernoulli Filter and Its Applications to Track Before Detection[D]. Xi′an:Xidian University,2012.

[4]張鶴冰.概率假設密度濾波算法及其在多目標跟蹤中的應用[D].哈爾濱：哈爾濱工程大學,2012.

ZHANG He-bing.Probability Hypothesis Density Filter Algorithm and Its Application in Multi-Targets Tracking[D].Harbin:Harbin Engineering University,2012.

[5]BELL B M，CATHEY F W. The Iterated Kalman Filter Update as a Gauss-Newton Method [J]. IEEE Trans on Automatic Control, 1993, 38(2): 294-297.

[6]JULIER S J, UHLMANN J K, DURRANT-WHYTE H F.A New Method for the Nonlinear Transformation of Means and Covariance in Filters and Estimators [J]. IEEE. Transactions on Automatic Control , 2000,45(3):477-482.

[7]JULIER S J,UHLMANN J K. A New Extension of the Kalman Filter to Nonlinear System [C]∥SPIE,1997:3068,182-193.

[8]伊建君,張建秋,林青.Unsecented卡爾曼濾波-卡爾曼濾波算法[J].系統(tǒng)工程與電子技術(shù)，2008,30(4):617-620.

YI Jian-jun,ZHANG Jian-qiu,LIN Qing.Filter Algorithm Based on Unsented Kalman Filter [J]. Systems Engineering and Electronics,2008,30(4):617-620.

[9]JULIER S J. The Spherical Simplex Unscented Transformation[C]∥ Proc. Am. Contr. Conf. Denver, Colorado, 2003: 2430-2434.

[10]陳里銘, 陳喆, 殷福亮,等. 基于數(shù)值積分卡爾曼概率假設密度濾波的多說話人的跟蹤方法[J]. 信號處理, 2012, 25(10): 1209-1214.

CHEN Li-ming, CHEN Zhe, YIN Fu-liang, et al. Quadrature Kalman PHD Filter for Multiple Speakers Tracking[J].Signal Processing,2012,25(10):1209-1214.

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

MU Jing,CAI Yuan-li. Interated Cubature Kalman Filter and Its Application[J].Systems Engineering and Electronics,2011,33(7):1454-1457.

[12]武良丹,賀西平,張小鳳.超聲回波參數(shù)的高斯與牛頓迭代計算[J].陜西師范大學學報,2006,34(3):56-58.

WU Liang-dan,HE Xi-ping,ZHANG Xiao-feng.The Calculation of Ultrasonic Echo Parameter by Gauss-Newton Iterative Method[J].Journal of Shaanxi Normal University,2006,34(3):56-58.

[13]王曉, 韓崇昭, 連峰. 基于隨機有限集的目標跟蹤方法研究及最新進展[J]. 工程數(shù)學學報, 2012, 29(4): 568-578.

WANG Xiao,HAN Chong-zhao,LIAN Feng.Survey of Target Tracking Based on Random Finite Set[J]. Chinese Journal of Engineering Mathematics, 2012,29(4):568-578.

[14]巫春玲, 韓崇昭.平方根求積分卡爾曼濾波器[J].電子學報, 2009,37(5):987-992.

WU Chun-ling,HAN Chong-zhao.Square Root Quadrature Kalman Filter[J]. Acta Electronica Sinica,2009,37(5):987-992.

[15]劉玉磊,馮新喜,鹿傳國,等.帶漸消因子的Quandrature卡爾曼濾波[J].宇航學報,2013,37(8):1328-1338.

LIU Yu-lei, FENG Xin-xi,LU Chuan-guo,et al.A Fading Quadrature Kalman Filter for Nonlinear Systems[J]. Journal of Astronautics,2013,37(8):1328-1338.

Probability Hypothesis Density Filter Algorithm Based on Iterated Quadrature Kalman Filter

MU Xiang-qiang，WANG Zhao-ying，WEI Zhang，KONG Yun-bo

(AFEU,Infornmation and Navigation College,Shaanxi Xi’an 710077,China)

Under the condition of multi-target tracking, by analyzing the error of quadrature Kalman probability hypothesis density filter (QK-PHD), the iterated QKF algorithm is proposed. The IQKF-PHD is used to re-estimate the one-step state and measurement predictions by virtue of the measurement updated state variable, and then the mean and variance of state variable are estimated. After several iterations, the outputs of IQKF-PHD have higher accuracy and less variance. Matlab simulation results demonstrate that the tracking accuracy and the outputs of IQKF-PHD are superior to those of QKF-PHD.

iterated;multitarget tracking;state variable; quadrature;Kalman filter; probability hypothesis density(PHD)

2015-06-19；

2015-09-21

陜西省自然科學基金(2011JM.8023)

穆祥強(1989-)，男，江蘇宿遷人。碩士生，研究方向為多目標跟蹤。

通信地址：201600上海市松江區(qū)九亭鎮(zhèn)王家?guī)炻?75樓2號1404室E-mail：1097516121@qq.com

10.3969/j.issn.1009-086x.2016.04.016

TN713；TP391.9

A

1009-086X(2016)-04-0095-06