Multiple Target Tracking using Sequential Monte Carlo Methods and Statistical Data Association Oliver Frank1 , Juan Nieto2 , Jose Guivant2 , Steve Scheding2 1
Swiss Federal Institute of Technology Zurich (ETH), Switzerland
[email protected]
2
Australian Centre for Field Robotics, The University of Sydney, Australia {j.nieto,jguivant,scheding}@acfr.usyd.edu.au
Abstract This paper presents two approaches for the problem of Multiple Target Tracking (MTT) and specifically people tracking. Both filters are based on Sequential Monte Carlo Methods (SMCM) and Joint Probability Data Association (JPDA). The filters have been implemented and tested on real data from a laser measurement system. Experiments show that both approaches are able to track multiple moving persons. A comparison of both filters is given and the advantages and disadvantages of the two approaches are presented.
The first approach is based on [1] and the coupled version was derived by [2] as Monte Carlo JPDA (SIR/MCJPDA). This paper is organised as follows. Section 2 presents the technical background for this paper. The two proposed filters are examined in Section 3 and Section 4. Off-line applications of real laser data obtained from a range-bearing laser are presented in Section 5 and a comparison of both filters is given.
2 Background 1 Introduction Multiple Target Tracking (MTT) is used to extract information from a dynamic system. Examples include the prediction of stock prices, monitoring of trucks in mining applications, radar tracking of military vehicles and tracking of people for monitoring and surveillance systems. In this paper two filters for tracking multiple moving targets are presented. Both filters are based on Sequential Monte Carlo Methods (SMCM) and Joint Probability Data Association (JPDA). SMCM is used to solve nonlinear, non-Gaussian elements of a dynamic system. By combining the SMCM approach together with JPDA, it is possible to handle multiple hypotheses. In conflict situations this is of great advantage compared to single hypothesis methods. In the two filters, the state estimation is carried out in two different ways by considering the following different assumptions: • The states of the targets conditioned on the past observations are mutually independent. This leads to the Independent Sample Based Joint Probabilistic Data Association Filter (ISBJPDAF). • Considering the targets’ states, conditioned on the past observations, as correlated, gives the Coupled Sample Based Joint Probabilistic Data Association Filter (CSBJPDAF).
In the following section, the problem of multiple target tracking is introduced and the technical background is presented. Consider the evolution of a state sequence {xk ; k ∈ N} of a target, given by xk+1 = fk (xk + uk ) + vk ,
(1)
where xk ∈ Rn is the state of interest at time step k, uk ∈ Rr a known control input, fk (·, ·) a mapping of the states and control input at time step k to the states at time step k + 1, and vk a random vector describing both dynamic driving noise and uncertainty in the state transition model itself. The objective of tracking is to recursively estimate xk from measurements given by the observation model zk = hk (xk , uk ) + wk ,
(2)
where zk ∈ Rmk is the observations made at time step k, hk (·, ·) is a mapping of state and control input to observations, and wk a random vector describing both measurement corruption noise and uncertainties in the measurement model itself. The tracking problem can be stated as follows. A state transition equation (1) describes the a priori distribution {xk ; k ∈ N} of a dynamic process and an observation equation (2) describes the likelihood of the observations {zk ; k ∈ N}, k being a discrete time index. In a Bayesian
framework, all information about x0:k ≡ {x0 , x1 , . . . , xk } given observations z1:k ≡ {z1 , z2 , . . . , zk } up to and including time step k can be obtained from the joint a posteriori distribution p(x0:k | z1:k ). In many applications we are interested in estimating recursively this joint a posteriori distribution and particularly its marginals p(xk | z1:k ).
An approximation of (4) is then given by
I N (gk ) = =
N
(i) (i) i=1 gk (x0:k ) w(x0:k ) N (i) 1 j=1 w(x0:k ) N
1 N
N i=1
2.1 Sequential Monte Carlo Method Over the last few years, particle filters have been applied with great success to a variety of state estimation problems including mobile robot localisation [3], visual tracking [4], simultaneous localisation and map building (SLAM) [5, 6], speech recognition and target tracking [1, 2]. A recent book provides a detailed overview of the state of the art [7]. For simplicity, it is assumed that the system’s states are modelled as Markovian, nonlinear and non-Gaussian. The aim is to estimate recursively the joint a posteriori distribution p(x0:k | z1:k ), the marginal a posteriori distribution p(xk | z1:k ) and the expectations I(gk )
= Ep(x0:k |z1:k ) [gk (x0:k )] = gk (x0:k ) p(x0:k | z1:k ) dx0:k
(3)
(i)
gk (x0:k ) w˜t (i)
(8)
(9)
where the normalised importance weights (i)
w(x ) w˜k (i) = N 0:k (i) j=i w(x0:k )
(10)
are evaluated by normalising the unnormalised importance weights (i)
(i)
p(z1:k | x0:k )p(x0:k )
(i)
w(x0:k ) =
(i)
q(x0:k | z1:k )
.
(11)
By factorising the proposal distribution q(x0:k | z1:k ) = q(x0 )
k
q(xj | x0:j−1 , z1:j ),
(12)
j=1
(4)
the weights can be updated recursively according to for any function of interest gk integrable with respect to p(x0:k | z1:k ). Mostly, we are interested in the conditional mean, in which case gk (x0:k ) = x0:k . (i)
We assume that N i.i.d random samples {x0:k }N i=1 , also called particles are given according to the joint a posteriori probability p(x0:k | z1:k ). This distribution is then estimated by PN ( dx0:k | z1:k ) =
N 1 (i) δ(x0:k − x0:k ), N i=1
(5)
where δ denotes the Dirac delta function. An estimation of (4) is given as
= =
=
(i) p(zk wk−1
gk (x0:k ) PN ( dx0:k | z1:k ) N 1 (i) gk (x0:k ). N i=1
(6) (7)
(i)
(i)
(i)
| xk ) p(xk | xk−1 ) (i)
(i)
q(xk | x0:k−1 , z1:k )
.
(13)
A common problem is the degeneracy phenomenon, whereby after a few iterations all but one particle will have negligible weight. This is explained by the KongLiu-Wong theorem [10]. To reduce the effect of degeneracy a good choice of the proposal distribution is important. Additionally, resampling is used whenever a noticeable degeneracy is observed. As a measure of degeneracy, the effective number of particles introduced in [10] can be approximated by: N ef f = N
i=1
IN (gk )
(i) wk
1 (w˜k (i) )2
,
(14)
(i)
where w ˜k is obtained from (10). In this paper, the a priori density is used as the proposal density (i)
q(xk | x0:k−1 , z1:k ) = p(xk | xk−1 ),
(15)
and substituting (15) into (13) yields Importance sampling [8, 9] is used because it is generally impossible to sample efficiently from the joint a posteriori distribution p(x0:k | z1:k ). An arbitrary proposal distribution q(x0:k | z1:k ), called importance sampling distribution or importance density is introduced.
(i)
(i)
(i)
wk = wk−1 p(zk | xk ).
(16)
Further, systematic resampling [11] was used since it is simple to implement and the computational cost is O(N ).
where c is a normalisation constant. Additionally, it is assumed that the measurements are detected independently of each other. The probability of the assignment θ conditioned on the sequence of the targets’ states P (θ | x1:k ) is approximated by m −(τ −n)
Figure 1: Example measurement to track association probˆi denote the predicted target posilem, where z tions and zi denote the measurement positions.
2.2 Gating Gating is used to select the measurements to be incorporated into the state estimator. This is done to avoid searching for the measurements in the entire measurements space and thus reduces unnecessary computations by the association functions. The normalised innovation dk is defined as d2 = νkT S−1 k νk ,
(17)
where νk is the innovation between the measurement zk ˆk = hk (ˆ and the predicted measurement z xk|k−1 ), and ˆ k denotes the predicted state. Sk is the innovation cox variance matrix according to Sk = E[νk νkT ]. By assuming that the measurement is of dimension M and that the true measurement is Gaussian distributed with covariance Sk around the predicted measurement, it follows that the M -dimensional Gaussian probability density function for the innovation is p(νk ) =
1 (2π)M/2 |Sk |1/2
exp
−d2k , 2
(18)
where |Sk | is the determinant of Sk . Thus the true measurement will be in the following gate Gk (γ) = {zk : d2 ≤ γ}
(19)
with probability determined by the gate threshold γ. 2.3 Joint Probability Data Association Data association is one of the most important problems in MTT. In this paper, the Joint Probabilistic Data Association (JPDA) approach is used, which is an extension of the PDA [12] approach for single targets. In the JPDA, a known number of targets τ is assumed. The index t designates one among the τ targets. The measurements k at time step k are denoted zk = {zk }m m=0 , where an artificial measurement z0 is introduced to handle false alarms or clutter and the number of measurements is given by mk . The measurement to target association probabilities are evaluated jointly across the targets. Let θ denote the joint association event (the time index k is omitted for simplicity) and θj,t is the particular event which assigns measurement j to the target t. By assume that the estimation problem is Markovian and by applying Bayes’ theorem, the joint association probabilities are 1 P (θ | z1:k ) = p(zk | θ, x1:k ) P (θ | x1:k ), c
(20)
τ −n (1 − PD )n PF Ak P (θ | x1:k ) = PD
,
(21)
where PD denotes the probability of detection, n is the number of z0 assignments and PF A denotes the probability of false alarm. The normalised innovation between the measurement j and the predicted measurement of target t is denoted by dj,t and Sj,t is the innovation covariance between measurement j and the predicted measurement of target t. By transforming (18) to pj,t =
−d2j,t 1 , exp 2 (2π)M/2 |Sj,t |1/2
(22)
the association likelihood that measurement j assigns target t is determined. Finally, the probability of an individual joint association event is given by m −(τ −n) τ −n P (θ | z1:k ) = PD (1−PD )n PF Ak pj,t . (23) θj,t ∈θ
A hypothesis table will be used to illustrate the required probability computations from the example based on Figure 1. Table 1 gives the probabilities associated with the various hypotheses after gating, where PD and PF A are assumed to be known constants. Track 1 0 0 0 0 2 2 2 3 3 3
Track 2 0 1 2 3 0 1 3 0 1 2
False Alarm 1, 2, 3 2, 3 1, 3 1, 2 1, 3 3 1 1, 2 2 1
P (θ | z1:k ) (1 − PD )2 PF3 A (1 − PD ) PD p12 PF2 A (1 − PD ) PD p22 PF2 A (1 − PD ) PD p32 PF2 A (1 − PD ) PD p21 PF2 A 2 PD p21 p12 PF A 2 PD p21 p32 PF A (1 − PD ) PD p31 PF2 A 2 PD p31 p12 PF A 2 PD p31 p22 PF A
Table 1: Hypotheses table for two tracks (τ = 2) and three measurements (mk = 3) as illustrated in Figure 1 and using the gating method.
3 Coupled Sample Based JPDA This method consists of estimating the state vector made by concatenating the state vectors of each target {x1k , . . . , xτk } = x1:τ k . At time step k, the tracking model is decomposed in τ pairs of partial equations: xtk ztk
= fk (xtk−1 ) + vkt = hk (xtk ) + wkt
∀t = 1, . . . , τ ∀t = 1, . . . , τ ,
(24) (25)
where t denotes one among the τ targets and the process noise vkt and the measurement noise wkt are considered to be independent. The probability of an individual joint association event is given by
m −(τ −n)
τ −n (1 − PD )n PF Ak P (θ | z1:k )(i) = PD
N 1 (i) p , N i=1 j,t
(29)
θj,t ∈θ
(i)
=
θj,t ∈θ
(i)
where pj,t is the sample based version of (22). The unnormalised weights in (11) are now determined by
m −(τ −n)
τ −n P (θ | z1:k ) = PD (1−PD )n PF Ak
pj,t , (26)
(i) wk
To determine P (θ | z1:k ), the following version of (23) is used:
(i)
P (θ | z1:k ) .
(27)
(i)
where pj,t is again the sample based version of (22). The sample weights are computed after (i),t wk
=
mk j=0
(i),t
βj,t p(zk,j | xk
).
(30)
∀θ
The CSBJPDAF algorithm can be summarised as follows:
The ISBJPDAF algorithm can be summarised as follows:
Algorithm 3.1 (CSBJPDAF) 1. Set k = 0, generate N samples from all targets t = 1, . . . , τ : (i),1 (i),τ (i),1:τ N (i),1:τ }i=1 . Thus x0 is {x0 , . . . , x0 } = {x0 a concatenation of the state vectors of all targets (i),t is drawn from p(xt0 ). t = 1, . . . τ , where x0
Algorithm 4.1 (ISBJPDAF) 1. Set k = 0, generate N samples from all targets t = (i),t is drawn from p(xt0 ). 1, . . . , τ independently. x0 2. Compute βj,t = θ: θj,t ∈ θ P (θ | z1:k ) as the summation over all the joint events in which the marginal event θj,t of interest occurs.
2. For each particle compute the weights for all measurementto track association (i) (i) and normalise for each wk = ∀θ P (θ | z1:k )
3. For each target t = 1, . . . , τ compute the indepen(i),t = dent weights for each particle according to wk mk (i),t β p(z | x ) and normalise the weights k,j k j=0 j,t
(i)
measurement: w ˜k = 3.
(i)
wk N i=1
(i)
wk
(i),t
.
for each target: w ˜k
(i∗),1:τ N }i=1 , by resampling Generate a new set {xk (i),1:τ N }i=1 , where with replacement N times from {xk (i∗),1:τ (i),1:τ (i) = xk )=w ˜k . P (xk (i),1:τ
4. Predict new particles: xk+1 for i = 1, . . . , N .
(i∗),1:τ
= f (xk
(i),1:τ
, vk
)
5. Increase k and iterate to item 2.
4 Independent Sample Based JPDA An approximation to reduce the computational costs is to use τ individual particle filters to track the different targets. The problem of measurement to target association can still be solved by using JPDA. By assuming that the states of the targets conditioned on the past observations are mutually independent, the marginal joint association probabilities have to be computed. They are obtained from the joint probabilities by summing over all the joint events in which the marginal event of interest occurs: θ: θj,t ∈ θ
P (θ | z1:k ).
wk N i=1
(i),t
wk
. (i∗),t
}N 4. For each target, generate a new set {xk i=1 , by (i),t N resampling with N times from {xk }i=1 , where (i∗),t (i),t (i),t = xk ) = w ˜k . P (xk (i),t
(i∗),t
(i),t
5. Predict new particles xk+1 = f (xk , vk 1, . . . , N for each target t = 1, . . . , τ .
) for i =
6. Increase k and iterate to item 2.
Obviously, the estimation of the combined state space has the disadvantage of computational costs. The complexity of this approach grows exponentially with the number of objects to be tracked.
βj,t =
(i),t
=
(28)
5 Experimental Results This section presents experimental results of the presented sample based JPDA filters. In particular the CSBJPDAF, summarised in Algorithm 3.1 and the ISBJPDAF, summarised in Algorithm 4.1 are applied to real data sets of a Sick LMS 200 range-bearing laser sensor. Both filters are tracking multiple moving objects (people walking around randomly). It is shown that the CSBJPDAF as well as the ISBJPDAF are able to track the people reliably. Finally a discussion and comparison of both algorithms is given. In the following experiment, target position (x, y) and velocity (vx , vy ) in Cartesian coordinates are used as states xk = {xk , yk , vx,k , vy,k } of the dynamic system at time step k. A very simple clustering algorithm is used to generate tentative measurements from the raw laser data. The discrete sample time is ∆T and the following con-
35
30
30
25
25
20
20
[m]
[m]
35
15
15
10
10 5
5 0 −20
−10
0
10
0 −20
20
−10
0
CSBJPDAF using 6000 particles at iteration 140 35
30
30
25
25
20
20
15
15
10
10
5
5
0 −20
−10
0
20
CSBJPDAF using 6000 particles at iteration 150
35
respectively. For the following experiments the variances are assumed to be σr = 0.15 m, σθ = 0.02 rad, σx = 1.5 m, σy = 1.5 m and ∆T = 0.2 s.
10 [m]
[m]
[m]
where the process and observation noise vk and wk respectively are assumed to be zero-mean Gaussian with known covariance 2 2 σx 0 σr 0 Q= and R = (33) 0 σy2 0 σθ2
CSBJPDAF using 6000 particles at iteration 130
CSBJPDAF using 6000 particles at iteration 120
[m]
stant velocity model is applied: ∆T 2 I2×2 ∆T I2×2 I2×2 2 xk+1 = xk + vk , (31) 02×2 I2×2 ∆T I2×2 x2k + yk2 (32) zk = + wk , arctan (yk2 /x2k )
10
0 −20
20
−10
0
10
20
[m]
[m]
Figure 4: The CSBJPDAF applied to track four people. (N = 6000, γ = 30, PD = 0.9, PF A = 0.1) ISBJPDAF using 6000 particles at iteration 120
ISBJPDAF using 6000 particles at iteration 130
35
35
30
30
25
25
20
20
[m]
[m]
An example raw data frame given by the Sick LMS 200 and the environmental setup are illustrated in Figure 2. The laser is positioned at the origin of the coordinate system. The estimated trajectories of the moving people are given in Figure 3, where the experiment was taken over approximately 50 seconds. Because of the single observation position a lot of occlusion situations occur. From the selected sequence given in Figure 4 and 5 it can be seen that both filters are tracking the targets reliably.
15
15
10
10
5
5
0 −20
−10
0
10
0 −20
20
−10
0
[m] 35
80
30
bank
building wall
70
25
[m] 15
people
[m]
[m]
20
40 30
10 20
5
10 0
trees −50
0
50
0 −20
100
[m]
35
35
30
30
25
25
20
20
−10
15
15
10
10 5
5
0
10
20
[m]
0 −20
−10
0
10 [m]
Figure 2: Raw data of the LMS 200 transformed to Cartesian coordinates and the environmental setup. 35
30
30
25
25
20
20 [m]
[m]
35
15
15
10
10
5 0 −10
5
−5
0 [m]
5
0 −10
20
building wall
60 50
10 [m]
ISBJPDAF using 6000 particles at iteration 150
ISBJPDAF using 6000 particles at iteration 140
[m]
90
−5
0
5
[m]
Figure 3: Tracking four people. The estimated trajectories of the CSBJPDAF and the ISBJPDAF are shown on the left and right side respectively.
5.1 Comparison Both presented filters solve the tracking problem reliably. To compare the computational cost, the experiment was repeated with a different number of particles. The performance of both approaches is discussed in the following. Computational Cost As illustrated in Table 2, the computational cost of the
20
0 −20
−10
0
10
20
[m]
Figure 5: The ISBJPDAF is applied to track four people. (N = 2000, γ = 20, PD = 0.7, PF A = 0.4)
ISBJPDAF is higher compared to the CSBJPDAF for the same number of particles. Obviously, the required number of particles is strongly correlated to the number of states to be tracked. Let s denote the number of states for a single target and τ the number of targets. The number of states presented by each particle is τ s for the CSBJPDAF, whereas it is just s for the ISBJPDAF. Therefore, the required number of particles is much less for the ISBJPDAF than for the CSBJPDAF and the relative computational cost for a well tuned setup is lower for the ISBJPDAF than of the CSBJPDAF as indicated by ( ) in Table 2. Tracking Behaviour Two different conditions are to be considered: • The targets’ states are not correlated or mutually independent, e.g. if the targets are spread in the field of view and are far apart from each other.
# particles 2000 4000 6000 8000
ISBJPDAF 22.5 % () 45.7 % 75.5 % 100 %
CSBJPDAF 14.6 % 25.2 % 38.4 % () 52.3 %
6 Conclusions
Table 2: Computational costs per iteration (Matlab implementation on a 500MHz PC with 256MB RAM).
• The targets’ states are strongly correlated, e.g. close to each other. In case the targets are not strongly correlated, the tracking behaviour of the ISBJPDAF and the CSBJPDAF are comparable. The disadvantage of the CSBJPDAF is that the computational cost is higher and therefore the ISBJPDAF is to be preferred if the targets are not correlated. Whenever the targets are strongly correlated, e.g. close to each other, the CSBJPDAF is more stable. Although JPDA is used in the ISBJPDAF as well as in the CSBJPDAF, the evaluation of the particle weights in the ISBJPDAF is independent for each target. Hence, the ISBJPDAF could lose one target in a bad configuration if two tracks are associated with the same measurement as illustrated in Figure 6. In the CSBJPDAF, each particle represents the concatenation of all targets, thus decreasing the likelihood of losing track of a target. ISBJPDAF using 2000 particles at iteration 18 35
30
30
25
25
20
20
[m]
[m]
ISBJPDAF using 2000 particles at iteration 17 35
15
15
10
10 5
5 0 −20
−10
0
10
0 −20
20
−10
0
30
25
25
20
20
[m]
[m]
35
30
15
15
10
10
5
5
−10
0
10 [m]
20
ISBJPDAF using 2000 particles at iteration 22
35
0 −20
10 [m]
[m] ISBJPDAF using 2000 particles at iteration 19
20
0 −20
−10
0
10
20
[m]
Figure 6: Example sequence where a target is lost by using the ISBJPDAF. (N = 2000, γ = 30, PD = 0.95, PF A = 0.05)
5.2 Hybrid Filter The previous discussion leads naturally to the idea of combining the advantages of both filters: the computational cost of the ISBJPDAF and the tracking stability of the CSBJPDAF in critical situations. The goal is to track the targets independently whenever possible. As soon as some targets’ states become correlated, those targets are to be tracked with the coupled filter approach whereas all the other targets are still to be tracked with the independent filter approach.
In this paper a solution for tracking multiple moving objects is presented. Two different approaches are examined: the couple sample based JPDAF and the independent sample based JPDAF. Both filters use SMC methods. By applying SMCM arbitrary densities over the state space of the individual targets can be represented and are not restricted to linear Gaussian models as in other approaches. JPDA is used to assign the measurements to the individual targets. The technique has been implemented and evaluated on real laser data by off-line processing. The experiments demonstrate that both approaches are able to track multiple people. A new approach is proposed that combines the advantages of both filters: the stability of the couple based JPDAF in critical situations and the low computational costs of the independent sample based JPDAF. References [1] D. Schultz, W. Burgard, D. Fox, and A.B. Cremers, “Tracking multiple moving targets with a mobile robot using particle filters and statistical data association,” in IEEE International Conference on Robotics and Automation, 2001. [2] R. Karlsson and F. Gustafsson, “Monte carlo data association for multiple target tracking,” 2001, Rickard Karlsson and Fredrik Gustafsson. Monte Carlo data association for multiple target tracking. In IEE Target tracking: Algorithms and applications, The Netherlands, Oct 2001. [3] D. Fox, S. Thrun, W. Burgard, and F. Dellaert, “Particle filters for mobile robot localization,” in Sequential Monte Carlo Methods in Practice, Arnaud Doucet, Nando de Freitas, and Neil Gordon, Eds., New York, 2001, Springer. [4] M. Isard and A. Blake, “Condensation - conditional density propagation for visual tracking,” International Journal on Computer Vision, vol. 29, no. 1, pp. 5–28, 1998. [5] M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit, “Fastslam: A factored solution to the simultaneous localization and mapping problem.,” in American Association for Artificial Intelligence, 2002. [6] J. Nieto, J. Guivant, E. Nebot, and S. Thrun, “Real time data association for fastslam,” Accepted for ICRA 2003, 2002. [7] A. Doucet, N. de Freitas, N. Gordon, and editors, Sequential Monte Carlo Methods in Practice, Springer-Verlag, New York, 2001. [8] J.M. Hammersley and D.C. Handscomb, Monte Carlo Methods, John Wiley & Sons, New York, 1964. [9] A. Doucet, “On sequential simulation-based methods for bayesian filtering,” Tech. Rep., Signal Processing Group, Dept. of Engineering, University of Cambridge, UK, 1998. [10] A. Kong, J.S. Liu, and W.H. Wong, “Sequential imputations and bayesian missing data problems,” Journal of the American Statistical Association, vol. 89, pp. 278–288, 1994. [11] S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tutorial on particle filters for on-line non-linear/nongaussian bayesian tracking,” IEEE Transactions on Signal Processing, vol. 50, no. 2, pp. 174–188, Feb. 2002. [12] S. Blackman and R. Popoli, Design and Analysis of Modern Tracking Systems, Artech House Radar Library, 1999.
