Cubature Kalman probability hypothesis density filter based on multi-sensor consistency fusion①

Hu Zhentao (胡振濤), Hu Yumei, Guo Zhen, Wu Yewei

(*Institute of Image Processing and Pattern Recognition, Henan University, Kaifeng 475004, P.R.China)(**College of Automation, Northwestern Polytechnical University, Xi’an 710072, P.R.China)

?

Cubature Kalman probability hypothesis density filter based on multi-sensor consistency fusion①

Hu Zhentao (胡振濤)②*, Hu Yumei**, Guo Zhen*, Wu Yewei*

(*Institute of Image Processing and Pattern Recognition, Henan University, Kaifeng 475004, P.R.China)(**College of Automation, Northwestern Polytechnical University, Xi’an 710072, P.R.China)

The GM-PHD framework as recursion realization of PHD filter is extensively applied to multi-target tracking system. A new idea of improving the estimation precision of time-varying multi-target in non-linear system is proposed due to the advantage of computation efficiency in this paper. First, a novel cubature Kalman probability hypothesis density filter is designed for single sensor measurement system under the Gaussian mixture framework. Second, the consistency fusion strategy for multi-sensor measurement is proposed through constructing consistency matrix. Furthermore, to take the advantage of consistency fusion strategy, fused measurement is introduced in the update step of cubature Kalman probability hypothesis density filter to replace the single-sensor measurement. Then a cubature Kalman probability hypothesis density filter based on multi-sensor consistency fusion is proposed. Capabilily of the proposed algorithm is illustrated through simulation scenario of multi-sensor multi-target tracking.

multi-target tracking, probability hypothesis density (PHD), cubature Kalman filter, consistency fusion

0 Introduction

Multi-target tracking techniques are always the hotspot research in target tracking field. The probability hypothesis density (PHD) filter as recursion that propagates the first-order statistical moment of random finite sets (RFS) of states, is an attractive approach to track unknown and time-varying targets in the presence of measurement uncertainty, clutter, noise, and detection uncertainty[1]. However, PHD filter contains multiple integrals with no closed forms in general. Due to its inherent computational hurdle, the application and popularization of PHD filter is limited. To solve this problem, some researches and work mainly focus on two categories. One of the effective implementations is sequential Monte Carlo PHD (SMC-PHD) filter[2,3]. In the non-linear and non-Gaussian system, the relationship between PHD filter and sequential Monte Carlo method is established through approximating PHD function by a group of random samples in state space, and leads the integral computation to be replaced by samples mean[4]. However, a large number of particles, needed to ensure filtering precision in the realization of SMC-PHD filter, lead to increase of computation cost, and extracting multi-target estimation is an additional cost. Moreover, the stochastic sampling mechanism often leads particle to degeneracy after a few iterations. The adverse effect caused by particle degeneracy is mitigated in a certain degree through re-sampling, but the re-sampling process results in the reduction of particle diversity. In addition, an estimated state is obtained through dividing the particle into different clusters in SMC-PHD filter, which leads to state estimation unreliable. The other one is Gaussian mixture PHD (GM-PHD) filter[5,6], for jointly estimating the time-varying number of targets and their states, closed-form recursions are given for propagating means, covariance, and weights of the constituent Gaussian component of posterior intensity, which meets three assumptions: ① Targets and sensor follow a linear and Gaussian model. ② The survival and detection probabilities are independent. ③ The intensities of birth and spawn RFSs are Gaussian mixture. In Ref.[7], Clark proved uniform convergence of the errors in GM-PHD filter. Aiming at the multi-detection from a same target, Tang derived a general multi-detection PHD update formulation, and established its recursion realization under the GM-PHD framework[8].

However, with regard to the non-linear feature of multi-target system, assumption ① is extended to non-linear Gaussian model. Therefore, the non-linear filter such as extended Kalman filter (EKF) and unscented Kalman filter (UKF) are considered to unite the PHD filter under the framework of Gaussian mixture[9,10]. The implementation mechanism of EKF is to realize local linearization of state equation and observation equation. It only calculates the posterior mean and covariance accurately to the first order with all higher order moments truncated. If the nonlinearity of estimated system is very strong, usually EKF can not obtain good filtering result and even lead to the filtering divergence phenomenon[11,12]. While unscented Kalman filter (UKF)[13]and cubature Kalman filter (CKF)[14]are both typical implementation of deterministic sampling filter, UKF approaches nonlinear state posterior distribution by UT transformation strategy, and it has higher universality for non-linear system with Gaussian noise. But whether the parameters are selected reasonably or not in UKF, they may affect targets estimation precision directly. In addition, the problem that filtering variance is not positive definite may occur. However, in the implementation of CKF, a third-degree spherical-radial cubature rule is established to compute integrals numerically. The weights in CKF are positive to ensure that the filtering covariance is positive definite matrix, and it is verified that CKF is superior to UKF[15]. Therefore, CKF is adopted to realize PHD recursion under the framework of Gaussian mixture in this paper.

