A Real-Time Multiple Target Tracking Algorithm Using ... - IEEE Xplore

0 downloads 0 Views 543KB Size Report
combines the merged probabilistic data association (MPDA) technique together with the smoothing particle filter to track multiple targets. The MPDA approach ...
A Real-Time Multiple Target Tracking Algorithm Using Merged Probabilistic Data Association Technique and Smoothing Particle Filter Hazem Kamel

Wael Badawy

Electrical and Computer Engineering University of Calgary Calgary, AB, Canada [email protected]

Electrical and Computer Engineering University of Calgary Calgary, AB, Canada [email protected]

Abstract— In this paper, we present a tracking system that combines the merged probabilistic data association (MPDA) technique together with the smoothing particle filter to track multiple targets. The MPDA approach combines the probabilistic nearest-neighbor filter (PNNF) together with the probabilistic data association (PDA) approach, in the data association step, to track multiple targets in dense clutter environment. Due to the high uncertainty when applying a particle filter to track a maneuverable target, the smoothing particle filter is used. Results show that combining MPDA together with smoothing particle filter can achieve a robust and real-time tracking system for tracking multiple targets even in dense clutter environment.

I.

II. A.

Background There are two main approaches for data association: Nearest-Neighbor (NN) approach and All-Neighbors (AN) Approach. In the NN approach, at most – if present – one observation can be used to update some track. This approach is based on minimizing a distance function between the predicted position and the actual measurements lying within its neighborhood (gate). We may define two types of distance functions: (1) NN with minimum statistical distance (NN-MD) and (2) NN with normalized distance (NN-ND) [1].

INTRODUCTION

Meanwhile, the Probabilistic Data Association (PDA) technique is considered to be the first technique based on AN approach [2] followed by the Joint PDA (JPDA) to deal with the MTT problem [3]. However, due the computational complexity engendered with the use of JPDA, several algorithms have been introduced, e.g. Ad hoc JPDA [4], suboptimal JPDA [5], integrated PDA [6], and dominant PDA [7].Recently, the Interacting Multiple Models (IMM) together with PDA-based estimator showed more improvements in solving the data association problem [8]. Unfortunately, the IMM-PDA has more computational complexity than the JPDA itself because it combines two PDA algorithms. Meanwhile, the PDA failed to track targets with low Signal-to-Noise Ratio (SNR) when compared to Viterbi Data Association (VDA) and Fuzzy Data Association (FDA) techniques as mentioned in [9].

Tracking multiple objects depends on the cooperation between a good data association approach together with a robust tracker. Data association is the subject that deals with the integration of measurements (observations) from one or more targets. Each measurement may originate from one outof-several object or from false detection, e.g. clutter, noise, or jamming. The uncertainty in measurement origin is a complicated factor, in addition to the statistical in the values of measurements. In order to estimate the states of the objects (e.g., position and velocity vectors), we need to resolve this uncertainty. If an incorrect observation is associated with a track, it could diverge this track and causes other tracks to diverge also. Thus, data association is considered the key element and the most important component of any multiple objects tracking system. In this paper, we use the Merged Probabilistic Data Association (MPDA) technique in tracking multiple targets. Section 2 presents an overview on the various data association techniques as well as the MPDA technique. In section 3, both conventional and smoothing particle filter are presented. The proposed algorithm is described in section 4 followed by target models formulation in section 4. Finally, performance analysis and simulation results are shown in section 5 followed by a summary and conclusion. 0-7803-9497-6/06/$20.00 © 2006 IEEE.

DATA ASSOCIATION TECHNIQUES

B. Probabilistic Nearest-Neighbor Filter (PNNF) The Nearest-Neighbor Filter (NNF) algorithm is driven in two steps: (1) prediction step identical to that used in Kalman filter and (2) update step as follows [10]: (a) For the case of M0:

218

xˆ k = x k

(

PD PG 1 − Cτ

Pˆk = Pk +

g

)K S K k

1 − PD PG

k

T k

α=

(1)

1 − PD PR ( D)Cτ ( D) 1 − PD PR ( D) PD e

β1 =

(b) For the case of MT and MF ( M 0 ):

PD e

xˆ k = x k + K k (z k − H k x k ) Pˆk = Pk − K k S k K kT



n 2

n



(6)

n 2

1

+ (2π ) 2 S k 2 λ (1 − PD PR ( D) )

where PR(D) is the probability of the target within an elliptic gate with gate size D and Cτ(D) are given by the following equations:

(2)

where PD is the probability of detection, PG the probability of the target within the validation gate, and Cτ the constant g

covariance ratio. PG and Cτ are defined by:

PR ( D) =

