Feedback Particle Filter-based Multiple Target Tracking using Bearing ...
Feedback Particle Filter-based Multiple Target Tracking using Bearing-only Measurements Adam Tilton, Tao Yang, Huibing Yin and Prashant G. Mehta
Abstract—This paper describes the joint probabilistic data association-feedback particle filter (JPDA-FPF) introduced in our earlier paper [1]. The JPDA-FPF is based on the feedback particle filter concept (see [2],[3]). A remarkable feature of the JPDAFPF algorithm is its innovation error-based feedback structure, even with data association uncertainty in the general nonlinear case. The classical Kalman filter-based joint probabilistic data association filter (JPDAF) is shown to be a special case of the JPDA-FPF. A multiple target tracking application is presented: In the application, bearing only measurements with multiple sensors are used to track targets in the presence of data association uncertainty. It is shown that the algorithm is successfully able to track targets with significant uncertainty in initial estimate, and even in the presence of certain “track coalescence” scenarios.
I. I NTRODUCTION Filtering with data association uncertainty is important to a number of applications, including, air and missile defense systems, air traffic surveillance, weather surveillance, ground mapping, geophysical surveys, remote sensing, autonomous navigation and robotics [4], [5], [6]. In each of these applications, there exists data association uncertainty in the sense that one can not assign individual measurements to individual targets in an apriori manner. Given the large number of applications, algorithms for filtering problems with data association uncertainty have been extensively studied in the past; cf., [4], [7] and references therein. One important and widely used algorithm is the joint probabilistic data association filter (JPDAF) [4]. In this algorithm, a Kalman filter is used to solve the filtering problem. For the data association problem, the central object of interest is the computation (or approximation) of the measurement-totarget association probability. The association probability is approximated using the Bayes’ formula, and integrated with the Kalman filter to obtain the JPDAF algorithm. The development of particle filters has naturally led to investigation of data association algorithms based on importance sampling techniques. This remains an active area of research; cf., [8], [9], [10], [11] and references therein. One early contribution is the multitarget particle filter (MPFT) in [12]. In this paper and related studies (e.g., [13], [14]), a particle filter is used to solve the filtering problem. The association probabilities are obtained via the use of Markov Chain MonteCarlo (MCMC) techniques. Financial support from NSF grants EECS-0925534 and the AFOSR grant FA9550-09-1-0190 is gratefully acknowledged. A. Tilton, T. Yang, H. Yin, and P. G. Mehta are with the Coordinated Science Laboratory and the Department of Mechanical Science and Engineering at the University of Illinois at Urbana-Champaign (UIUC) {atilton2;
taoyang1;yin3; mehtapg}@illinois.edu
In a recent work [1], we introduced a novel particle filter algorithm for solution of the joint filtering-data association problem. The proposed algorithm is referred to as joint probabilistic data association-feedback particle filter (JPDA-FPF). The JPDA-FPF algorithm is based on the feedback particle filter concept (see [2],[3]). A feedback particle filter is a controlled system to approximate the solution of the nonlinear filtering problem. The filter has a feedback structure similar to the Kalman filter: At each time t, the control is obtained by using a proportional gain feedback with respect to a certain modified form of the innovation error. The filter design amounts to design of the proportional gain – the solution is given by the Kalman gain in the linear Gaussian case. Figure 1 depicts a comparison of the Kalman filter and the feedback particle filer. It was shown in [1] that the joint probabilistic data association-feedback particle filter (JPDA-FPF) represents a generalization of the Kalman filter-based joint probabilistic data association (JPDAF). One remarkable conclusion is that the JPDA-FPF retains the innovation error-based feedback structure even for the nonlinear problem. The innovation errorbased feedback structure is expected to be useful because of the coupled nature of the filtering and the data association problems. The aim of this paper is to describe applications of the JPDA-FPF algorithm. Beyond [1], the contributions of this paper are as follows: • Notation. The algorithm is described for an ODE model for the signal and the observation process, as opposed to the SDE formalism employed in [1]. The formalism is better suited for implementation and application of the JPDA-FPF algorithm. • JPDA-FPF algorithms. The basic PDA-FPF algorithm presented in [1] is now generalized to the multi-target problem with data association uncertainty. Notation for associations in the general case is introduced, the resulting feedback particle filter algorithm described and compared with the linear case (see Table I). • Multiple Target Tracking Application. A multiple target tracking application is presented: In this application, bearing only measurements with multiple sensors are used to track targets in the presence of data association uncertainty. It is shown that the algorithm is successfully able to track targets with significant uncertainty in initial estimate, and even in the presence of certain “track coalescence” problem scenarios. The outline of the remainder of this paper is as follows. The JPDA-FPF algorithm is first described for single target in the presence of clutter, in Sec. II. The multiple target case is
2058
discussed in Sec. III. The application results are described in Sec. IV. II. F EEDBACK PARTICLE F ILTER WITH DATA A SSOCIATION U NCERTAINTY In this section, we describe the basic probabilistic data association-feedback particle filter (PDA-FPF) for tracking a single target in the presence of multiple measurements. The filter for multiple targets is obtained as a straightforward extension, and described in Sec. III. A. Problem statement, Assumptions and Notation The following notation is adopted: (i) At time t, the target state is denoted by Xt ∈ Rd . (ii) At time t, the observation vector Y t := ¯ is assumed fixed and (Yt1 ,Yt2 , . . . ,Ytm¯ )T , where m Ytm ∈ Rs . (iii) At time t, the association random variable is denoted ¯ It is used to associate one meaas At ∈ {0, 1, . . . , m}. surement to the target: At = m signifies that the mth measurement Ytm is ‘associated’ with the target, and At = 0 means that the target is not detected at time t. The following models are assumed for the state and observation processes: (i) The state Xt evolves according to a nonlinear ODE: X˙t = a(Xt ) + σB B˙t ,
(1)
where a(·) are C1 function, and {B˙t } is assumed to be a standard d−dim white noise with σB ∈ R. (ii) In [1], the association random process At is modeled as a jump Markov process in continuous-time. For the discussion here, we simply assume a uniform prior on At (This would correspond to the mixing rate of the Markov process being very large). (iii) At and Xt are assumed to be mutually independent. (iv) At time t, the observation model is given by, Ytm = 1[At =m] h(Xt ) + W˙ tm ,
(2)
{W˙ tm }
¯ where for m ∈ {1, . . . , m}, are mutually independent white noise processes. The covariance matrix of the observation noise is denoted as R, assumed to be a positive definite symmetric matrix. We define: ( 1 when At = m 1[At =m] := 0 otherwise.
X∈A
p(x,t) dx = P{Xt ∈ A|Z t }
The methodology comprises of the following two parts: (i) Evaluation of association probability, and (ii) Integration of association probability in the feedback particle filter framework.
The association probability is defined as the probability of the association [At = m] conditioned on Z t : βtm , Pr([At = m]|Z t ),
¯ m = 0, 1, ..., m.
(3)
Since the events are mutually exclusive and exhaustive, ¯ m ∑m m=0 βt = 1. In the following, we integrate association probability with the feedback particle filter, which is used to approximate evolution of the posterior. Next, an algorithm to approximate the association probability is discussed. Separate algorithms for data association and posterior are motivated in part by the classical JPDA filtering literature [15], [4], [7]. A separate treatment is also useful while considering multiple target tracking problems. For such problems, one can extend algorithms for data association in a straightforward manner, while the algorithm for posterior remains as before. Additional details appear in Sec III. C. Feedback Particle Filter Following the feedback particle filter methodology, the model for the particle filter is given by, X˙ti = a(Xti ) + σB B˙ti +Uti ,
(4)
where Xti ∈ Rd is the state for the ith particle at time t, Uti is its control input, and {B˙ti } are mutually independent standard white noise processes, i ∈ {1, 2, . . . , N}. We assume the initial conditions {X0i }Ni=1 are i.i.d., independent of {B˙ti }, and drawn from the initial distribution p∗ (x, 0) of X0 . Both {B˙ti } and {X0i } are also assumed to be independent of Xt ,Yt . There are two types of conditional distributions of interest in our analysis: 1) p(x,t): Defines the conditional dist. of Xti given Zt . 2) p∗ (x,t): Defines the conditional dist. of Xt given Zt . The control input Uti are said to be optimal if p ≡ p∗ . That is, given p∗ (·, 0) = p(·, 0), our goal is to choose Uti in the feedback particle filter so that the evolution equations of these conditional distributions coincide. It is shown in [1] that the optimally controlled dynamics of the ith particle have the following form, ¯ m
X˙ti = a(Xti ) + σB B˙ti +
∑ βtm K(Xti ,t)Iti,m
(5)
m=1
where Iti,m is a modified form of the innovation process,
The problem is to obtain the posterior distribution of Xt given the history of observations (filtration) Y t := σ (Y τ : τ ≤ t). The posterior is denoted by p, so that for any measurable set A ⊂ Rd , Z
B. Association Probability for a Single Target
Iti,m := Ytm − [
βtm βm h(Xti ) + (1 − t )hˆ t ], 2 2
(6)
where hˆ t := E[h(Xt )|Z t ] = h(x)p(x,t) dx. In a numerical implementation, we approximate hˆ ≈ N1 ∑Ni=1 h(Xti ) =: hˆ (N) . The gain function K is the solution of a certain EulerLagrange boundary value problem (E-L BVP): R
ˆ T R−1 p. ∇ · (pK) = −(h − h)
(7)
The filter (5)-(7) is referred to as the feedback particle filter. It is shown in [1] that the feedback particle filter is
2059
Fig. 1.
Innovation error-based feedback structure for (a) Kalman filter and (b) nonlinear feedback particle filter.
consistent with the nonlinear filter, given consistent initializations p( · , 0) = p∗ ( · , 0). Consequently, if the initial conditions {X0i }Ni=1 are drawn from the initial distribution p∗ ( · , 0) of X0 , then, as N → ∞, the empirical distribution of the particle system approximates the posterior distribution p∗ ( · ,t) for each t. The filter requires solution of the E-L BVP (7) for each value of time t. For the numerical results presented in this paper, we use the following approximation of this solution 1 N (N) T −1 R . (8) K ≈ ∑ Xti h(Xti ) − hˆ t N i=1 The justification for the approximation appears in [16]. D. Filter for Association Probability In [1], a Wonham filter is described for evolution of the association probability. In the limit of large transition rate, the following heuristic is valid: βtm =
L(Ytm ) j ¯ ∑m j=1 L(Yt )
,
¯ m = 1, 2, ..., m,
1 d 2
(2π) |R|
Z 1 2
Rd
exp −kYtm − h(x)k2R p(x,t) dx, (10)
where the weighted norm with respect to R is defined as: 1 2 1 T −1 kV kR := V R V (11) 2 The formula (10) is the nonlinear counterpart of the formula used to obtain association probability in the classical PDAF (see Section 6.4 in [15]). As in the PDAF, the formula follows from application of the Bayes’ rule. The derivation appears in Appendix D in [1]. For the numerical results presented in this paper, we use the following approximation of (10): L(Ytm ) ≈
K(x,t) = Σt H T R−1
1
(2π) 2 |R| 2
X˙ti = A Xti + σB B˙ti + Σt H T R−1
¯ m
βtm i βtm m m β Y − H( X + (1 − )µ ) . (14) t ∑ t t 2 t 2 m=1
The following theorem states that p = p∗ in this case. That is, the conditional distributions of Xt and Xti coincide. The proof is omitted. Theorem 2: Consider the single target tracking problem with a linear model defined by the state-observation equations (12a,12b). The PDA-FPF is given by (14). In this case the posterior distributions of Xt and Xti coincide, whose conditional mean and covariance are given by the following,
∑ βtm (Ytm − Hµt )
(15)
m=1
Σ˙ t = AΣt + Σt AT + I − Σt H T R−1 HΣt
¯ m
∑ (βtm )2 .
(16)
m=1
In practice {µt , Σt } in (14)- (16) are approximated as sample means and sample covariances from the ensemble {Xti }Ni=1 .
∑ exp[−||Ytm − h(Xti )||2R ].
E. Comparison of PDA-FPF and PDAF
1 N i ∑ Xt , N i=1
µt ≈ µt
:=
(N) Σt ≈ Σt
1 N i (N) := ∑ (Xt − µt )2 . N − 1 i=1
i=1
X˙t = A Xt + B˙t , Yt = H Xt + W˙ t ,
¯ m
µ˙ t = Aµt + Σt H T R−1
(N)
In this section, we compare the PDA-FPF to PDAF with the aid of a linear example. Consider the following linear model:
(13)
The formula (13) is verified by direct substitution in (7) where the distribution p is Gaussian. Using the formula for the gain function, the linear PDA-FPF is given by,
N
1 d
(2π) 2 |Σt | 2
be Gaussian with mean µt and variance Σt . Then the solution of the E-L BVP (7) is given by:
(9)
where L(Ytm ) =
where A is a d × d matrix and H is an m × d matrix. We assume the initial distribution p∗ (x, 0) is Gaussian with mean µ0 and covariance matrix Σ0 . The following lemma provides the solution of the gain function K(x,t) in the linear case. Lemma 1: Considerthe linear observation (12b). Suppose 1 1 T T p(x,t) = d 1 exp − 2 (x − µt ) Σt (x − µt ) is assumed to
(17)
The data association probability βtm is also evaluated by using particles:
(12a) (12b)
2060
βtm ∝
1 1 N (2π) d2 |R| 12
N
∑ exp
i=1
−kYtm − HXti k2R .
TABLE I C OMPARISON OF THE NONLINEAR PDA-FPF
State model βtm
PDAF ALGORITHM
PDAF
PDA-FPF
X˙t = AXt + σB B˙t
X˙t = a(Xt ) + σB B˙t
Yt = HXt + W˙ t
Observation model Assoc. Prob.
ALGORITHM WITH THE LINEAR
Yt = h(Xt ) + W˙ t t
:= P([At = m]|Z ) m
m
Yˆt = Hµt
β β Yˆti,m = [ 2t h(Xti ) + (1 − 2t )hˆ t ]
Innovation error
Itm = Ytm − Yˆt
Iti,m = Ytm − Yˆti,m
Feedback control
Utm = Kg Itm
Uti,m = K(Xti ,t)Iti,m
Kalman Gain
Sol. of a BVP (7)
¯ m m ∑m m=1 βt Ut m ∝ exp −kYt − Yˆt k2R
¯ m ∑m m=1 βt Ut 1 N ∝ N ∑i=1 exp −kYtm − h(Xti )k2R X˙ti = a(Xti ) + σB B˙ti + K(Xti ,t) ∑m βtmUti,m
Prediction
Gain Control input Formula for βtM Filter equation
i,m
µ˙ t = Aµt + Kg ∑m βtmUtm
F. Comparison of PDA-FPF and PDAF Table I provides a comparison of the PDA-FPF and the PDAF algorithms. The main point to note is that the feedback particle filter has an innovation error-based structure: In effect, the ith particle makes a prediction Yˆti,m as a weighted-average of h(Xti ) and hˆ t . This is then used to compute an innovation error Iti,m . The Bayes’ update step involves gain feedback of the innovation error.
at1 , Yt2 originates from target at2 , . . . , Ytn¯ originates from target atn¯ . The following models are assumed for the three stochastic processes. (i) The dynamics of the nth target evolves according to the nonlinear ODE: X˙tn = an (Xtn ) + σBn B˙tn , {B˙tn
where ∈ are mutually independent white noise processes. (ii) The observation model is given by: 1 1 Yt h(Xt1 ) W˙ t .. .. .. (19) . = Ψ(At ) . + . , ¯ ¯ ¯ n n n ˙ Yt h(Xt ) Wt
III. M ULTIPLE TARGET T RACKING U SING F EEDBACK PARTICLE F ILTER In this section, we extend the PDA-FPF to the multiple target tracking problem. The resulting filter is referred to as the joint probabilistic data association feedback particle filter (JPDA-FPF). For notational ease, we assume that all measurements originate from targets (That is, there is no clutter).
where {W˙ tn } are mutually independent white noise processes with covariance matrix R whose dimension is s×s. Furthermore, {W˙ tn } are mutually independent with {B˙tn } and Ψ(At ) is the permutation matrix for association At . Example 1: Suppose n¯ = 2. The permutation matrices are Is 0 0 Is Ψ({1, 2}) = , Ψ({2, 1}) = , (20) 0 Is Is 0
A. Problem Statement, Assumptions and Notation The following notation is adopted: (i) There are n¯ distinct targets. The set of targets is denoted by an index set N = {1, 2, . . . , n¯ }. The set of permutations of N is denoted by P(N ). The cardinality of the set P(N ), |P(N )| = n¯ !. A typical elements of P(N ) is denoted as a = (a1 , a2 , . . . , an¯ ), where an ∈ N for all n∈N . (ii) At time t, the state of the nth target is denoted as Xtn ∈ Rd for n ∈ N . (iii) At time t, there is exactly one measurement per target for a total of n¯ measurements. There are no missing measurements and no measurements because of clutter. The observation vector Y t := (Yt1 ,Yt2 , . . . ,Ytn¯ )T , where the nth entry, Ytn ∈ Rs , originates from one of the targets in N. (iv) At time t, the association random variable is denoted as At ∈ P(N ). It is used to associate targets with the measurements: At = at = (at1 , at2 , . . . , atn¯ ) ∈ P(N ) signifies that the measurement Yt1 originates from target
(18)
¯ Rd }nn=1
where Is is an identity matrix with dimension s. (iii) The model for At is the same as the model assumed in discussions of PDA-FPF (see Sec. II-A) (iv) At and X t are assumed to be mutually independent. The problem is to design n¯ feedback particle filters, where the nth filter is intended to estimate the posterior distribution of the nth target given the history of all unassociated observations (filtration) Y t := σ (Y s : s ≤ t). The posterior distribution is denoted as pn (x,t). As before, the algorithm comprises of the following two parts: (i) Evaluation of the measurement-to-target association probability, and (ii) Integration of association probability with the feedback particle filter.
2061
B. JPDA-FPF The association probability is defined as the conditional probability of association [atr = n]: βtn,r := Pr [atr = n] Z t ,Y t . (21) In words, βtn,r is the conditional probability that the rth measurement, Ytr , originates from the nth target. Using these probabilities, and denoting the state of the ith particle for the nth target at time t as Xtn,i , i ∈ {1, 2, . . . , N}, the JPDA-FPF algorithm for the nth target is as follows: X˙tn,i
n
=a
(Xtn,i ) + σBi B˙tn +
n¯
∑
βtn,r Kn (Xtn,i ,t)Itn,i,r ,
where Itn,i,r is a modified form of the innovation process, n,r βtn,r ˆ n βt n,i n,i,r r h(Xt ) + (1 − )ht (23) It := Yt − 2 2 with hˆ tn := E[h(Xtn )|Z t ], and Kn is control gain obtained from solving a certain EL-BVP: (24)
where pn (x,t) denotes the posterior distribution. In practice it is approximated by a constant-valued formula: T N ˜ n = 1 ∑ Xtn,i h(Xtn,i ) − hˆ tn(N) R−1 , K N i=1
(25)
n(N)
where hˆ t = N1 ∑Ni=1 h(Xtn,i ). Finally, the association probabilities are calculated easily using Bayes’ rule as in Sec. II-D. In particular, βtn,r =
∑
{b∈P(N
b
γt ,
(26)
d 2
(2π) |R| ≈
n¯ 1 2
k=1 n¯
1 d
∏
1
(2π) 2 |R| 2
∏
k=1
Rd
h i exp −kYtk − h(x)k2R pbk (x,t) dx
n¯
m=1 r=1
where formulae for innovation error and the gain function are similar to (23)- (25). The association probabilities are obtained, separately for each sensor, using formulae similar to (26). IV. N UMERICS A. Single target tracking problem Consider a target tracking problem with two bearing-only sensors [17]. A single target moves in a two-dimensional (2d) plane according to the standard white-noise acceleration model: X˙t = AXt + ΓB˙t , (28) where X := (X1 ,V1 , X2 ,V2 )T ∈ R4 , (X1 , X2 ) denotes the position and (V1 ,V2 ) denotes the velocity. The matrices, 0 0 0 1 0 0 1 0 0 0 0 0 Γ = σB A= 0 0 , 0 0 0 1 , 0 1 0 0 0 0
(29)
where W˙ t is a standard 2d white noise, h = (h1 , h2 )T and ! (sen j) x2 − x2 , j = 1, 2, h j (x1 , v1 , x2 , v2 ) = arctan (sen j) x1 − x1
k=1
1
¯ m
∑ ∑ βtn,r;m Kn;m (Xtn,i ,t)Itn,i,r;m ,
Yt = h(Xt ) + σW W˙ t ,
n¯ b γt ∝ ∏ Pr Ytk [atk = bk ] , =
X˙tn,i = an (Xtn,i ) + σBi B˙tn,i +
and B˙t is a standard 2d white noise. The observation model is given by,
):br =n}
where
Z
¯ and n, r ∈ N . It is the conditional for m ∈ {1, 2, . . . , m} probability that the rth measurement from the mth sensor originates from the nth target. Using the association probabilities, the filter for the nth target is
(22)
r=1
∇ · (pn Kn ) = −(h − hˆ tn )T R−1 pn ,
In particular, we denote the measurements for the mth sensor ¯ )T . The association probabilities are as Y tm = (Yt1;m , . . . ,Ytn;m now denoted as βtn,r;m = Pr [atr;m = n] Z t ,Y tm (27)
! h i 1 N b ,i ∑ exp −kYtk − h(Xt k )k2R . N i=1
(sen j)
As the number of targets increases, there is an exponential growth in the number of associations. In practice, one may also wish to consider approaches to reduce filter complexity, e.g., by assigning gating regions for the measurements; cf., Sec. 4.2.3 in [15]. This is the subject of future investigation. C. Multi-sensor Case ¯ sensors that provide indepenNow suppose there are m dent measurements of n¯ targets. The filter for this case is a straightforward generalization of the single-sensor filter described in the preceding section: The notation specific to ¯ is tagged with sensor index m. sensor m ∈ {1, 2, . . . , m}
(sen j)
) denote the position of sensor j. where (x1 , x2 Figure 2 depicts a sample path obtained for a typical numerical experiment. The sensor and target locations are depicted together with an estimate (conditional mean) that is approximated using a feedback particle filter. Since there is only single target, there is no data association uncertainty. The background depicts the ensemble of observations that were made over the simulation run. Each point in the ensemble is obtained by using the process of triangulation based on two (noisy) angle measurements. The simulation parameters are: The initial position of the target is depicted, the initial velocity was chosen as (0.2, −5) and σB = 0.1; The two sensor positions are depicted and σW = 0.017; The particle filter comprised of N = 200 particles
2062
40 Meas.
30
Tgt. Ptcle. Mean Sensor
20
Tgt. Init. Ptcle. Init
10
0
-10 -30
-20
-10
0
10
20
30
Fig. 2. Simulation results: Comparison of the true target trajectory with the estimate obtained using FPF. Fig. 4. Illustration of “ghost” target in the two-sensor two-target case: The ghost appears because of incorrect data association.
association probabilities for target 1, and βt2,1;1 , βt2,2;1 are the probabilities for target 2 (see (27)). The simulation parameters are: The two targets start at position (−20, 50) and (20, 50), respectively. The initial velocity vectors are V01 = V02 = (0.0, −5.0). σB = 0.1, σW = 0.017 and the number of particles N = 200.
Fig. 3.
Simulation results for two target example.
whose initial position is chosen from a Gaussian distribution whose mean is depicted. The gain function is obtained using the constant gain approximation in (25). The simulation results show that the filter can adequately track the target. B. Two targets Consider a tracking problem with two targets as depicted in Fig. 3. The targets move in the 2d plane according to the standard white-noise model (28). There are two bearing-only sensors also depicted in the figure. At time t, each sensor obtains two angle only measurements according to model (29). There is data association uncertainty in this case, in the sense that one cannot assign measurements to individual targets in an apriori manner. In this particular example, errors in data association can lead to appearance of a “ghost” target (see Fig. 4). In the simulation study, tracks are initialized at the location of the “ghost” target (black circles in Fig. 3). Particles are initialized by drawing from a Gaussian distribution whose mean is set to be the “ghost” target location. As depicted in Fig. 4, this position is identified by using the process of triangulation based on initial positions of the two targets. Figure 3 depicts the estimate (mean of particles) obtained using the JPDAFPF algorithm. At each time t, four association probabilities are approximated for each sensor: βt1,1;1 , βt1,2;1 are the two
C. Track coalescence example Track coalescence is a common problem in multiple target tracking applications. Track coalescence can occur when two closely spaced targets move with approximately the same velocity over a time period [6], [18]. With standard implementations of JPDAF and SIR particle filter algorithms, the target tracks tends to coalesce even after the targets have moved apart [18]. In the following example, we describe simulation results for JPDA-FPF for a model problem scenario related to [19]. As in the preceding example, there are two targets in the 2d plane, modeled by (28). Initially, the two targets are at position (−40, 600) and (40, 600), respectively. They move towards each other with initial velocity vector (4.5, −5.0) and (−4.5, −5.0), respectively. When they are close to each other, they move vertically together for some time (velocity along x is set to 0). Finally, they move away from each other with velocity vectors (−4.5, −5.0) and (4.5, −5.0), respectively. Two sensors obtain two bearing-only measurements, but with data association uncertainty. Figure 5(a) depicts the results of a single simulation: The tracks are initialized at the actual target location. The estimates are obtained using the JPDA-FPF with N = 200 particles. Figure 5(b) depicts the association probability during the simulation run. As shown in the figure, the tracks coalesce when the two targets are close. However, the filter tracks the targets once they move away. That is, the track coalescence problem is successfully avoided. D. Three targets In this numerical experiment, the objective is to track three targets with two bearing-only sensors. The initial target
2063
Fig. 5.
Fig. 6.
(a) Simulation results for the track coalescence example; (b) Plot of data association probability.
Simulation results for the three targets example.
positions are (−40, 300), (0, 300) and (40, 300), respectively. The initial velocity vectors are V 1 = V 2 = V 3 = (0, −5). Figure 6 depicts the results of a simulation study. With only two sensors, the filter performance degrades as the number of targets increases beyond 3. ACKNOWLEDGEMENT We are grateful to Dr. Samuel Blackman for suggesting the application problem described in this paper. R EFERENCES [1] T. Yang, G. Huang, and P. G. Mehta, “Joint probabilistic data association-feedback particle filter for multiple target tracking applications,” In Proc. of American Control Conference, Jun 2012. [2] T. Yang, P. G. Mehta, and S. P. Meyn, “A mean-field control-oriented approach to particle filtering,” In Proc. of American Control Conference, pp. 2037–2043, June 2011. [3] ——, “Feedback particle filter with mean-field coupling,” In Proc. of IEEE Conference on Decision and Control, pp. 7909–7916, December 2011.
[4] Y. Bar-Shalom, F. Daum, and J. Huang, “The probabilistic data association filter,” IEEE Control Systems Magazine, vol. 29, no. 6, pp. 82–100, Dec 2009. [5] S. Thrun, “Probabilistic robotics,” Commun. ACM, vol. 45, pp. 52–57, March 2002. [Online]. Available: http://doi.acm.org/10.1145/504729.504754 [6] S. S. Blackman, Multiple-Target Tracking with Radar Applications. Boston, MA: Artech House, 1986. [7] T. Kirubarajan and Y. Bar-Shalom, “Probabilistic data association techniques for target tracking in clutter,” Proceedings of The IEEE, vol. 92, no. 3, pp. 536–557, 2004. [8] B. Kragel, S. Herman, and N. Roseveare, “A comparison of methods for estimating track-to-track assignment probabilities,” IEEE Trans. Aerospace and Electronic Systems, 2011, in Press. [9] F. Daum and J. Huang, “Generalized particle flow for nonlinear filters,” In Proc. of SPIE, p. 7698, 2010. [10] G. Terejanu, P. Singla, T. Singh, and P. D. Scott, “Uncertainty propagation for nonlinear dynamical systems using gaussian mixture models,” AIAA Journal of Guidance, Control and Dynamics, vol. 31, no. 6, pp. 1623–1633, Nov. 2008. [11] A. Budhiraja, L. Chen, and C. Lee, “A survey of numerical methods for nonlinear filtering problems,” Physica D: Nonlinear Phenomena, vol. 230, no. 1-2, pp. 27–36, 2007. [12] C. Hue, J.-P. L. Cadre, and P. Prez, “Tracking multiple objects with particle filtering,” IEEE Trans. Aerospace and Electronic Systems, vol. 38, no. 3, pp. 791–812, July 2002. [13] I. Kyriakides, D. Morrell, and A. Papandreou-Suppappola, “Sequential Monte Carlo methods for tracking multiple targets with deterministic and stochastic constraints,” IEEE Trans. Signal Process., vol. 56, no. 3, pp. 937–948, 2008. [Online]. Available: http://dx.doi.org/10.1109/TSP.2007.908931 [14] S. Oh, S. Russell, and S. Sastry, “Markov chain Monte Carlo data association for multi-target tracking,” IEEE Trans. Automat. Control, vol. 54, no. 3, pp. 481–497, 2009. [Online]. Available: http://dx.doi.org/10.1109/TAC.2009.2012975 [15] Y. Bar-Shalom and T. E. Fortmann, Tracking and Data Association. San Diego, CA: Academic Press, 1988. [16] T. Yang, R. S. Laugesen, P. G. Mehta, and S. Meyn, “Multivariable feedback particle filter,” submitted to IEEE Conference on Decision and Control, Dec 2012. [17] Y. Bar-Shalom, T. Kirubarajan, and X.-R. Li, Estimation with Applications to Tracking and Navigation. New York, NY, USA: John Wiley & Sons, Inc., 2002. [18] H. A. P. Blom and E. A. Bloem, “Probabilistic data association avoiding track coalescence,” vol. 45, no. 2, pp. 247–259, 2000. [19] ——, “Joint particle filtering of multiple maneuvering targets from unassociated measurements,” Journal of Advancement Information Fusion, vol. 1, pp. 15–36, 2006.
2064
Abstract—This paper describes the joint probabilistic data association-feedback particle filter (JPDA-FPF) introduced in our earlier paper [1]. The JPDA-FPF is based on the feedback particle filter concept (see [2],[3]). A remarkable feature of the JPDAFPF algorithm is its innovation error-based feedback structure, even with data association uncertainty in the general nonlinear case. The classical Kalman filter-based joint probabilistic data association filter (JPDAF) is shown to be a special case of the JPDA-FPF. A multiple target tracking application is presented: In the application, bearing only measurements with multiple sensors are used to track targets in the presence of data association uncertainty. It is shown that the algorithm is successfully able to track targets with significant uncertainty in initial estimate, and even in the presence of certain “track coalescence” scenarios.
I. I NTRODUCTION Filtering with data association uncertainty is important to a number of applications, including, air and missile defense systems, air traffic surveillance, weather surveillance, ground mapping, geophysical surveys, remote sensing, autonomous navigation and robotics [4], [5], [6]. In each of these applications, there exists data association uncertainty in the sense that one can not assign individual measurements to individual targets in an apriori manner. Given the large number of applications, algorithms for filtering problems with data association uncertainty have been extensively studied in the past; cf., [4], [7] and references therein. One important and widely used algorithm is the joint probabilistic data association filter (JPDAF) [4]. In this algorithm, a Kalman filter is used to solve the filtering problem. For the data association problem, the central object of interest is the computation (or approximation) of the measurement-totarget association probability. The association probability is approximated using the Bayes’ formula, and integrated with the Kalman filter to obtain the JPDAF algorithm. The development of particle filters has naturally led to investigation of data association algorithms based on importance sampling techniques. This remains an active area of research; cf., [8], [9], [10], [11] and references therein. One early contribution is the multitarget particle filter (MPFT) in [12]. In this paper and related studies (e.g., [13], [14]), a particle filter is used to solve the filtering problem. The association probabilities are obtained via the use of Markov Chain MonteCarlo (MCMC) techniques. Financial support from NSF grants EECS-0925534 and the AFOSR grant FA9550-09-1-0190 is gratefully acknowledged. A. Tilton, T. Yang, H. Yin, and P. G. Mehta are with the Coordinated Science Laboratory and the Department of Mechanical Science and Engineering at the University of Illinois at Urbana-Champaign (UIUC) {atilton2;
taoyang1;yin3; mehtapg}@illinois.edu
In a recent work [1], we introduced a novel particle filter algorithm for solution of the joint filtering-data association problem. The proposed algorithm is referred to as joint probabilistic data association-feedback particle filter (JPDA-FPF). The JPDA-FPF algorithm is based on the feedback particle filter concept (see [2],[3]). A feedback particle filter is a controlled system to approximate the solution of the nonlinear filtering problem. The filter has a feedback structure similar to the Kalman filter: At each time t, the control is obtained by using a proportional gain feedback with respect to a certain modified form of the innovation error. The filter design amounts to design of the proportional gain – the solution is given by the Kalman gain in the linear Gaussian case. Figure 1 depicts a comparison of the Kalman filter and the feedback particle filer. It was shown in [1] that the joint probabilistic data association-feedback particle filter (JPDA-FPF) represents a generalization of the Kalman filter-based joint probabilistic data association (JPDAF). One remarkable conclusion is that the JPDA-FPF retains the innovation error-based feedback structure even for the nonlinear problem. The innovation errorbased feedback structure is expected to be useful because of the coupled nature of the filtering and the data association problems. The aim of this paper is to describe applications of the JPDA-FPF algorithm. Beyond [1], the contributions of this paper are as follows: • Notation. The algorithm is described for an ODE model for the signal and the observation process, as opposed to the SDE formalism employed in [1]. The formalism is better suited for implementation and application of the JPDA-FPF algorithm. • JPDA-FPF algorithms. The basic PDA-FPF algorithm presented in [1] is now generalized to the multi-target problem with data association uncertainty. Notation for associations in the general case is introduced, the resulting feedback particle filter algorithm described and compared with the linear case (see Table I). • Multiple Target Tracking Application. A multiple target tracking application is presented: In this application, bearing only measurements with multiple sensors are used to track targets in the presence of data association uncertainty. It is shown that the algorithm is successfully able to track targets with significant uncertainty in initial estimate, and even in the presence of certain “track coalescence” problem scenarios. The outline of the remainder of this paper is as follows. The JPDA-FPF algorithm is first described for single target in the presence of clutter, in Sec. II. The multiple target case is
2058
discussed in Sec. III. The application results are described in Sec. IV. II. F EEDBACK PARTICLE F ILTER WITH DATA A SSOCIATION U NCERTAINTY In this section, we describe the basic probabilistic data association-feedback particle filter (PDA-FPF) for tracking a single target in the presence of multiple measurements. The filter for multiple targets is obtained as a straightforward extension, and described in Sec. III. A. Problem statement, Assumptions and Notation The following notation is adopted: (i) At time t, the target state is denoted by Xt ∈ Rd . (ii) At time t, the observation vector Y t := ¯ is assumed fixed and (Yt1 ,Yt2 , . . . ,Ytm¯ )T , where m Ytm ∈ Rs . (iii) At time t, the association random variable is denoted ¯ It is used to associate one meaas At ∈ {0, 1, . . . , m}. surement to the target: At = m signifies that the mth measurement Ytm is ‘associated’ with the target, and At = 0 means that the target is not detected at time t. The following models are assumed for the state and observation processes: (i) The state Xt evolves according to a nonlinear ODE: X˙t = a(Xt ) + σB B˙t ,
(1)
where a(·) are C1 function, and {B˙t } is assumed to be a standard d−dim white noise with σB ∈ R. (ii) In [1], the association random process At is modeled as a jump Markov process in continuous-time. For the discussion here, we simply assume a uniform prior on At (This would correspond to the mixing rate of the Markov process being very large). (iii) At and Xt are assumed to be mutually independent. (iv) At time t, the observation model is given by, Ytm = 1[At =m] h(Xt ) + W˙ tm ,
(2)
{W˙ tm }
¯ where for m ∈ {1, . . . , m}, are mutually independent white noise processes. The covariance matrix of the observation noise is denoted as R, assumed to be a positive definite symmetric matrix. We define: ( 1 when At = m 1[At =m] := 0 otherwise.
X∈A
p(x,t) dx = P{Xt ∈ A|Z t }
The methodology comprises of the following two parts: (i) Evaluation of association probability, and (ii) Integration of association probability in the feedback particle filter framework.
The association probability is defined as the probability of the association [At = m] conditioned on Z t : βtm , Pr([At = m]|Z t ),
¯ m = 0, 1, ..., m.
(3)
Since the events are mutually exclusive and exhaustive, ¯ m ∑m m=0 βt = 1. In the following, we integrate association probability with the feedback particle filter, which is used to approximate evolution of the posterior. Next, an algorithm to approximate the association probability is discussed. Separate algorithms for data association and posterior are motivated in part by the classical JPDA filtering literature [15], [4], [7]. A separate treatment is also useful while considering multiple target tracking problems. For such problems, one can extend algorithms for data association in a straightforward manner, while the algorithm for posterior remains as before. Additional details appear in Sec III. C. Feedback Particle Filter Following the feedback particle filter methodology, the model for the particle filter is given by, X˙ti = a(Xti ) + σB B˙ti +Uti ,
(4)
where Xti ∈ Rd is the state for the ith particle at time t, Uti is its control input, and {B˙ti } are mutually independent standard white noise processes, i ∈ {1, 2, . . . , N}. We assume the initial conditions {X0i }Ni=1 are i.i.d., independent of {B˙ti }, and drawn from the initial distribution p∗ (x, 0) of X0 . Both {B˙ti } and {X0i } are also assumed to be independent of Xt ,Yt . There are two types of conditional distributions of interest in our analysis: 1) p(x,t): Defines the conditional dist. of Xti given Zt . 2) p∗ (x,t): Defines the conditional dist. of Xt given Zt . The control input Uti are said to be optimal if p ≡ p∗ . That is, given p∗ (·, 0) = p(·, 0), our goal is to choose Uti in the feedback particle filter so that the evolution equations of these conditional distributions coincide. It is shown in [1] that the optimally controlled dynamics of the ith particle have the following form, ¯ m
X˙ti = a(Xti ) + σB B˙ti +
∑ βtm K(Xti ,t)Iti,m
(5)
m=1
where Iti,m is a modified form of the innovation process,
The problem is to obtain the posterior distribution of Xt given the history of observations (filtration) Y t := σ (Y τ : τ ≤ t). The posterior is denoted by p, so that for any measurable set A ⊂ Rd , Z
B. Association Probability for a Single Target
Iti,m := Ytm − [
βtm βm h(Xti ) + (1 − t )hˆ t ], 2 2
(6)
where hˆ t := E[h(Xt )|Z t ] = h(x)p(x,t) dx. In a numerical implementation, we approximate hˆ ≈ N1 ∑Ni=1 h(Xti ) =: hˆ (N) . The gain function K is the solution of a certain EulerLagrange boundary value problem (E-L BVP): R
ˆ T R−1 p. ∇ · (pK) = −(h − h)
(7)
The filter (5)-(7) is referred to as the feedback particle filter. It is shown in [1] that the feedback particle filter is
2059
Fig. 1.
Innovation error-based feedback structure for (a) Kalman filter and (b) nonlinear feedback particle filter.
consistent with the nonlinear filter, given consistent initializations p( · , 0) = p∗ ( · , 0). Consequently, if the initial conditions {X0i }Ni=1 are drawn from the initial distribution p∗ ( · , 0) of X0 , then, as N → ∞, the empirical distribution of the particle system approximates the posterior distribution p∗ ( · ,t) for each t. The filter requires solution of the E-L BVP (7) for each value of time t. For the numerical results presented in this paper, we use the following approximation of this solution 1 N (N) T −1 R . (8) K ≈ ∑ Xti h(Xti ) − hˆ t N i=1 The justification for the approximation appears in [16]. D. Filter for Association Probability In [1], a Wonham filter is described for evolution of the association probability. In the limit of large transition rate, the following heuristic is valid: βtm =
L(Ytm ) j ¯ ∑m j=1 L(Yt )
,
¯ m = 1, 2, ..., m,
1 d 2
(2π) |R|
Z 1 2
Rd
exp −kYtm − h(x)k2R p(x,t) dx, (10)
where the weighted norm with respect to R is defined as: 1 2 1 T −1 kV kR := V R V (11) 2 The formula (10) is the nonlinear counterpart of the formula used to obtain association probability in the classical PDAF (see Section 6.4 in [15]). As in the PDAF, the formula follows from application of the Bayes’ rule. The derivation appears in Appendix D in [1]. For the numerical results presented in this paper, we use the following approximation of (10): L(Ytm ) ≈
K(x,t) = Σt H T R−1
1
(2π) 2 |R| 2
X˙ti = A Xti + σB B˙ti + Σt H T R−1
¯ m
βtm i βtm m m β Y − H( X + (1 − )µ ) . (14) t ∑ t t 2 t 2 m=1
The following theorem states that p = p∗ in this case. That is, the conditional distributions of Xt and Xti coincide. The proof is omitted. Theorem 2: Consider the single target tracking problem with a linear model defined by the state-observation equations (12a,12b). The PDA-FPF is given by (14). In this case the posterior distributions of Xt and Xti coincide, whose conditional mean and covariance are given by the following,
∑ βtm (Ytm − Hµt )
(15)
m=1
Σ˙ t = AΣt + Σt AT + I − Σt H T R−1 HΣt
¯ m
∑ (βtm )2 .
(16)
m=1
In practice {µt , Σt } in (14)- (16) are approximated as sample means and sample covariances from the ensemble {Xti }Ni=1 .
∑ exp[−||Ytm − h(Xti )||2R ].
E. Comparison of PDA-FPF and PDAF
1 N i ∑ Xt , N i=1
µt ≈ µt
:=
(N) Σt ≈ Σt
1 N i (N) := ∑ (Xt − µt )2 . N − 1 i=1
i=1
X˙t = A Xt + B˙t , Yt = H Xt + W˙ t ,
¯ m
µ˙ t = Aµt + Σt H T R−1
(N)
In this section, we compare the PDA-FPF to PDAF with the aid of a linear example. Consider the following linear model:
(13)
The formula (13) is verified by direct substitution in (7) where the distribution p is Gaussian. Using the formula for the gain function, the linear PDA-FPF is given by,
N
1 d
(2π) 2 |Σt | 2
be Gaussian with mean µt and variance Σt . Then the solution of the E-L BVP (7) is given by:
(9)
where L(Ytm ) =
where A is a d × d matrix and H is an m × d matrix. We assume the initial distribution p∗ (x, 0) is Gaussian with mean µ0 and covariance matrix Σ0 . The following lemma provides the solution of the gain function K(x,t) in the linear case. Lemma 1: Considerthe linear observation (12b). Suppose 1 1 T T p(x,t) = d 1 exp − 2 (x − µt ) Σt (x − µt ) is assumed to
(17)
The data association probability βtm is also evaluated by using particles:
(12a) (12b)
2060
βtm ∝
1 1 N (2π) d2 |R| 12
N
∑ exp
i=1
−kYtm − HXti k2R .
TABLE I C OMPARISON OF THE NONLINEAR PDA-FPF
State model βtm
PDAF ALGORITHM
PDAF
PDA-FPF
X˙t = AXt + σB B˙t
X˙t = a(Xt ) + σB B˙t
Yt = HXt + W˙ t
Observation model Assoc. Prob.
ALGORITHM WITH THE LINEAR
Yt = h(Xt ) + W˙ t t
:= P([At = m]|Z ) m
m
Yˆt = Hµt
β β Yˆti,m = [ 2t h(Xti ) + (1 − 2t )hˆ t ]
Innovation error
Itm = Ytm − Yˆt
Iti,m = Ytm − Yˆti,m
Feedback control
Utm = Kg Itm
Uti,m = K(Xti ,t)Iti,m
Kalman Gain
Sol. of a BVP (7)
¯ m m ∑m m=1 βt Ut m ∝ exp −kYt − Yˆt k2R
¯ m ∑m m=1 βt Ut 1 N ∝ N ∑i=1 exp −kYtm − h(Xti )k2R X˙ti = a(Xti ) + σB B˙ti + K(Xti ,t) ∑m βtmUti,m
Prediction
Gain Control input Formula for βtM Filter equation
i,m
µ˙ t = Aµt + Kg ∑m βtmUtm
F. Comparison of PDA-FPF and PDAF Table I provides a comparison of the PDA-FPF and the PDAF algorithms. The main point to note is that the feedback particle filter has an innovation error-based structure: In effect, the ith particle makes a prediction Yˆti,m as a weighted-average of h(Xti ) and hˆ t . This is then used to compute an innovation error Iti,m . The Bayes’ update step involves gain feedback of the innovation error.
at1 , Yt2 originates from target at2 , . . . , Ytn¯ originates from target atn¯ . The following models are assumed for the three stochastic processes. (i) The dynamics of the nth target evolves according to the nonlinear ODE: X˙tn = an (Xtn ) + σBn B˙tn , {B˙tn
where ∈ are mutually independent white noise processes. (ii) The observation model is given by: 1 1 Yt h(Xt1 ) W˙ t .. .. .. (19) . = Ψ(At ) . + . , ¯ ¯ ¯ n n n ˙ Yt h(Xt ) Wt
III. M ULTIPLE TARGET T RACKING U SING F EEDBACK PARTICLE F ILTER In this section, we extend the PDA-FPF to the multiple target tracking problem. The resulting filter is referred to as the joint probabilistic data association feedback particle filter (JPDA-FPF). For notational ease, we assume that all measurements originate from targets (That is, there is no clutter).
where {W˙ tn } are mutually independent white noise processes with covariance matrix R whose dimension is s×s. Furthermore, {W˙ tn } are mutually independent with {B˙tn } and Ψ(At ) is the permutation matrix for association At . Example 1: Suppose n¯ = 2. The permutation matrices are Is 0 0 Is Ψ({1, 2}) = , Ψ({2, 1}) = , (20) 0 Is Is 0
A. Problem Statement, Assumptions and Notation The following notation is adopted: (i) There are n¯ distinct targets. The set of targets is denoted by an index set N = {1, 2, . . . , n¯ }. The set of permutations of N is denoted by P(N ). The cardinality of the set P(N ), |P(N )| = n¯ !. A typical elements of P(N ) is denoted as a = (a1 , a2 , . . . , an¯ ), where an ∈ N for all n∈N . (ii) At time t, the state of the nth target is denoted as Xtn ∈ Rd for n ∈ N . (iii) At time t, there is exactly one measurement per target for a total of n¯ measurements. There are no missing measurements and no measurements because of clutter. The observation vector Y t := (Yt1 ,Yt2 , . . . ,Ytn¯ )T , where the nth entry, Ytn ∈ Rs , originates from one of the targets in N. (iv) At time t, the association random variable is denoted as At ∈ P(N ). It is used to associate targets with the measurements: At = at = (at1 , at2 , . . . , atn¯ ) ∈ P(N ) signifies that the measurement Yt1 originates from target
(18)
¯ Rd }nn=1
where Is is an identity matrix with dimension s. (iii) The model for At is the same as the model assumed in discussions of PDA-FPF (see Sec. II-A) (iv) At and X t are assumed to be mutually independent. The problem is to design n¯ feedback particle filters, where the nth filter is intended to estimate the posterior distribution of the nth target given the history of all unassociated observations (filtration) Y t := σ (Y s : s ≤ t). The posterior distribution is denoted as pn (x,t). As before, the algorithm comprises of the following two parts: (i) Evaluation of the measurement-to-target association probability, and (ii) Integration of association probability with the feedback particle filter.
2061
B. JPDA-FPF The association probability is defined as the conditional probability of association [atr = n]: βtn,r := Pr [atr = n] Z t ,Y t . (21) In words, βtn,r is the conditional probability that the rth measurement, Ytr , originates from the nth target. Using these probabilities, and denoting the state of the ith particle for the nth target at time t as Xtn,i , i ∈ {1, 2, . . . , N}, the JPDA-FPF algorithm for the nth target is as follows: X˙tn,i
n
=a
(Xtn,i ) + σBi B˙tn +
n¯
∑
βtn,r Kn (Xtn,i ,t)Itn,i,r ,
where Itn,i,r is a modified form of the innovation process, n,r βtn,r ˆ n βt n,i n,i,r r h(Xt ) + (1 − )ht (23) It := Yt − 2 2 with hˆ tn := E[h(Xtn )|Z t ], and Kn is control gain obtained from solving a certain EL-BVP: (24)
where pn (x,t) denotes the posterior distribution. In practice it is approximated by a constant-valued formula: T N ˜ n = 1 ∑ Xtn,i h(Xtn,i ) − hˆ tn(N) R−1 , K N i=1
(25)
n(N)
where hˆ t = N1 ∑Ni=1 h(Xtn,i ). Finally, the association probabilities are calculated easily using Bayes’ rule as in Sec. II-D. In particular, βtn,r =
∑
{b∈P(N
b
γt ,
(26)
d 2
(2π) |R| ≈
n¯ 1 2
k=1 n¯
1 d
∏
1
(2π) 2 |R| 2
∏
k=1
Rd
h i exp −kYtk − h(x)k2R pbk (x,t) dx
n¯
m=1 r=1
where formulae for innovation error and the gain function are similar to (23)- (25). The association probabilities are obtained, separately for each sensor, using formulae similar to (26). IV. N UMERICS A. Single target tracking problem Consider a target tracking problem with two bearing-only sensors [17]. A single target moves in a two-dimensional (2d) plane according to the standard white-noise acceleration model: X˙t = AXt + ΓB˙t , (28) where X := (X1 ,V1 , X2 ,V2 )T ∈ R4 , (X1 , X2 ) denotes the position and (V1 ,V2 ) denotes the velocity. The matrices, 0 0 0 1 0 0 1 0 0 0 0 0 Γ = σB A= 0 0 , 0 0 0 1 , 0 1 0 0 0 0
(29)
where W˙ t is a standard 2d white noise, h = (h1 , h2 )T and ! (sen j) x2 − x2 , j = 1, 2, h j (x1 , v1 , x2 , v2 ) = arctan (sen j) x1 − x1
k=1
1
¯ m
∑ ∑ βtn,r;m Kn;m (Xtn,i ,t)Itn,i,r;m ,
Yt = h(Xt ) + σW W˙ t ,
n¯ b γt ∝ ∏ Pr Ytk [atk = bk ] , =
X˙tn,i = an (Xtn,i ) + σBi B˙tn,i +
and B˙t is a standard 2d white noise. The observation model is given by,
):br =n}
where
Z
¯ and n, r ∈ N . It is the conditional for m ∈ {1, 2, . . . , m} probability that the rth measurement from the mth sensor originates from the nth target. Using the association probabilities, the filter for the nth target is
(22)
r=1
∇ · (pn Kn ) = −(h − hˆ tn )T R−1 pn ,
In particular, we denote the measurements for the mth sensor ¯ )T . The association probabilities are as Y tm = (Yt1;m , . . . ,Ytn;m now denoted as βtn,r;m = Pr [atr;m = n] Z t ,Y tm (27)
! h i 1 N b ,i ∑ exp −kYtk − h(Xt k )k2R . N i=1
(sen j)
As the number of targets increases, there is an exponential growth in the number of associations. In practice, one may also wish to consider approaches to reduce filter complexity, e.g., by assigning gating regions for the measurements; cf., Sec. 4.2.3 in [15]. This is the subject of future investigation. C. Multi-sensor Case ¯ sensors that provide indepenNow suppose there are m dent measurements of n¯ targets. The filter for this case is a straightforward generalization of the single-sensor filter described in the preceding section: The notation specific to ¯ is tagged with sensor index m. sensor m ∈ {1, 2, . . . , m}
(sen j)
) denote the position of sensor j. where (x1 , x2 Figure 2 depicts a sample path obtained for a typical numerical experiment. The sensor and target locations are depicted together with an estimate (conditional mean) that is approximated using a feedback particle filter. Since there is only single target, there is no data association uncertainty. The background depicts the ensemble of observations that were made over the simulation run. Each point in the ensemble is obtained by using the process of triangulation based on two (noisy) angle measurements. The simulation parameters are: The initial position of the target is depicted, the initial velocity was chosen as (0.2, −5) and σB = 0.1; The two sensor positions are depicted and σW = 0.017; The particle filter comprised of N = 200 particles
2062
40 Meas.
30
Tgt. Ptcle. Mean Sensor
20
Tgt. Init. Ptcle. Init
10
0
-10 -30
-20
-10
0
10
20
30
Fig. 2. Simulation results: Comparison of the true target trajectory with the estimate obtained using FPF. Fig. 4. Illustration of “ghost” target in the two-sensor two-target case: The ghost appears because of incorrect data association.
association probabilities for target 1, and βt2,1;1 , βt2,2;1 are the probabilities for target 2 (see (27)). The simulation parameters are: The two targets start at position (−20, 50) and (20, 50), respectively. The initial velocity vectors are V01 = V02 = (0.0, −5.0). σB = 0.1, σW = 0.017 and the number of particles N = 200.
Fig. 3.
Simulation results for two target example.
whose initial position is chosen from a Gaussian distribution whose mean is depicted. The gain function is obtained using the constant gain approximation in (25). The simulation results show that the filter can adequately track the target. B. Two targets Consider a tracking problem with two targets as depicted in Fig. 3. The targets move in the 2d plane according to the standard white-noise model (28). There are two bearing-only sensors also depicted in the figure. At time t, each sensor obtains two angle only measurements according to model (29). There is data association uncertainty in this case, in the sense that one cannot assign measurements to individual targets in an apriori manner. In this particular example, errors in data association can lead to appearance of a “ghost” target (see Fig. 4). In the simulation study, tracks are initialized at the location of the “ghost” target (black circles in Fig. 3). Particles are initialized by drawing from a Gaussian distribution whose mean is set to be the “ghost” target location. As depicted in Fig. 4, this position is identified by using the process of triangulation based on initial positions of the two targets. Figure 3 depicts the estimate (mean of particles) obtained using the JPDAFPF algorithm. At each time t, four association probabilities are approximated for each sensor: βt1,1;1 , βt1,2;1 are the two
C. Track coalescence example Track coalescence is a common problem in multiple target tracking applications. Track coalescence can occur when two closely spaced targets move with approximately the same velocity over a time period [6], [18]. With standard implementations of JPDAF and SIR particle filter algorithms, the target tracks tends to coalesce even after the targets have moved apart [18]. In the following example, we describe simulation results for JPDA-FPF for a model problem scenario related to [19]. As in the preceding example, there are two targets in the 2d plane, modeled by (28). Initially, the two targets are at position (−40, 600) and (40, 600), respectively. They move towards each other with initial velocity vector (4.5, −5.0) and (−4.5, −5.0), respectively. When they are close to each other, they move vertically together for some time (velocity along x is set to 0). Finally, they move away from each other with velocity vectors (−4.5, −5.0) and (4.5, −5.0), respectively. Two sensors obtain two bearing-only measurements, but with data association uncertainty. Figure 5(a) depicts the results of a single simulation: The tracks are initialized at the actual target location. The estimates are obtained using the JPDA-FPF with N = 200 particles. Figure 5(b) depicts the association probability during the simulation run. As shown in the figure, the tracks coalesce when the two targets are close. However, the filter tracks the targets once they move away. That is, the track coalescence problem is successfully avoided. D. Three targets In this numerical experiment, the objective is to track three targets with two bearing-only sensors. The initial target
2063
Fig. 5.
Fig. 6.
(a) Simulation results for the track coalescence example; (b) Plot of data association probability.
Simulation results for the three targets example.
positions are (−40, 300), (0, 300) and (40, 300), respectively. The initial velocity vectors are V 1 = V 2 = V 3 = (0, −5). Figure 6 depicts the results of a simulation study. With only two sensors, the filter performance degrades as the number of targets increases beyond 3. ACKNOWLEDGEMENT We are grateful to Dr. Samuel Blackman for suggesting the application problem described in this paper. R EFERENCES [1] T. Yang, G. Huang, and P. G. Mehta, “Joint probabilistic data association-feedback particle filter for multiple target tracking applications,” In Proc. of American Control Conference, Jun 2012. [2] T. Yang, P. G. Mehta, and S. P. Meyn, “A mean-field control-oriented approach to particle filtering,” In Proc. of American Control Conference, pp. 2037–2043, June 2011. [3] ——, “Feedback particle filter with mean-field coupling,” In Proc. of IEEE Conference on Decision and Control, pp. 7909–7916, December 2011.
[4] Y. Bar-Shalom, F. Daum, and J. Huang, “The probabilistic data association filter,” IEEE Control Systems Magazine, vol. 29, no. 6, pp. 82–100, Dec 2009. [5] S. Thrun, “Probabilistic robotics,” Commun. ACM, vol. 45, pp. 52–57, March 2002. [Online]. Available: http://doi.acm.org/10.1145/504729.504754 [6] S. S. Blackman, Multiple-Target Tracking with Radar Applications. Boston, MA: Artech House, 1986. [7] T. Kirubarajan and Y. Bar-Shalom, “Probabilistic data association techniques for target tracking in clutter,” Proceedings of The IEEE, vol. 92, no. 3, pp. 536–557, 2004. [8] B. Kragel, S. Herman, and N. Roseveare, “A comparison of methods for estimating track-to-track assignment probabilities,” IEEE Trans. Aerospace and Electronic Systems, 2011, in Press. [9] F. Daum and J. Huang, “Generalized particle flow for nonlinear filters,” In Proc. of SPIE, p. 7698, 2010. [10] G. Terejanu, P. Singla, T. Singh, and P. D. Scott, “Uncertainty propagation for nonlinear dynamical systems using gaussian mixture models,” AIAA Journal of Guidance, Control and Dynamics, vol. 31, no. 6, pp. 1623–1633, Nov. 2008. [11] A. Budhiraja, L. Chen, and C. Lee, “A survey of numerical methods for nonlinear filtering problems,” Physica D: Nonlinear Phenomena, vol. 230, no. 1-2, pp. 27–36, 2007. [12] C. Hue, J.-P. L. Cadre, and P. Prez, “Tracking multiple objects with particle filtering,” IEEE Trans. Aerospace and Electronic Systems, vol. 38, no. 3, pp. 791–812, July 2002. [13] I. Kyriakides, D. Morrell, and A. Papandreou-Suppappola, “Sequential Monte Carlo methods for tracking multiple targets with deterministic and stochastic constraints,” IEEE Trans. Signal Process., vol. 56, no. 3, pp. 937–948, 2008. [Online]. Available: http://dx.doi.org/10.1109/TSP.2007.908931 [14] S. Oh, S. Russell, and S. Sastry, “Markov chain Monte Carlo data association for multi-target tracking,” IEEE Trans. Automat. Control, vol. 54, no. 3, pp. 481–497, 2009. [Online]. Available: http://dx.doi.org/10.1109/TAC.2009.2012975 [15] Y. Bar-Shalom and T. E. Fortmann, Tracking and Data Association. San Diego, CA: Academic Press, 1988. [16] T. Yang, R. S. Laugesen, P. G. Mehta, and S. Meyn, “Multivariable feedback particle filter,” submitted to IEEE Conference on Decision and Control, Dec 2012. [17] Y. Bar-Shalom, T. Kirubarajan, and X.-R. Li, Estimation with Applications to Tracking and Navigation. New York, NY, USA: John Wiley & Sons, Inc., 2002. [18] H. A. P. Blom and E. A. Bloem, “Probabilistic data association avoiding track coalescence,” vol. 45, no. 2, pp. 247–259, 2000. [19] ——, “Joint particle filtering of multiple maneuvering targets from unassociated measurements,” Journal of Advancement Information Fusion, vol. 1, pp. 15–36, 2006.
2064