The appropriate selection of filtering algorithm leads to the improvement of targets tracking precision. Measurement, obtained by sensor for providing latest information in the update step, is also an alternative vital factor to enhance estimation precision. The technique of information fusion based on multi-sensor measurement system[16,17]is a popular method to extend measurement range, improve information redundancy and credibility, through the synergy between sensors. Therefore, a consistency fusion strategy is proposed to process the multi-sensor measurement through constructing consistency matrix. On this basis, a cubature Kalman probability hypothesis density filter based on multi-sensor consistency fusion is proposed.

The rest of the paper is organized as follows. In Section 1, the background information on PHD filter is presented. Section 2 proposes a cubature Kalman probability hypothesis density (CK-PHD) filter for single-sensor multi-target tracking under Gaussian mixture framework. Then, in Section 3, a consistency fusion strategy is established for fusing multi-sensor measurement through constructing consistency matrix. Furthermore, a new cubature Kalman probability hypothesis density filter based on multi-sensor consistency fusion (MC-CK-PHD) is proposed by introducing the fused measurement during update step in Section 4. The proposed algorithms are illustrated in Section 5 through a simulation example. Finally, conclusions are summarized in Section 6.

1 PHD filter

An optical Bayesian filter using RFS or point process for multi-target tracking is very computationally challenging, especially when the target number is large. To reduce complexity, Mahler devises PHD filter as an approximation of an optimal multi-target Bayesian filter. And it propagates the first-order statistical moment of the posterior multi-target state, i.e., the posterior density is propagated in PHD filter. Let the posterior density equal to Ik-1|k-1(xk-1|Z1:k-1) at time k. The recursion steps of PHD filter are as follows:

? Prediction steps:

Lk|k-1(xk|Z1:k-1)=γk(xk) + [∫pS,k(xk-1)fk|k-1(xk|xk-1) +∫βk|k-1(xk|xk-1)] ×Lk-1|k-1(xk-1|Z1:k-1)dxk-1

(1)

where γk(xk) is the intensity of target appearing at time k, pS,k(xk-1) is the target survival probability, fk|k-1(xk|xk-1) is the single target Markov transition density, and βk|k-1(xk|xk-1) is the intensity of spawning of target from existing ones.

? Update steps:

(2)

where ψ(zk|Z1:k-1)=∫pD,kf(zk|xk)Lk|k-1(xk|Z1:k-1), pD,k(xk-1) denotes the detection probability, f(zk|xk) is the single target likelihood function, λkand ck(zk) are the false alarm(clutter) intensity and false alarm spatial density, respectively.

The expected number of targets is given by

Nk|k=∫Lk|k(xk|Z1:k)dxk

(3)

The PHD filter completely avoids the combinatorial computation arising from the unknown association of measurements with appropriate targets. However, the closed-form solutions of recursion in PHD filter cannot be achieved in general which results in that it is difficult for PHD filter to realize engineering application. And numerical integration suffers from the “curse of dimensionality”[5]. In Ref.[3], it is shown that Gaussian mixture probability hypothesis density (GM-PHD) filter provides a closed-form solution for multi-target tracking without measurement-to-track data association.

2 Cubature Kalman probability hypothesis density filter

In this section, combining CKF with PHD under Gaussian mixture framework, a cubature Kalman probability hypothesis density (CK-PHD) filter is proposed for jointly estimating time-varying number and position of targets.

(4)

L=2n denotes the number of cubature points, and n denotes the dimension of estimated system state, ξjis the jth cubature point.

The GM-PHD filter propagates the multi-target posterior density through Gaussian mixture components, providing a closed-form solution under the three assumptions. The mathematical express of the three assumptions is given[18]:

fk|k-1(xk|xk-1)=N(xk; fk-1xk-1, Qk-1)

(5)

gk(zk|xk)=N(zk;hkxk,Rk)

(6)

pS,k(xk)=pS,k

(7)

pD,k(xk)=pD,k

(8)

(9)

(10)

where, J and ω are the number and the weight of Gaussian mixture components, respectively.

? Prediction steps:

The predicted intensity for time k is also a Gaussian mixture and is given by