g

PG =

1

g

−q 2

∫ q e dq n 2 2 Γ (n / 2) 0

Cτ ( D ) =

(3)

∫ q e dq n

−1

where γ is the gate size, Γ (n / 2) = (2π n / 2 )/ nc n , and c2=π for n = 2 in a 2-dimensional space. The target of interest is assumed to be detectable. The PNNF was introduced in [10] to overcome the drawback of the NNF. It takes into account the probabilities of the possible events engendered by the data association with the nearest-neighbor measurement {MT, MF, M0}. The PNNF is summarized in two steps: (1) prediction step identical to that in Kalman filter algorithm and (2) update step as follows:

(

PD PG 1 − Cτ 1 − PD PG

g

)K S K k

k

T k

Dmn =

(4)

q

(7)

∫ q 2 e 2 dq 0

D

n

−1



q

(b) For the case of M 0 xˆ k = x k − K k β1v k Pk M = Pk − K k S k K kT + αK k S k K kT Pˆk = Pk M β 0 + Pk − K k S k K kT β1 + β 0 β1 K k vk vkT K kT F

(xˆm − xn )2 + (yˆ m − yn )2

(8)

where Dmn is the distance from observation n to track m, xˆ m is the predicted x coordinate, yˆ m is the predicted y coordinate, xn is the measured x coordinate, and yn is the measured y coordinate. Then, the distance function Dmn is used to weight each probability of the measurement n to be associated with the track m such that

S k = H k Pk H kT + Rk

(

0

Consider N measurements exist within the gate of the track m. These N measurements are reflected echoes from the target of interest or other targets, noise, and/or clutter. Let us define the distance function to be Dmn as given in the NN-MD approach to be

(a) For the case of M0

F

q

C. Merged Data Association Technique The Merged Probabilistic Data Association (MPDA) technique is based on the idea of merging the JPDA algorithm together with the NN approach. Its main idea is based on a weighted sum of all observations within the predicted position gate. This weight depends on the statistical distance between each measurement and the predicted position such that the nearest measurement has the highest weight and vice versa.

0

Pˆk = Pk +





0

−q

n ∫ q 2 e 2 dq

xˆ k = x k

n

n

2 2 ∫ q e dq

n ∫ q 2 e 2 dq

0

γ

D

2 Γ (n / 2) D

−q 2

n 2

γ

Cτ =

n −1 2

γ

1 n 2

(5)

)

where Pk M is the update error covariance conditioned on MF, vk is the residual of the NNF measurement. Assuming n=2 for 2-dimensional space, D is the normalized distance squared (NDS) of the NNF measurement, λ is the spatial clutter density, β0 is the probability that the NN measurement is not target originated (β0=1-β1), α and β1 can be given by:

0  N Pmn = 1 − Dmn ∑ Dmn n =1  N −1 

F

n = 0 (no valid measurements)

(9)

1≤ n ≤ N

with N

∑ Pmn = 1

n =1

219

(10)

where missing lt(y;x)∝ p(Yt = yt | X t = xt ) normalization should not depend on x. The first particle set S0 is created by drawing N independent realizations from p(X0) and assigning uniform weight 1/N to each of them. Then, suppose we dispose at time t-1 of the particle set S t −1 = (s tn−1 , q tn−1 )n =1,..., N

where Dmn is given in (8). We can notice that Pmn increases as Dmn decreases in (9). This results in high-weighting the measurements with coordinates (xn , yn) that are close to the predicted position ( xˆ m , yˆ m ) ; meanwhile low-weighting those measurements far from the predicted position. III.

BASIC AND SMOOTHING PARTICLE FILTER

N

where ∑ qtn−1 = 1 . Posteriori marginal Lt-1 is then estimated by

nx

Assuming (Xt)∈ R a stochastic process with state equation: X t = Φ t (X t −1 , Vt ) (11) Therefore, the measurement equation is given by Yt = H t (X t ,Wt ) (12)

n =1

N

the probability density LS = ∑ qtn−1δ s . Then, the weight of t −1

each particle is updated during the correction step. In practice, the particle set is finite and the major drawback of this algorithm is the degeneracy of the particle set: only few particles keep high weights and the others have very small ones. The resampling is a good way to eliminate this drawback because it cancels the particles of smallest weights. To measure the degeneracy of the algorithm, the effective sample size Neff has been defined in [11,12] by N 2 Nˆ = 1 ∑ (q n ) (19)

where (Vt)∈ R n and (Wt)∈ R n are either white or colored noises and Φt and Ht do not follow any linearity hypothesis. Y0:t denotes the sequence of the random variables (Y0,…,Yt) where y 0:t is one realization of this sequence. The conditional density Lt of the state Xt is defined by Lt = p(X t | Y0 = y 0 , ..., Yt = y t ) . Moreover, estimating any functional of the state g(Xt) is given by the expectation E [g (X t ) | Y0:t ] . The Recursive Bayesian filter resolves exactly this problem in prediction and correction steps at each time t. Assume Lt-1 is known. The prediction step will be: p(X t = x t | Y0:t −1 = y 0:t −1 ) (13) = ∫ p(X t = x t | X t −1 = x )Lt −1 ( x) dx v

w

eff

∫ p(X t = x t | X t −1 = x , Vt = v ) p(Vt = v | X t −1 = x )dv

R nv

= ∫ δ (x t − Φ t (x, v )) p (Vt = v ) dv

t

n =1

The resampling is then done only if Nˆ eff < N threshold [13]. The particle filter algorithm is given in the Table I. However, due to the high uncertainty and incompleteness of the information in maneuvering target-tracking problem weakens the particle filter. To overcome this weakness, both smoothing and auxiliary particle filter were introduced in [14,15]. Although both filers solved the problem of tracking a maneuverable object, the smoothing particle filter showed better performance and efficiency [16]. Consequently, we applied it in this paper. Its algorithm combines the particle filter while smoothing of the PDF of system modes; which settles the maneuverability of the target. Table II shows the main idea of the smoothing particle filter.

Rnx

Using (11), we can calculate p(X t = xt | X t −1 = x ) : p(X t = x t | X t −1 = x ) =

n t −1

n =1

(14) (15)

R nv

where δ(x) is the Dirac distribution. Baye’s rule is used to figure Lt in the correction step p(Yt = yt | X t = xt )p(X t = xt | Y0:t −1 = y 0:t −1 ) Lt (xt ) = (16) p(Yt = yt | Y0:t −1 = y0:t −1 ) Applying (12), we can rewrite p(Yt = yt | X t = xt ) as

TABLE I.

BASIC PARTICLE FILTER

for n=1,…,N do Generate a random sample v tn from p(Vt).

Compute s tn|t −1 = Φ t (s tn−1 , v tn ) end for Correction for n=1,…,N do l t yt ; stn|t −1 qtn−1 Compute qtn = N n n ∑ l t y t ; st|t −1 qt −1

p(Yt = y t | X t = x t )

(17) = ∫R δ (Yt − H t (x t , w)) p(Wt = w)dw As well, the denominator in (16) could be expressed as follows: p(Yt = y t | Y0:t −1 = y 0:t −1 ) (18) = ∫R p(Yt = y t | X t = x ) p(X t = x | Y0:t −1 = y 0:t −1 ) dx The application of the particle filter requires the knowledge of how to: • sample from initial prior marginal p(X0), • sample from p(Vt) for all t, and • to compute p(Yt = yt | X t = xt ) for all t through known function lt such that nw

(

n =1

nx

)

(

end for Estimation N

)

( )

Estimate E{xt} by Eˆ {xt }= ∑ qtn g stn|t −1 Effective size estimation N

( )

Calculate Nˆ eff = 1 ∑ qtn n =1

220

n =1

2

IV.

Resampling if Nˆ eff < N threshold then

Let M be the numbers of objects to track m. Therefore, the initial particle set S 0 = (s 0n , 1 N ) where n=1,…,N. Assume that the state vector at a time t is defined by X t = (X t1 ,..., X tM ) . Consequently, the state equation (11) and the measurement equation (12) will be written as X ti = Φ ti (X ti−1 , Vt i ) ∀i = 1,..., M (20)

for n=1,…,N do N

Draw stn from ∑ qtk δ s k t |t −1 k =0

Set qtn = 1 / N end for else for n=1,…,N do s tn = stn|t −1 end for end if

Yt j = H ti (X ti , Wt j )

SMOOTHING PARTICLE FILTER

mode

(

) ( )

p µ1i | Y1 = p µ j ,

probabilities

weighted random particles sik,n (n=1,...,N) as mentioned in [19]. A sample set constitutes a discrete approximation of a PDF. Each sample is given by (xik,n , wik. n ) where xik,n is the

)}

state vector and wik,n is an importance factor. The prediction step of Bayesian filtering is realized by drawing samples from the set computed in the previous iteration and by updating their state according to the prediction model p(xik | xik −1 ) . Meanwhile, a measurement Y(k) is integrated into the samples obtained in the prediction step to fulfill the correction step. In the MPDA, the probabilities Pij are considered in the correction step such that

white noise PDF. Compute xti+1 = E xti+1 and N µ i

{ }

Compute

the