Lk|k-1(xk|Z1:k-1)=LS,k|k-1(xk|Z1:k-1) +Lβ,k|k-1(xk|Z1:k-1)+γk(xk)

(11)

(12)

(13)

(14)

(15)

(16)

(17)

State one-step prediction and its error covariance of the existing targets

(18)

(19)

State one-step prediction and its error covariance of the spawned targets

(20)

(21)

? Update steps:

(22)

(23)

(24)

(25)

(26)

(27)

(28)

(29)

(30)

(31)

(32)

(33)

(34)

(35)

(36)

3 Consistency fusion strategy

In the situation of multi-sensor measurement system, redundant and complementary information is extracted and utilized as much as possible to reduce the dependence of measurement noise statistics information. In this paper, the consistency distance and consistency matrix is built to characterize the mutual support degree between multi-sensor measurements. On this basis, the consistency fusion strategy for multi-sensor measurement is established through constructing consistency matrix. The elements in the matrix denote the mutual support degree. The measurement weights are allocated legitimately to utilize measurement effectively in fusion process.

Considering the matrix of mutual support degree between multi-sensor measurements, the graphical representation of confidence distance is in Fig.1, and the equation is defined as

(37)

Fig.1 Consistency distance

(38)

(39)

(40)

where the consistency matrix Ψkand weight coefficient vector αkare expressed

(41)

(42)

The numerical characteristic of the elements in Ψkshows: all diagonal elements are equal to 1, so Ψkis a positive definite symmetric matrix. The other elements in this matrix are positive and not greater than 1. According to Perron-Frobenius theorem: there is a maximum modulus eigenvalue λk>0. Only when all elements in eigenvector corresponding to eigenvalue λkare positive, λkβk=Ψkβk. Let αk=βk, combined with Eq.(40), then

(43)

(44)

(45)

The fused measurement noise variance is

(46)

Combining the above analysis, the pseudo-code of consistency fusion is given as follows:

Algorithm1：Consistencyfusiongiventhemulti?sensormeasurement{zik|zik=h(xk)+vik，i=1，2，…，N}calculatetheconfidencedistancefori=1，…，N forj=1，…，N Rijk=(zik-zjk)Τ(zik-zjk)/(Rvik+Rvjk) endendcalculatetheconsistencydistancefori=1，…，N forj=1，…，N Θijk=1-Rijk/max(max(Rijk)) endendfindthemaximumeigenvalueandcorrespondingeigenvectorofconsistencydistance[β，λ]=eig(Ψk)m=max(max(λ))calculatetheweightωikofzik，andnormalizationfori=1，…，N αik=abs(β(i，m))endαik=αik/∑Ni=1αikmeasurementfusionfori=1，…，N ＾z′k=∑Ni=1αikzikend

4 Cubature Kalman probability hypothesis density filter based on multi-sensor consistency fusion

A CK-PHD filter is extended to multi-sensor case. Assume that there are N sensors and that the measurement noises with the same covariance are irrelevant Gaussian white noise. Then consistency fusion strategy is designed to obtain fusion measurement. Based on the above work, a cubature Kalman probability hypothesis density filter based on multi-sensor consistency fusion is proposed. The key steps of MC-CK-PHD filter are given as follows:

Algorithm2：CubatureKalmanprobabilityhypothesisdensi?tyfilterbasedonmulti?sensorconsistencyfusiongiven{ω(i)k-1，m(i)k-1，P(i)k-1}Jk-1i=1andthemeasurementsetZk．step1．predictionforbirthtargets i=0． forj=1，…，Jγ，k i：=i+1 ω(i)k|k-1=ω(j)γ|k，x(i)k|k-1=x(j)γ|k，P(i)k|k-1=P(j)γ|k． end forj=1，…，Jβ，k forl=1，…，Jk-1 i：=i+1 ω(i)k|k-1=ω(l)k-1ω(j)β，k ＾x(i)k|k-1=f(j)β|k-1＾x(l)β|k-1+v(j)β|k-1 P(i)k|k-1=Q(j)γ|k-1+f(j)β|k-1P(l)β|k-1f(j)β|k-1Τ end end forj=1，…，i setμ：=＾x(j)k|k-10é?êêù?úú，C：=P(j)k|k?100Rké?êêù?úú usethethird?degreespherical?radialcubatureruletogenerateasetofcubaturepointswithmeanμ，covarianceC，andweightsdenotedby{y(l)k，μ(l)}Ll=1． z(l)k|k-1：=hk(x(l)k|k-1，ε(l)k)，l=1，…，L． η(j)k|k-1=∑Ll=1μlz(l)k|k-1 P(j)zz，k=∑Ll=1μl(z(l)k|k-1-η(j)k|k-1)(z(l)k|k-1-η(j)k|k-1)Τ P(j)xz，k=∑Ll=1μl(z(l)k|k-1-＾x(j)k|k-1)(z(l)k|k-1-η(j)k|k-1)Τ K(j)k=P(j)xz，k[P(j)zz，k]-1 P(j)k|k=P(j)k|k-1-K(j)k[P(j)xz，k]Τ endStep2．constructionofexistingtargetcomponents forj=1，…，i i：=i+1 ω(i)k|k-1=pS，kω(i)k-1|k-1 setμ：=＾x(j)k|k-100é?êêêù?úúú，C：=P(j)k|k-1000Qk-1000Rké?êêêêù?úúúú usethethird?degreespherical?radialcubatureruletogenerateasetofcubaturepointswithmeanμ，covarianceC，andweightsdenotedby{y(l)k，μ(l)}Ll=1．

5 Simulation results and analysis

where, ω=0.025rad/s is the angular acceleration of targets, Τ=1 is the sampling period. pS,k=0.99, pD,k=0.98, U=5, Jmax=100, T_prun=10e-5.

Table 1 The rest initial value of parameters in the algorithm

Fig.2 Measurement and true trajectories

(a) EK-PHD

(b) UK-PHD

(c) CK-PHD

(d) MC-CK-PHD

Fig.3 The target trajectories and their estimations of (a) EK-PHD, (b) UK-PHD, (c) CK-PHD and (d) MC-CK-PHD

The proposed algorithm is compared with EK-PHD filter and UK-PHD filter presented in Ref.[5]. The results and analysis of simulation are given below.

The measurement and the real trajectories of the targets are given in Fig.2. Note that square marks and circle marks denote the initial position and final position of targets, respectively.

To verify the effectiveness of the proposed algorithm, Fig.3 gives the target trajectories and their estimations of (a) EK-PHD, (b) UK-PHD, (c) CK-PHD and (d) MC-CK-PHD. The figures illustrate that state estimation through MC-CK-PHD filter approximates real trajectories mostly.

Fig.4 illustrates the comparison of the four algorithms estimation precisions of the number of targets. The plots demonstrate that both CK-PHD filter and MC-CK-PHD filter are superior to EK-PHD filter and UK-PHD filter for estimating the number of targets. Meanwhile, the MC-CK-PHD filter is more reliable than CK-PHD filter because consistency fusion strategy in MC-CK-PHD filter makes sure that fused measurement is more precise than single-sensor measurement does. For quantitative comparison, Table 2 gives the average estimation error of targets number through the four algorithms after 50 simulations. It is clear that the EK-PHD filter and UK-PHD filter have the average estimation error of 9.20 and 9.22 respectively, and the error of CK-PHD filter and MC-CK-PHD filter are 8.06 and 8.02, respectively. The results further suggest that the average estimation error of MC-CK-PHD filter is the lowest, namely, MC-CK-PHD filter outperforms others in targets number estimation.

To verify the capability of proposed algorithm more clearly, Fig.5 gives the comparison of average OSPAs of EK-PHD filter, UK-PHD filter, CK-PHD filter and MC-CK-PHD filter after 50 Monte Carlo simulations. It shows that the average OSPA of CK-PHD filter is lower than EK-PHD’s and UK-PHD’s, and that the average OSPA of MC-CK-PHD filter is the smallest in all. Fig.5 also illustrates that both CK-PHD filter and MC-CK-PHD filter have the advantage of position estimation precision. Further, MC-CK-PHD filter is superior to CK-PHD filter. Table 3 gives the comparison of the total OSPAs of all step time.

(a) EK-PHD

(b) UK-PHD

(c) CK-PHD

(d) MC-CK-PHD

Fig.4 Real number of targets and their estimation of (a) EK-PHD, (b) UK-PHD, (c) CK-PHD and (d) MC-CK-PHD

Table 2 The comparison of average estimation error of the four algorithms for targets number

Fig.5 The comparison of OSPAs

AlgorithmsEK?PHDUK?PHDCK?PHDMC?CK?PHDOSPAsummation121．8744121．8126103．254171．4176

6 Conclusions