t

posterior

(

N

)

smoothed

)

(

mode

1 kj kj ∑ p yt +1 | xt +1 π t +1 , where c j =1 c is the normalizing factor. Using the posterior mode probabilities, predict the particles again as in second step. Calculate the likelihood weights and 1 normalize π ti+1 = p yt +1 | xti+1 , where c’ is used to c' normalize the weights sum to 1. Calculate probabilities of the system mode at time step

probabilities p µ ti | Yt +1 =

(

t+1: p

(

µ ti+1

)

Nt

| Yt +1 ∝ ∑ p j =1

µ ti

p(µ ti+1 | Yt +1 ) ∝ ∑ p(µ ti+1 | µ ti ) p(µ t j | Yt +1 )Pij Nt

j =1

(

| µ ti

) p(µ

t

j

TABLE III.

)

| Yt +1 , i=1,…,N.

PROPOSED ALGORITHM

for every track m for n = 1 to N do Calculate the statistical distance Dmn from each measurement n to track m. Calculate the probability Pmn. Calculate p µ ti+1 | Yt +1 the probability of the system mode. end for Substitute p µ ti+1 | Yt +1 in the smoothing particle filter update equation. Obtain the estimated and predicted positions using

Perform resampling and roughening procedure of the set x ti+1 , π ti+1 i =1,..., N to overcome the poorness of the

(

(23)

which means the closer is the measurement to the predicted position the higher is the probability of the system mode. The whole algorithm can be explained in Table III.

)

µ ti+1

j

measurement y tj was issued from the ith object. Following the same idea of [17,18], we redefine lt by l ti ( y; x ) ∝ p(Yt j = y | K t j = i, X ti = x ) (22) As we are tracking an object moving in a cluttered environment, consequently the received measurement can be originated from either a true target or from clutter (false alarm). The false alarms follow a uniform distribution in the observation field. In our approach, the key idea underlying all particle filters is to represent the density p(xik | Y k ) by a set S ik of N

j=1,…,N. end for for i=1,…, N do Compute x ti+1 = Φ (x ti , µ ti , wt ) where µ ti is a sample drawn from the system mode set M with distribution p µ ti | Yt j =1,..., N and wt is a sample drawn from the

{(

(21)

where Kt∈ {1,...M } is a stochastic such that K t = i if the jth

for i=1,…, N do simulate a sample x1j from p(x1) with equal weights. initial

if K t j = i

mt

TABLE II.

set

MPDA WITH SMOOTHING PARTICLE FILTER

)

particle filter. end for Increase t and iterate to second step

(

(

221

)

)

smoothing particle filter model equations. end for V.

TARGET MODELS

Our main objective is to track multiple objects performing different trajectories. The state of the objects are defined by X k = [xk y k x& k y& k ] (24) where xk and yk represent the object’s position at time k; as well x& k and y& k represent the object’s velocity. Consider the scan rate is T. The maneuvering target system dynamics are given by (25) Xk+1 = Φ Xk + Wk where Wk is a vector Gaussian white noise process The transition matrix Φ is defined as follows: (1) Constant velocity model 1 0 T 0    0 1 0 T Φ = (26) 0 0 1 0    0 0 0 1  (2) Coordinated turn model  sin(θ&T ) cos(θ&T ) − 1  1 0 θ& θ&   1 − cos(θ&T )   & 0 1 sin(θT )  Φ = (27) θ&   & sin(θT )  0 0 cos(θ&T ) −   θ& &T ) &T )  0 0 θ θ sin( cos(  

(c) Figure 1. Tracking overlaping trajectories

(a)

(b)

(c) Figure 2. Tracking large-angle crossing targets

where θ& denotes the turn rate in radians/second. The skate dancers are supposed to be moving with maximum speed 39 ft/sec and in an area of 195×82 ft2. VI.

SIMULATION RESULTS

(a)

To evaluate the performance of our approach, we performed extensive experiments with the targets’ models. Additionally, we carried out a series of Monte Carlo simulation experiments. All experiments demonstrate that our approach can accurately and reliably keep track of the moving objects even in difficult situations. The following figures show (a) the true trajectory of two targets, (b) the estimated trajectories compared to the true ones, and (c) the error between the true and estimated trajectories.

(a)

(b)

(c) Figure 3. Tracking small-angle crossing targets

(b)

(a)

222

(b)

[2]

[3]

[4]

(c) [5]

Figure 4. Tracking close targets moving in parallel

[6]

[7]

[8]

(a)

(b)

[9]

[10]

[11]

(c)

[12]

Figure 5. Tracking close targets moving in parallel [13]

VII. SUMMARY AND CONCLUSION In this paper we presented a new algorithm for keeping track of multiple objects. In order to avoid nonlinearity generated from maneuverability of those dancers, they are tracked using a smoothing particle filter. Moreover, to overcome conflict situation generated with difficult trajectories, Merged Probabilistic Data Association (MPDA) approach is applied to solve the problem of assigning measurements to each trajectory. By integrating particle filters with MPDA, our approach has the advantages of both: it can track nonlinear models of the individual objects while still being able to efficiently solve the data association problem. The technique has been implemented and evaluated on real moving objects as well as in Monte Carlo simulation runs. It can be shown that the system converges to the steady-state error rapidly. Consequently, it can be applied for real-time applications.

[14]

[15]

[16]

[17]

[18]

[19]

REFERENCES [1]

S. Blackman, Multiple Target Tracking with Radar Applications, Artech House, 1986.

223

Barry Y. Bar-Shalom and E. Tse, “Tracking in a cluttered environment with probabilistic data association,” Automatica, vol. 11, September 1975, pp. 451-460. T. Fortemann, Y. Bar-Shalom, and M. Scheffe, “Multi-target tracking using joint probabilistic data association,” Proceeding of IEEE Conference on Decision and Control, December 1980, pp.807-812. R. Fitzgerald, “Development of practical PDA logic for multitarget tracking by microprocessor,” in Multi-Target Multi-Sensor Tracking: Advanced Application, edited by Y. Bar-Shalom, Artech House, Norwood, MA, 1990, pp. 1-23. J. Rocher and G. Phillis, “Suboptimal joint probabilistic data association,” IEEE Transactions on Aerospace and Electronic Systems, vol. 29, No. 2, April 1993, pp. 510-517. D. Musicki, R. Evans, and S. Stankovic, “Integrated probabilistic data association,” IEEE Transactions on Automatic Control, vol. 39, No. 6, June 1994, pp. 1237-1241. Q. Pan, H. Zang, and Y. Xiang, “Multitarget tracking using dominant probability data association,” Proceedings of the American Control Conference, January 1994, pp. 1047-1050. S. Blackman and R. Popoli, Design and Analysis of Modern Tracking Systems, Norwood, MA: Artech House, 1999. A. Gad, F. Majdi, and M. Farooq, “A comparison of data association techniques for target tracking in clutter,” Proceedings of the 5th International Conference on Information Fusion, 2002, vol. 2 , 8-11 July, 2002, pp.1126 – 1133. X.R. Li, “The PDF of nearest-neighbor measurement and a probabilistic nearest-neighbor filter for tracking in clutters,” Proceedings of the 32nd CDC, Texas, Dec. 1993, pp. 918-923. A. Kong, J.S. Liu, and W.H. Wong, “Sequential imputation method and Bayesian missing data problems,” J. Amer Statist. Assoc., 89:278-288, 1994. J.S. Liu, “Metropolized independent sampling with comparison to rejection sampling and importance sampling,” Statistics and Computing, 6:113-119, 1996. D. Lurie and H.O. Hartley, “Machine-generation of order statistics for Monte Carlo computations,” The American Statistics, 26:26-27, February 1972. Y.Q. Li, Y. Shen, and Z.Y. Liu, “A new smoothing particle filter to track a maneuvering target,” Proceedings of the Second International Conference on Machine Learning and Cybernetics, Wan, pp. 10041008, 2-5 November, 2003. R. Karlsson and N. Bergman, “Auxiliary particle filters for tracking a maneuvering target,” Proc. of the 39th IEEE Conference on Decision and Control, Sydney, Australia, pp. 3891-3895, December. H. Kamel and W. Badawy, “A comparison between the smoothing and auxiliary particle filter in tracking a highly maneuverable object in a multiple-sensors network,” Proceedings of SPIE on Acquisition, Tracking, and Pointing XIX, vol. 5810, March 29, 2005, pp. 74-81. R. Streit and T. Luginbuhl, “Maximum likelihood method for probabilistic multi-hypothesis tracking,” in Proceedings of SPIE International Symposium, Signal and Data Processing of Small Targets 1994, vol. 2335, 1994. Carine Hue, Jean-Pierre Le Cadre, and Patrick Pérez, “A particle filter to track multipple objects,” in IEEE Workshop on Multi-Object Tracking, Vancouver, Canada, July 2001, pp. 61-68. D. Schulzl, W. Burgard, D. Fox, and A. B. Cremers, “Tracking multiple moving targets with a mobile robot using particle filters and statistical data association,” Proceedings of the 2001 IEEE International Conference on Robotics 8 Automation, Seoul, Korea. May 21-26, 2001.