In this study, the multi-target tracking problem on estimation precision in linear is considered under PHD filter framework. Combined with the advantaged of CKF, CK-PHD filter is proposed based on single-sensor measurement system. And it is a generalized solution for estimating targets number and position. Furthermore, a consistency fusion strategy is established, and introduced into the CK-PHD filter. On this basis, the implementation denoted as MC-CK-PHD filter has been presented. Simulation results show that the CK-PHD filter and MC-CK-PHD filter outperform the published EK-PHD filter and UK-PHD filter in the scenario with time-varying number of multi-targets. Meanwhile, the MC-CK-PHD filter is superior to CK-PHD filter in targets number estimation and position estimation.

[ 1] Mahler R. Multitarget Bayes filtering via first-order multitarget moments. IEEE Transactions on Aerospace and Electronic Systems, 2003, 39(4): 1152-1178

[ 3] Ba-Ngu V, Sumeetpal S, Doucet A. Sequential Monte Carlo methods for multitarget filtering with random finite sets. IEEE Transactions on Aerospace and Electronic Systems, 2005, 41 (4): 1224-1245

[ 4] Baser E, Efe M. A novel auxiliary particle PHD filter. In: Proceedings of the 15th IEEE International Conference on Information Fusion, Singapore, 2012, 165-172

[ 5] Ba-Ngu V, Ma W K. The Gaussian mixture probability hypothesis density filter. IEEE Transactions on Signal Processing, 2006, 54(11): 4091-4104

[ 6] Pasha S A, Ba-Ngu V, Hoang D T, et al. A Gaussian mixture PHD filter for jump Markov system models. IEEE Transactions on Aerospace and Electronic Systems, 2009, 45(3): 919-936

[ 7] Clark D, Ba-Ngu V. Convergence analysis of the Gaussian mixture PHD filter. IEEE Transactions on Signal Processing, 2007, 55(4): 1204-1212

[ 8] Tang X, Chen X, McDonald M, et al. A multiple-detection probability hypothesis density filter. IEEE Transactions on Signal Processing, 2015, 63(8): 2007-2019

[ 9] Melzi M, Ouldali A, Messaoudi Z. Multiple target tracking using the extended Kalman particle probability hypothesis density filter. In: Proceedings of the 18th European Signal Processing Conference, Aalborg, Denmark, 2010, 1821-1826

[10] Kurian A P, Puthusserypady S. Performance analysis of nonliner predictive filer based on chaotic synchronization. IEEE Transactions on Circuits & Systems II: Express Briefs, 2006, 53(9): 886-890

[11] Melzi M, Ouldali A, Messaoudi Z. The unscented Kalman particle PHD filter for joint multiple target tracking and classification. In: Proceedings of the 19th International Conference on Signal Processing, Barcelona, Spain, 2011, 1415-1419

[12] Gustafsson F, Hendeby G. Some relations between extended and unscented Kalman filters. IEEE Transactions on Signal Processing, 2012, 60(2): 545-555

[13] Julier S J, Uhlmann J K. Unscented filtering and nonlinear estimation. Proceedings of the IEEE, 2004, 92(3):401-422

[14] Arasaratnam I, Haykin S. Cubature Kalman filters. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269

[15] Wang H, Yu D. Jiang J. Comparison and error analysis of integral-free Kalman tracking filter algorithms. In: Proceedings of the 7th IEEE International Conference on Image and Signal Processing, Dalian, China, 2014, 783-787

[16] Bar-Shalom Y, Li X R. Multitarget multisensor tracking: principles and techniques. IEEE Systems Magazine on Aerospace and Electronic, 1996, 16(4):93

[17] Mahler R P S. Statistical Multisource Multitarget Information Fusion. USA: Artech House, 2007

[18] Chakravorty R, Challa S. Multitarget tracking algorithm-joint IPDA and Gaussian mixture PHD filter. In: Proceedings of the 12th IEEE International Conference on Information Fusion, Seattle, USA, 2009, 316-323

Hu Zhentao,born in 1979. He received his Ph.D degrees in Control Science and Engineering from Northwestern Polytechnical University in 2010. He also received his B.S. and M.S. degrees from Henan University in 2003 and 2006 respectively. Now, he is an assistant professor of college of computer and information engineering, Henan University. His research interests include complex system modeling and estimation, target tracking and particle filter, etc.

10.3772/j.issn.1006-6748.2016.04.006

① Supported by the National Natural Science Foundation of China (No. 61300214), the Science and Technology Innovation Team Support Plan of Education Department of Henan Province (No. 13IRTSTHN021), the Post-doctoral Science Foundation of China (No. 2014M551999), and the Outstanding Young Cultivation Foundation of Henan University (No. 0000A40366).

② To whom correspondence should be addressed. E-mail: hym_henu@163.com Received on Oct. 12， 2015