The PHD Filter for Extended Target Tracking With Estimable Extent ...
The PHD Filter for Extended Target Tracking With Estimable Extent Shape Parameters of Varying Size Anthony Swain and Daniel Clark EECE EPS Heriot Watt University Edinburgh, UK Email: [email protected] and [email protected]
Abstract—In extended target tracking, targets potentially produce more than one measurement per time step. In recent random finite set (RFS) approaches, the set of measurements obtained from an extended target is modelled as a point process. In this paper, we expand on the RFS approach to extended target tracking by considering a hierarchical point process representation of multiple extended target, more specifically a Poisson cluster process. This allows us to impose a geometric shape, in particular an ellipse, on each extended target. The set of target states, which are characterised by the kinematic variables and the shape parameters, represents the higher level (parent) process and the set of points on the boundary, from which measurements are generated, represents the lower level (daughter) process. We describe the PHD filter for multiple extended targets, whose extents vary in size, that estimates the shape parameters of the targets jointly with their positions and velocities. The main contribution of this paper is the practical implementation we propose, based on a particle-system representation for the targets’ shape and a Gaussian mixture formulation for the kinematic state per particle. The method is demonstrated on simulated data for multiple elliptical shaped extended targets.
Keywords: random finite sets, PHD filters, cluster processes, Gaussian mixture. I. I NTRODUCTION The random finite set (RFS) approach to multi-target tracking has proven to be a useful method that allows the problem of estimating multiple dynamic targets in the presence of false measurements and detection uncertainty to be cast in a Bayesian filtering framework [1]. By treating the collection of individual targets as a set-valued state and the collection of observation measurements as a set-valued observation, this theoretically optimal approach is an elegant generalisation of the single-target Bayes filter. However the propagation of the posterior probability distribution in the multi-target Bayes filter is often computationally unattainable due to the high dimensionality of the target RFS. To overcome this intractability, the Probability Hypothesis Density (PHD) filter was introduced [1], which propagates the first-order moment, otherwise known as the intensity. The PHD filter is typically implemented using a particle representation applying sequential Monte Carlo (SMC) techniques [2], [3] or with a Gaussian mixture formulation [4]– [6] and has applications in radar [7], computer vision [8] and acoustics [9], to name but a few. In most applications, including the PHD filter, it is assumed that each target produces at most one measurement per time-
step. In extended target tracking, targets potentially produce more than one measurement per time-step. Multiple measurements per target per time-step gives rise to the possibility of estimating the target’s shape and size in addition to its position and velocity. An extension of the PHD filter to handle extended targets was presented in [10], recently implemented using a Gaussian mixture formulation [11], which involved representing the measurements generated from targets as a spatial point process [12], specifically a Poisson point process. That is, at each time-step, a Poisson distributed number of measurements are generated, spatially distributed around the target. Such a measurement model implies the target generated measurements resemble a cluster of points, rather than a geometrically structured arrangement. A number of methods based on the RFS approach have been proposed for estimating the size and shape of a target [13], [14]. In [13], a direct application of the Gaussian mixture PHD filter for extended targets [11] is presented in which a method is proposed for calculating a likelihood function that relates a set of measurements, potentially, to a target whose structure resembles that of a rectangle or an ellipse. In [14], a partitioned state representation of an extended target is considered, which consists of linear (position & velocity) and non-linear (heading & target shape parameters) components. The PHD filter is applied for jointly estimating this target state and a set of measurement generating points defined on the boundary of each target’s geometrically structured shape (rectangle), and implemented using a Rao-Blackwellized particle filter as demonstrated for the Simulation Localisation and Mapping (SLAM) problem in [15]. Other proposed approaches for extended target size and shape estimation include a Random Hypersurface Model method [16] that assumes varying measurement sources which lie on scaled versions of shape boundaries, and a novel SMC approach [17] that considers target generated measurements bounded by a circular region which is estimated jointly with the target’s kinematic state. In this paper we consider an alternative approach whereby multiple extended targets are modelled as a hierarchical point process as proposed in [18]. Hierarchical point processes, known as cluster point processes, have been previously exploited in the group target tracking problem [19], [20], the SLAM problem [21], and more recently in sensor registration
1111
[22]. In this paper, we specifically consider a Poisson cluster point process representation of multiple extended targets, which allows us to impose a geometric shaped extent on each target, where the centres of each target’s extent represent the higher level (parent) process on which a dynamic model can be specified, and the set of points on the boundary of each target’s extent represents the lower level (daughter) process from which the measurements are generated. In the PHD filter for this approach to extended target tracking, the kinematic states and the shape parametric variables are estimated jointly via the first-order moment. The remainder of the paper is organised as follows: Sections II and III present the proposed methodology and implementation. The PHD recursion in Section II can easily be derived using a general chain rule and Gateaux differentials [23]–[25]. Section IV details a multiple extended target tracking example, whose extents are elliptical in shape and vary in size over time. The estimation results from the PHD filter implementation for this example using simulated data are then discussed, followed by concluding remarks in Section V. II. E XTENDED TARGET TRACKING USING CLUSTER PROCESSES
In this work, we model multiple extended targets, whose extents have a geometrically shaped structure and vary in size, as a doubly-stochastic point process. In particular, we suppose the multiple extended targets have a Poisson cluster process representation, defined by 𝕏𝑘 = {(X𝑘,1 , Ξ𝑘,1 ), . . . , (X𝑘,𝑛 , Ξ𝑘,𝑛 )} ]𝑇 [ where for 𝑖 = 1, . . . , 𝑛 the state X𝑘,𝑖 = x𝑘,𝑖 s𝑘,𝑖 consists of the random vector x𝑘,𝑖 defining the kinematic state and the random vector s𝑘,𝑖 defining the shape parameter variables for target 𝑖. The RFS Ξ𝑘,𝑖 = {𝝃𝑘,1 , . . . , 𝝃 𝑘,𝑛𝑖 } represents the points on the boundary of the extent for target 𝑖. The parent process is represented by the RFS of target states X𝑘,𝑖 with which there corresponds a daughter process, represented by the RFS Ξ𝑘,𝑖 conditioned on X𝑘,𝑖 , for 𝑖 = 1, . . . , 𝑛. In addition, we consider a measurement model, defined by the RFS 𝑍𝑘 , that takes into account detection uncertainty and false measurements (clutter) and is formed by the union of target generated measurements Θ𝑘 (𝕏𝑘 ) and clutter 𝐾𝑘 , i.e. 𝑍𝑘 = 𝐾𝑘 ∪ Θ𝑘 (𝕏𝑘 ).
(1)
In recent RFS approaches [10], [11], [13], [14], the set of false measurements 𝐾𝑘 is modelled as a Poisson point process, the probability density for which is ( ∫ ) ∏ 𝜆(z), (2) 𝑝𝑘 (𝐾𝑘 ) = exp − 𝜆(z) dz z∈𝐾𝑘
where 𝜆(z) is the intensity of the clutter process. An integral part of extended target tracking with the PHD filter is the partitioning of the measurement set. A partition 𝜋 is defined as a division of the RFS 𝑍𝑘 into subsets, denoted ∪ as 𝜑, such that 𝜑∈𝜋 𝜑 = 𝑍𝑘 . We denote the set of all possible partitions of 𝑍𝑘 as Π𝑍𝑘 . To assist the partitioning operation, instead of the false measurement model given by the
probability density in (2), we model 𝐾𝑘 as a Poisson cluster point process, the probability density for which is )∏ ( ∫ 𝜆1 (u𝜑𝜅 ) 𝑝𝑘 (𝐾𝑘 ) = exp − 𝜆1 (u𝜑𝜅 ) du𝜑𝜅 𝜑
)𝜅 ∏ ( ∫ 𝜆2 (z ∣ u𝜑𝜅 ), × exp − 𝜆2 (z ∣ u𝜑𝜅 ) dz z∈𝜑𝜅
(3) where 𝜆1 (u𝜑𝜅 ) is the intensity of the parent process whose state u𝜑𝜅 is the centroid of the measurements in subset 𝜑𝜅 and 𝜆2 (z ∣ u𝜑𝜅 ) is the intensity of the daughter process. With the clutter process modelled in this way, the assumption is that the measurements appear in clusters. Let 𝑝𝑘 (𝕏𝑘 ∣ 𝑍1:𝑘 ) be the posterior probability distribution of the Poisson cluster process representation of the multiextended target state 𝕏𝑘 , given a time sequence of measurement sets denoted by 𝑍1:𝑘 = 𝑍1 , . . . 𝑍𝑘 . The Bayesian filter recursion for estimating the posterior 𝑝𝑘 (𝕏𝑘 ∣ 𝑍1:𝑘 ) involves computing set integrals [26] due to the RFS of target states X𝑘,𝑖 representing the parent process as well as the RFS definition of Ξ𝑘,𝑖 for 𝑖 = 1, . . . , 𝑛. The set integrals in this recursion involve multiple infinite sums over all possible posterior distributions at different cardinalities in the parent and daughter processes which makes implementing the Bayes filter computationally intractable for variable numbers of targets X and corresponding boundary points 𝝃. Instead we propagate the first-order moment approximation of the posterior 𝑝𝑘 (𝕏𝑘 ∣ 𝑍1:𝑘 ), an approach proposed for multi-target tracking under the name of the Probability Hypothesis Density (PHD) filter in [1]. The PHD filter for multiple extended targets modelled as a Poisson cluster process, with geometric shaped target extents, jointly propagates the kinematic state and shape parametric via the first-order moment, or intensity. Throughout the PHD filter recursion, the propagation of the kinematic state will depend on the shape parametric since the RFS Ξ of boundary points, from which measurements are generated, updating the kinematic state, are partly determined by the shape parameters. Therefore the joint prior PHD can be factorized as follows 𝑀𝑘−1 (s′ , x′ ) = 𝑀𝑘−1 (s′ ) × 𝑀𝑘−1 (x′ ∣ s′ ),
(4)
where 𝑀𝑘−1 (s′ ) = 𝑀𝑘−1 (s𝑘−1 ) is the intensity of the shape parametric and 𝑀𝑘−1 (x′ ∣ s′ ) = 𝑀𝑘−1 (x𝑘−1 ∣ s𝑘−1 ) is the intensity of the kinematic state. Now suppose that the targets’ positions and shapes evolve independently of one another so that dynamics can be factorized as 𝑓𝑘∣𝑘−1 (X ∣ X′ ) = 𝑓𝑘∣𝑘−1 (s ∣ s′ )𝑓𝑘∣𝑘−1 (x ∣ x′ ), where 𝑓𝑘∣𝑘−1 (s ∣ s′ ) is the Markov transition density for the shape parametric and 𝑓𝑘∣𝑘−1 (x ∣ x′ ) is the Markov transition density for the kinematic state. This is representative of a scenario in which the targets’ shape parametrics do not include the orientations of the shapes, which are instead determined by the targets’ velocities, that are incorporated into the kinematic state. The PHD prediction equation is then
1112
𝑀𝑘∣𝑘−1 (s, x) = 𝑀𝑘∣𝑘−1 (s) × 𝑀𝑘∣𝑘−1 (x ∣ s),
(5)
where the predicted intensity of the shape parametric is ∫ 𝑀𝑘∣𝑘−1 (s) = 𝑀𝑘−1 (s′ ) 𝑓𝑘∣𝑘−1 (s ∣ s′ ) ds′ ,
very computationally demanding. If a single partition, denoted as 𝝅, is known the joint pseudo-likelihood reduces to (6) 𝐿𝑍𝑘 (s, x) = e−𝜇[1 ∣ s,x] +
where the predicted intensity of the kinematic state is
𝜑∈𝝅
𝑀𝑘∣𝑘−1 (x ∣ s) = 𝛾𝑘 (x ∣ s)+ ∫ 𝑝𝑆 (x′ ) 𝑓𝑘∣𝑘−1 (x ∣ x′ ) 𝑀𝑘−1 (x′ ∣ s) dx′ , (7) and 𝛾𝑘 (x ∣ s) is the intensity for birth targets. The probability of survival 𝑝𝑆 (x′ ) is also assumed to be shape independent. Given a new set of measurements 𝑍𝑘 and under the assumption that the daughter process can be approximated with a Poisson point process, the PHD update equation is 𝑀𝑘 (s, x) = ∫∫
𝐿𝑍𝑘 (s, x) 𝑀𝑘∣𝑘−1 (s) 𝑀𝑘∣𝑘−1 (x ∣ s) , 𝐿𝑍𝑘 (s, x) 𝑀𝑘∣𝑘−1 (s) 𝑀𝑘∣𝑘−1 (x ∣ s) ds dx (8)
where the joint pseudo-likelihood is ∑ ∑ 𝐿𝑍𝑘 (s, x) = e−𝜇[1 ∣ s,x] + 𝜔𝜋 𝜋∈Π𝑍𝑘
𝜑∈𝜋
Υ𝜑 (s, x) , Λ(𝜑) + 𝑀𝑘∣𝑘−1 [Υ𝜑 ] (9)
with partition weight given as ∏( ) Λ(𝜑) + 𝑀𝑘∣𝑘−1 [Υ𝜑 ] 𝜔𝜋 =
𝜑∈𝜋
∑
∏ (
),
Λ(𝜑′ ) + 𝑀𝑘∣𝑘−1 [Υ𝜑′ ]
(10)
𝜋 ′ ∈Π𝑍𝑘 𝜑′ ∈𝜋 ′
given the following denotations ∫ 𝜇[1 ∣ s, x] = 𝜇(𝝃 ∣ s, x) d𝝃, ∫ 𝑀𝑘∣𝑘−1 [Υ𝜑 ] = Υ𝜑 (s, x) 𝑀𝑘∣𝑘−1 (x ∣ s) dx, ( ∫ ) Υ𝜑 (s, x) = exp − 𝜇(𝝃 ∣ s, x) d𝝃 ) ∏ (∫ × 𝜇(𝝃 ∣ s, x) 𝑙z (𝝃, x) d𝝃 ,
∑
Υ𝜑 (s, x) . Λ(𝜑) + 𝑀𝑘∣𝑘−1 [Υ𝜑 ]
(13)
The study into appropriate partitioning methods is beyond the scope of this paper. However we assume, for the remainder of the paper, that such a partition 𝝅 exists for each measurement set. Finally, the presence of the lone exponential term in (13) can be accounted for by the Poisson point process approximation of the daughter process and the possible occurrence of an empty measurement set, attributing to missed detected targets. Hence this term can be regarded as the probability of missed detection, which for a daughter process with Poisson rate greater than 3 is less than 0.05, and can be interpreted in the following way, that the more boundary points on a target’s extent, the more measurements are likely to be generated resulting in a low probability of missed detection. III. I MPLEMENTATION We implement the PHD filter for the multiple extended target model, described in Section II, using a Dirac mixture model for the intensity of the target shape. Each component of this mixture model has associated with it a Gaussian mixture formulation for the intensity of the kinematic state conditioned on that particular shape component. At each iteration of the filter, we start with the following prior intensities 𝑁𝑘−1
𝑀𝑘−1 (s) =
∑ 𝑖=1
(𝑖)
(𝑖)
𝜛𝑘−1 𝛿(s − s𝑘−1 ),
(14)
(𝑖)
(𝑖) 𝑀𝑘−1 (x ∣ s𝑘−1 )
𝐽𝑘−1
=
(11)
z∈𝜑
for daughter process intensity 𝜇(𝝃 ∣ s, x) and singlemeasurement likelihood 𝑙z (𝝃, x). The clutter term is given by ∏ Λ(𝜑) = 𝜆1 (u𝜑 ) e−𝜆2 [1 ∣ u𝜑 ] 𝜆2 (z ∣ u𝜑 ), (12) z∈𝜑
∑ 𝑗=1
(𝑗∣𝑖)
(𝑖)
(𝑗∣𝑖)
(𝑗∣𝑖)
𝜔𝑘−1 𝒩 (x ∣ s𝑘−1 ; m𝑘−1 , P𝑘−1 ). (15)
The notation 𝒩 (x ; m, P) is used to denote a Gaussian distribution with mean vector m and covariance matrix P, and 𝛿(s) denotes the Dirac delta distribution centred at s. The mixture in (14) defines a particle representation of 𝑀𝑘−1 (s), which can be expressed as the set of weighted particles 𝑁𝑘−1 (𝑖) (𝑖) . {𝜛𝑘−1 , s𝑘−1 }𝑖=1 A. Gaussian mixture formulation
for false measurements modelled as a Poisson cluster process whose probability density is defined by (3). The likelihood 𝐿𝑍𝑘 (s, x) updates both the shape parametric s and kinematic state x and involves the summation over all possible partitions Π𝑍𝑘 of the measurement set. The number of partitions ∣Π𝑍𝑘 ∣ for a measurement set of size ∣𝑍𝑘 ∣ = 𝑛 equates to the 𝑛th Bell number (𝐵𝑛 = 1, 2, 5, 15, 52, 203, 877, 4140, 21147, 115975, . . . for 𝑛 = 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, . . . respectively). Consequently the operation of summing over all possible partitions in (9) is
Following the derivation of the Gaussian mixture PHD filter for single measurement targets in [4] and the single-group PHD filter in [20], a Gaussian mixture formulation can be attained for the predicted intensity of the kinematic state in (7) and the updated intensity of the kinematic state defined by (𝑖)
(𝑖)
(𝑖)
𝑀𝑘 (x ∣ s𝑘 ) = 𝐿𝑍𝑘 (s𝑘 , x) 𝑀𝑘∣𝑘−1 (x ∣ s𝑘 ), (𝑖)
(16)
where 𝐿𝑍𝑘 (s𝑘 , x) is the joint pseudo-likelihood given in (9) (𝑖) for each particle s𝑘 .
1113
(𝑖)
For the predicted intensity 𝑀𝑘∣𝑘−1 (x), we assume a linear Gaussian dynamic model given by 𝑓𝑘∣𝑘−1 (x ∣ x′ ) = 𝒩 (x ; Fx′ , Q),
(17)
where F is the state transition matrix and Q is the process noise covariance matrix. In addition suppose the probability of survival is state independent, i.e. 𝑝𝑆 (x′ ) = 𝑝𝑆 , and the intensity for the birth targets is a Gaussian mixture of the form (𝑖)
(𝑖) 𝛾𝑘 (x ∣ s𝑘 ) (𝑖)
∑
(𝑗∣𝑖) 𝜔𝛾,𝑘
𝑗=1 (𝑗∣)
(𝑖) 𝒩 (x ∣ s𝑘
(𝑗∣𝑖)
(𝑗∣𝑖) (𝑗∣𝑖) ; m𝛾,𝑘 , P𝛾,𝑘 ),
(18)
(𝑗∣𝑖)
(𝑖)
where 𝐽𝛾,𝑘 , 𝜔𝛾,𝑘 , m𝛾,𝑘 and P𝛾,𝑘 for 𝑗 = 1, . . . , 𝐽𝛾,𝑘 are given model parameters that determine the shape of the birth intensity for a particular particle 𝑖. The prediction for the kinematic state is then the same as that for the standard Gaussian mixture PHD filter (𝑖)
𝐽𝑘∣𝑘−1
(𝑖)
∑
𝑀𝑘∣𝑘−1 (x ∣ s𝑘 ) =
(𝑗∣𝑖)
(𝑖)
(𝑗∣𝑖)
(𝑗∣𝑖)
𝜔𝑘∣𝑘−1 𝒩 (x ∣ s𝑘 ; m𝑘∣𝑘−1 , P𝑘∣𝑘−1 ),
𝑗=1
(19) (𝑗∣𝑖)
(𝑗∣𝑖)
(𝑗∣𝑖)
where for birth targets we have 𝜔𝑘∣𝑘−1 = 𝜔𝛾,𝑘 , m𝑘∣𝑘−1 = (𝑗∣𝑖)
(𝑗∣𝑖)
(ℓ∣𝑖,𝑗)
and
(𝑖) 𝐽𝑘∣𝑘−1
(𝑗∣𝑖) 𝑝𝑆 𝜔𝑘−1 , (𝑗∣𝑖) Fm𝑘−1 ,
= =
⎡
(𝑖) 𝐽𝛾,𝑘
+
(ℓ∣𝑖,𝑗)
)𝑇
(ℓ∣𝑖,𝑗)
P𝝃,12
(𝑗∣𝑖)
P𝑘∣𝑘−1
(𝑖)
⎤ ⎦, (𝑗∣𝑖)
(𝑗∣𝑖)
(𝑗∣𝑖)
(ℓ∣𝑗,𝑖)
(ℓ∣𝑗,𝑖)
m𝝃 , P𝝃,11 are the means and covariances from the corresponding marginal Gaussian for the daughter process, (ℓ∣𝑗,𝑖) and P𝝃,12 are the covariances that describe the relationship between parent and daughter. As a consequence of the Gaussian mixture formulation for the daughter intensity, in the joint pseudo-likelihood given in (𝑖) (13), we have e−𝜇[1 ∣ s𝑘 ,x] = e−𝛼 , denoting the expected ∑𝐽𝝃(𝑗∣𝑖) (ℓ) number of boundary points as 𝛼 = ℓ=1 𝜐𝝃 , and in Υ𝜑 (s, x) we have ⎛ ∏ ∏ ⎜ (𝑖) ˆ R2 ) 𝜇[𝑙z ∣ s𝑘 , x] = ⎝ 𝒩 (z ; Hx, z∈𝜑
z∈𝜑
(𝑖)
(21)
ℓ=1
(ℓ)
(𝑖)
(ℓ∣𝑖,𝑗)
⎞
ℓ=1
(ℓ) (𝑖) (ℓ∣𝑖,𝑗) ˜ (ℓ∣𝑖,𝑗) )⎟ 𝜐𝝃 𝒩 (z ∣ s𝑘 ; Hm𝝃,𝑘∣𝑘−1 , R ⎠,
¯ 𝚤=1
(¯ 𝚥1 ,...,¯ 𝚥∣𝜑∣ )
(𝑖)
where the mean and covariance are given by ( )−1 ( ) (ℓ∣𝑖,𝑗) (ℓ∣𝑖,𝑗) (ℓ∣𝑖,𝑗) (𝑗∣𝑖) (𝑗∣𝑖) x − m𝑘∣𝑘−1 , + P𝝃,12 P𝑘∣𝑘−1 m𝝃,𝑘∣𝑘−1 = m𝝃 ( )−1 ( )𝑇 (ℓ∣𝑖,𝑗) (ℓ∣𝑖,𝑗) (ℓ∣𝑖,𝑗) (𝑗∣𝑖) (ℓ∣𝑖,𝑗) P𝝃,12 . P𝝃,𝑘∣𝑘−1 = P𝝃,11 − P𝝃,12 P𝑘∣𝑘−1 (23)
(¯ 𝚥 ∣𝑖,𝑗)
(26)
𝚥1 , . . . , 𝚥¯∣𝜑∣ ), then Now for further brevity, we denote 𝚥¯1:∣𝜑∣ = (¯ the measurement update for the kinematic state is (𝑖)
(𝑖)
𝑀𝑘 (x ∣ s𝑘 ) = e−𝛼 𝑀𝑘∣𝑘−1 (x ∣ s𝑘 )+
𝜑∈𝝅 𝑗=1 𝚥¯1:∣𝜑∣
(22)
⎞
¯ 𝚤 ˜ (¯𝚥¯𝚤 ∣𝑖,𝑗) )⎠ . ,R × 𝒩 (z¯𝚤 ∣ s𝑘 ; Hm𝝃,𝑘∣𝑘−1
∑ ∑ ∑
(ℓ∣𝑖,𝑗)
𝜐𝝃 𝒩 (𝝃 ∣ s𝑘 ; m𝝃,𝑘∣𝑘−1 , P𝝃,𝑘∣𝑘−1 ),
(25)
˜ (ℓ∣𝑖,𝑗) = HP(¯𝚥¯𝚤 ∣𝑖,𝑗) H𝑇 + R1 . where, for brevity, we denote R 𝝃,𝑘∣𝑘−1 This product of Gaussian mixtures can be expanded for 𝚥¯¯𝚤 = (𝑗∣𝑖) 1, . . . , 𝐽𝝃 and ¯𝚤 = 1, . . . , ∣𝜑∣ to give the following sum of Gaussian products ⎛ ∣𝜑∣ ∏ ∑ ˆ R2 ) ⎝ 𝜐 (¯𝚥¯𝚤 ) 𝒩 (z¯𝚤 ; Hx, 𝝃
(𝑖) 𝐽𝑘∣𝑘−1
(𝑗∣𝑖)
∑
∑
(20)
where 𝒩 (z ; H𝝃, R1 ) is the likelihood that the measurement z is related to the point 𝝃 on the boundary of the target’s shape, with projection matrix H and observation noise covariance ˆ R2 ) is the likelihood that the matrix R1 , and 𝒩 (z ; Hx, measurement z relates to the state vector x, with projection ˆ and observation noise covariance matrix R2 . In matrix H addition suppose the intensity of the daughter process is a mixture of conditional Gaussian functions of the form 𝐽𝝃
(𝑗∣𝑖)
, where m𝑘∣𝑘−1 ,
P𝑘∣𝑘−1 are the predicted means and covariances from (19),
×
P𝑘∣𝑘−1 = FP𝑘−1 F𝑇 + Q,
(𝑖) 𝐽𝑘−1 .
(24)
(𝑗∣𝑖)
ˆ R2 ), 𝑙z (𝝃, x) = 𝒩 (z ; H𝝃, R1 ) × 𝒩 (z ; Hx,
=
(ℓ∣𝑖,𝑗)
P𝝃,11
for 𝑗 = 1, . . . , 𝐽𝑘∣𝑘−1 and ℓ = 1, . . . , 𝐽𝝃
𝐽𝝃
For the updated intensity 𝑀𝑘 (x ∣ s𝑘 ) given in (16), we assume a separable linear Gaussian single-measurement likelihood given by
(𝑖) 𝜇(𝝃 ∣ s𝑘 , x)
and covariance given by the block
P𝝃,12
(𝑗∣𝑖)
=
𝑇
⎣(
m𝛾,𝑘 , P𝑘∣𝑘−1 = P𝛾,𝑘 , for persistent targets (𝑗∣𝑖) 𝜔𝑘∣𝑘−1 (𝑗∣𝑖) m𝑘∣𝑘−1
(𝑗∣𝑖)
m𝑘∣𝑘−1
m𝝃 matrix
(𝑗∣𝑖)
𝐽𝛾,𝑘
=
The Gaussian components in (22) are conditional Gaussian realisations from joint Gaussians with mean ] [
(¯ 𝚥
1:∣𝜑∣ 𝜔𝜑,𝑘
,𝑗∣𝑖)
(27) (¯ 𝚥
(𝑖)
𝒩 (x ∣ s𝑘 ; m𝑘 1:∣𝜑∣ (¯ 𝚥
,𝑗∣𝑖)
,𝑗∣𝑖)
(¯ 𝚥
, P𝑘 1:∣𝜑∣
,𝑗∣𝑖)
),
and covariances where for 1 ≤ ¯𝚤 ≤ ∣𝜑∣ the means m𝑘 1:¯𝚤 (¯ 𝚥 ,𝑗∣𝑖) P𝑘 1:¯𝚤 in the Gaussian components are ( ) (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) ˆm ˜ 𝑘 1:¯𝚤 ˜ 𝑘 1:¯𝚤 z¯𝚤 − H , =m + K𝑘 1:¯𝚤 m𝑘 1:¯𝚤 ( ) (¯ 𝚥1:¯𝚤 ,𝑗∣𝑖) (¯ 𝚥1:¯𝚤 ,𝑗∣𝑖) ˆ (¯ 𝚥 ,𝑗∣𝑖) ˜ 1:¯𝚤 P𝑘 = I − K𝑘 , H P 𝑘 (28)
1114
( )−1 (¯ 𝚥 ,𝑗∣𝑖) ˜ (¯𝚥1:¯𝚤 ,𝑗∣𝑖) H ˆ 𝑇 S(¯𝚥¯𝚤 ∣𝑖,𝑗) and K𝑘 1:¯𝚤 = P . The weights of 2 𝑘 the Gaussian components are (¯ 𝚥
1:∣𝜑∣ 𝜔𝜑,𝑘
,𝑗∣𝑖)
(𝑗∣𝑖) 𝜔𝑘∣𝑘−1
= Λ(𝜑) +
∑
(𝑖) 𝐽𝑘∣𝑘−1
𝑗=1
−𝛼
e ∑
𝚥¯1:∣𝜑∣
(𝑗∣𝑖) (𝑖) 𝐿𝚥¯1:∣𝜑∣ (s𝑘 )
(𝑗∣𝑖) 𝜔𝑘∣𝑘−1
e
−𝛼
(𝑗∣𝑖) (𝑖) 𝐿𝚥¯1:∣𝜑∣ (s𝑘 )
,
B. Particle representation To implement the Gaussian mixture formulation in Section III-A, we sample 𝑁𝑘−1 particles from the Markov transition density for the particle representation of 𝑀𝑘 (s), i.e. for 𝑖 = (𝑖) (𝑖) 1, . . . , 𝑁𝑘−1 , sample ˜s𝑘 ∼ 𝜋𝑘∣𝑘−1 (s ∣ s𝑘−1 ). The weights of the Dirac mixture can be updated then as follows (𝑖)
(29)
𝐿𝑍 (˜s ) (𝑖) (𝑖) 𝜛 ˜ 𝑘 = ∑𝑁 𝑘 𝑘 (𝑖) 𝜛𝑘−1 , 𝑘−1 s𝑘 ) 𝑖=1 𝐿𝑍𝑘 (˜
where (𝑗∣𝑖)
(𝑖)
∣𝜑∣ ∏
(¯ 𝚤,¯ 𝚥1:¯𝚤 ,𝑗∣𝑖)
𝜐𝝃 ¯𝚤 𝑞1
(𝑖)
(𝑖)
(𝑖) (z¯𝚤 ∣ s𝑘 ), where 𝐿𝑍𝑘 (˜s𝑘 ) can be determined from the updated intensity ¯ 𝚤=1 for the process (30) (𝑖) (𝑖) 𝐽𝑘∣𝑘−1 𝐽𝑘∣𝑘−1 ∑ ∑ ∑ (¯𝚥1:∣𝜑∣ ,𝑗∣𝑖) ∑ in which we have (𝑖) (𝑗∣𝑖) 𝜔𝑘∣𝑘−1 + 𝜔𝜑,𝑘 , 𝐿𝑍𝑘 (˜s𝑘 ) = e−𝛼 (¯ 𝚤,¯ 𝚥1:¯𝚤 ,𝑗∣𝑖) (𝑖) (𝑖) (¯ 𝚥1:¯𝚤 ∣𝑖,𝑗) (¯ 𝚥1:¯𝚤 ∣𝑖,𝑗) 𝑞1 (z¯𝚤 ∣ s𝑘 ) = 𝒩 (z¯𝚤 ∣ s𝑘 ; b1 , S1 ), 𝜑∈𝝅 𝑗=1 𝚥¯1:∣𝜑∣ 𝑗=1 (¯ 𝚤,¯ 𝚥 ,𝑗∣𝑖) (𝑖) (𝑖) ˆ (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ∣𝑖,𝑗) (36) ˜ 𝑘 1:¯𝚤 (z¯𝚤 ∣ s𝑘 ) = 𝒩 (z¯𝚤 ∣ s𝑘 ; H m , S2 1:¯𝚤 ), 𝑞2 1:¯𝚤 (31) (¯ 𝚥1:∣𝜑∣ ,𝑗∣𝑖) is as defined in (29). A resampling such that 𝜔𝜑,𝑘 ( )−1 (¯ 𝚥¯𝚤 ∣𝑖,𝑗) (𝑗∣𝑖) (¯ 𝚥¯𝚤 ∣𝑖,𝑗) procedure is then applied, which makes 𝑁𝑘 copies of particles P𝑘∣𝑘−1 = HP𝝃,12 , and, denoting A (𝑖) (𝑖) 𝑁𝑘−1 {𝜛 ˜ 𝑘 , ˜s𝑘 }𝑖=1 and eliminates low-weighted particles to give ( ) (¯ 𝚥 ∣𝑖,𝑗) (¯ 𝚥 ∣𝑖,𝑗) (¯ 𝚥 ∣𝑖,𝑗) (𝑗) (𝑖) (𝑖) 𝑘 = Hm𝝃 ¯𝚤 + A(¯𝚥¯𝚤 ∣𝑖,𝑗) 𝜼 𝑘 1:¯𝚤 − m𝑘∣𝑘−1 , b1 1:¯𝚤 {𝜛𝑘 , s𝑘 }𝑁 𝑖=1 . ( )𝑇 (¯ 𝚥 ∣𝑖,𝑗) (¯ 𝚥 ,𝑗∣𝑖) IV. S IMULATED E XAMPLE ˜ (¯𝚥¯𝚤 ∣𝑖,𝑗) , A(¯𝚥¯𝚤 ∣𝑖,𝑗) + R = A(¯𝚥¯𝚤 ∣𝑖,𝑗) Σ𝑘 1:¯𝚤 S1 1:¯𝚤 The methodology and implementation detailed in Sections (¯ 𝚥 ∣𝑖,𝑗) ˆP ˜ (¯𝚥1:¯𝚤 ,𝑗∣𝑖) H ˆ 𝑇 + R2 . =H S2 1:¯𝚤 𝑘 II & III apply to extended targets with any structural shape (32) in general. In this section we present a multiple extended target tracking example in which the extents are, specifically, Furthermore, for 1 ≤ ¯𝚤 ≤ ∣𝜑∣ ( ) elliptical in shape, that the proposed implementation has (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ∣𝑖,𝑗) ˜ (¯𝚥1:¯𝚤 ,𝑗∣𝑖) z¯𝚤 − b(¯𝚥1:¯𝚤 ∣𝑖,𝑗) , ˜ 𝑘 1:¯𝚤 = 𝜼 𝑘 1:¯𝚤 +K m 1 𝑘 been tested on, using simulated data. The generated target ) ( (33) trajectories are shown in Fig. 1 along with the elliptical shapes, (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) ˜ 1:¯𝚤 ˜ 1:¯𝚤 = I−K A(¯𝚥¯𝚤 ∣𝑖,𝑗) Σ𝑘 1:¯𝚤 , P 𝑘 𝑘 parameterized by the major and minor axes, which gradually ( )−1 ( ) reduce in size over 100 iterations, for each target. The target 𝑇 (¯ 𝚥 ∣𝑖,𝑗) ˜ (¯𝚥1:¯𝚤 ,𝑗∣𝑖) = Σ(¯𝚥1:¯𝚤 ,𝑗∣𝑖) A(¯𝚥¯𝚤 ∣𝑖,𝑗) S1 1:¯𝚤 and where K 𝑘 𝑘 trajectories are also shown plotted against time, along with the { (𝑗) simulated measurements, in Fig. 2. for ¯𝚤 = 1, m𝑘∣𝑘−1 (¯ 𝚥 ∣𝑖,𝑗) In the following subsection we first provide precise details 𝜼 𝑘 1:¯𝚤 = (¯ 𝚥 ,𝑗∣𝑖) m𝑘 1:¯𝚤−1 for 1 < ¯𝚤 ≤ ∣𝜑∣, of the simulation set-up before presenting the simulation { (𝑗) (34) results in Section IV-B. for ¯𝚤 = 1, P𝑘∣𝑘−1 (¯ 𝚥 ,𝑗∣𝑖) Σ𝑘 1:¯𝚤 = (¯ 𝚥1:¯𝚤−1 ,𝑗∣𝑖) P𝑘 for 1 < ¯𝚤 ≤ ∣𝜑∣. A. Set-up
𝐿𝚥¯1:∣𝜑∣ (s𝑘 ) =
(¯ 𝚥 )
(35)
(¯ 𝚤,¯ 𝚥1:¯𝚤 ,𝑗∣𝑖)
(z¯𝚤 ∣ s𝑘 ) 𝑞2
(𝑖)
(𝑖)
The resulting mixture in (27) has 𝐽𝑘∣𝑘−1 + ∣𝝅∣ × 𝐽𝑘∣𝑘−1 × (𝑗∣𝑖) 𝐽𝝃
∣𝜑∣ × components and left unchecked the size of the parent process mixture would grow at an approximately exponential rate with every time step. Many of these come from low-likelihood measurement associations and contribute little to the updated intensity in (27). There are a couple of Gaussian mixture reduction procedures we employ to then eliminate and merge such low-weight components. The first is a measurement gating procedure as described in [27] which we apply to determine how many boundary points 𝝃 are related to measurements z ∈ 𝜑 for each 𝜑 ∈ 𝝅, hence reducing the number of components in the mixture within the product given in (25). The other procedure is the pruning and merging one presented in [4]. Applying both procedures help maintain a more manageable approximation of the kinematic state intensity.
The target state X𝑘 consists of the kinematic state variable ]𝑇 [ x𝑘 = 𝑥𝑘,1 𝑥𝑘,2 𝑥𝑘,3 𝑥𝑘,4 , (37) ]𝑇 [ is the position vector (centre of the where 𝑥𝑘,1 𝑥𝑘,3 ]𝑇 [ ellipse) and 𝑥𝑘,2 𝑥𝑘,4 is the velocity vector at time-step 𝑘, and shape parametric variable ]𝑇 [ (38) s𝑘 = 𝑠𝑘,1 𝑠𝑘,2 𝑠𝑘,3 𝑠𝑘,4 , where 𝑠𝑘,1 is the magnitude of the major axis and 𝑠𝑘,3 is the magnitude of the minor axis of the ellipse, while 𝑠𝑘,2 & 𝑠𝑘,4 are the rates of change in the major and minor axes’ magnitudes respectively. Targets follow the linear Gaussian dynamic model (17) with ] [ F𝑘 02 , Q = 𝜎x2 Γ𝑘 Γ𝑇𝑘 , (39) F= 02 F𝑘
1115
500
400
400
400
300
200
100
y coordinate (in m)
500
y coordinate (in m)
y coordinate (in m)
500
300
200
100
0 0
100
200 300 x coordinate (in m)
400
200
100
0 0
500
300
100
(a) Trajectory 1
200 300 x coordinate (in m)
400
0 0
500
100
(b) Trajectory 2
200 300 x coordinate (in m)
400
500
(c) Trajectory 3
Figure 1: The multiple target trajectories (line) and size varying elliptical extents. Starting positions are indicated by the blue ’filled diamonds’ at times (a) 𝑘 = 1, (b) 𝑘 = 26, (c) 𝑘 = 51 and terminal positions are indicated by the blue ’filled squares’ at times (a) 𝑘 = 100, (b) 𝑘 = 125, (c) 𝑘 = 150. 500
x (in m)
400 300 200 100 0 0
50
100
150
500
y (in m)
400 300 200 100 0 0
50
100
150
Time−step
Figure 2: Measurements (green ‘×’) and target trajectories (line).
such that F𝑘 =
[
]
1 𝜏 , 0 1
[ Γ𝑘 =
𝜏
/ 2 0
2
𝜏 0
0/ 𝜏2 2
0 𝜏
]𝑇 ,
(40)
where 02 denotes the √ 2 × 2 zero matrix, 𝜏 = 1s is the sampling period and 𝜎x = 2 (m/s2 ) is the standard deviation of the process noise. The probability of survival is 𝑝𝑆 = 0.9. ]𝑇 [ ∈ 𝑍𝑘 reThe point measurements z = 𝑧1 𝑧2 late to points 𝝃 on the ellipse according to the likelihood 𝒩 (z ; √ H𝝃, R1 ) in (21) with H = I2 and R1 = 𝜎12 I2 where 𝜎1 = 5m. They relate to the target state x according to the ˜ R2 ) in (21) with likelihood 𝒩 (z ; Hx, [ ] 1 0 0 0 ˆ H= , (41) 0 0 1 0 √ and R2 = 𝜎22 I2 where 𝜎2 = 50m. The average number of observed points on the ellipse is 𝛼 = 5. The clutter term in the PHD update is given by (12) with 𝜆1 (u𝜑 ) = 𝜅1 𝑈 (u𝜑 ) and
𝜆2 (z ∣ u𝜑 ) = 𝜅2 𝒩 (z ; u𝜑 , R𝜅 ) where 𝑈 (u𝜑 ) is the uniform density over the observation region, 𝜅1 = 20 is the average number of false measurement clusters over the region, 𝜅2 = 1 is the average number of measurements per cluster and R𝜅 = 5 × I2 . (𝑖) The filter is initialised with the birth intensity 𝛾𝑘 (x ∣ s𝑘 ) at time-step 𝑘 = 1. Birth targets are measurement driven, a technique applied for the PHD filter in [28]. That is, in (18), for measurements in each cluster subset 𝜑 ∈ 𝝅 and (𝑖) (𝑖) 𝑗 = 1, . . . , 𝐽𝛾,𝑘 where 𝐽𝛾,𝑘 = ∣𝝅∣, we have [
(𝑗∣𝑖) m𝛾,𝑘
1 ∑ 𝑧1 = ∣𝜑∣ z∈𝜑 (𝑗∣𝑖)
0
1 ∑ 𝑧2 ∣𝜑∣ z∈𝜑
]𝑇 0 (𝑗∣𝑖)
,
(42)
with covariance P𝛾,𝑘 = 𝜎22 I4 and weight 𝜔𝛾,𝑘 = 0.1, for each particle 𝑖. The sampling distribution for the particle representation of a target’s shape is a linear truncated Gaussian. That is
1116
600
400
400
200
x (m)
600
400 x (m)
x (m)
600
200
200
0
0
0
−200
−200
−200
50
100
25
75
125
50 600
400
400
400
200 0
y (m)
600
y (m)
y (m)
0 600
200
50 Time−step
100
150
100 Time−step
150
200
0
0
100
0
25
75 Time−step
125
50
Figure 3: Position estimates (blue ‘circles’), uncertainty (grey shaded area) and target trajectories (line).
20 10
30 20
0
50
0 25
100
5
50 Time−step
100
100
150
100 Time−step
150
20
15 10
15 10 5
5
0
20
0 50
125
Minor axis (m)
Minor axis (m)
10
0
75
20
15
30
10
10
20 Minor axis (m)
Major axis (m)
30
0
40
40 Major axis (m)
Major axis (m)
40
0 25
75 Time−step
0 50
125
Figure 4: Shape estimates (blue ‘circles’), uncertainty (grey shaded area) and true shape variation (line). (𝑖)
(𝑖)
for persistent targets, we sample ˜s𝑘 ∼ 𝑓𝑘∣𝑘−1 (s ∣ s𝑘−1 ) = (𝑖) ˜ such that ˜s(𝑖) lies within the interval (a, b) 𝒩 (s ; Fs𝑘−1 , Q) 𝑘 where [ ]𝑇 [ ]𝑇 a = 19 −0.2 14 −0.1 , b = 32 0 21 0 , (43) F is as defined in (39) and ˜ = Q
2 2 2 2 Σs = diag([𝜎s,1 , 𝜎s,2 , 𝜎s,3 , 𝜎s,4 ]), (44) √ √ √ √ for 𝜎s,1 = 2m, 𝜎s,2 = 0.2m, 𝜎s,3 = 1m, 𝜎s,4 = 0.1m (𝑖) ˜𝑘 = and Γ𝑘 as defined in (40). For new targets, we sample c ]𝑇 [ (𝑖) (𝑖) (𝑖) ∼ 𝒩 (c ; bnew , Σnew ) such that c𝑘 lies within 𝑐𝑘,1 𝑐𝑘,2 [ ]𝑇 the interval (anew , bnew ), where anew = 19 14 , bnew = [ ]𝑇 (𝑖) 2 2 31 21 & Σnew = diag([𝜎s,1 , 𝜎s,3 ]), so that we have ˜s𝑘 = [ ]𝑇 (𝑖) (𝑖) 𝑐𝑘,1 0 𝑐𝑘,2 0 . Finally, the daughter process for each particle 𝑖 and kinematic state 𝑗 has the conditional Gaussian mixture form given (𝑗∣𝑖) (ℓ) = 𝛼, 𝜐𝝃 = 1, in (22) with 𝐽𝝃 (ℓ∣𝑖,𝑗)
(ℓ∣𝑖,𝑗)
P𝝃,12
ˆ = Hm P𝝃,11 = R1 , 𝑘∣𝑘−1 + t𝑘∣𝑘−1 , ⎡√ (𝑗∣𝑖) P𝑘∣𝑘−1 (1, 1) 0 0 √ = 𝛽𝜎1 ⎣ (𝑗∣𝑖) 0 0 P𝑘∣𝑘−1 (3, 3) (𝑗∣𝑖)
(ℓ∣𝑖,𝑗)
(ℓ∣𝑖,𝑗) t𝑘∣𝑘−1
=
, where
] [ (𝑖) (𝑗∣𝑖) (𝑖) (𝑗∣𝑖) 𝑠˜𝑘,1 cos(𝜃ℓ ) cos(𝜙𝑘∣𝑘−1 ) + 𝑠˜𝑘,3 sin(𝜃ℓ ) sin(𝜙𝑘∣𝑘−1 ) (𝑖)
(𝑗∣𝑖)
(ℓ∣𝑖,𝑗)
⎤ 0 ⎦ 0 (45)
(𝑖)
(𝑗∣𝑖)
𝑠˜𝑘,1 cos(𝜃ℓ ) sin(𝜙𝑘∣𝑘−1 ) − 𝑠˜𝑘,3 sin(𝜃ℓ ) cos(𝜙𝑘∣𝑘−1 ) (46) (𝑗∣𝑖)
Γ𝑘 Σs Γ𝑇𝑘 ,
m𝝃
(𝑗∣𝑖)
for all ℓ = 1, . . . , 𝐽𝝃
(𝑗∣𝑖)
(𝑗∣𝑖)
given 𝜃ℓ = 2𝜋ℓ/𝛼, 𝜙𝑘∣𝑘−1 = arctan(𝑚𝑘∣𝑘−1,4 /𝑚𝑘∣𝑘−1,2 ), (𝑗∣𝑖)
(𝑗∣𝑖)
denoting 𝑚𝑘∣𝑘−1,𝑛 as the 𝑛th element of mean vector m𝑘∣𝑘−1 , (𝑗∣𝑖)
and P𝑘∣𝑘−1 (𝑛, 𝑚) as the (𝑛, 𝑚) element of the covariance (𝑗∣𝑖)
matrix P𝑘∣𝑘−1 . Finally, we set 𝛽 = −0.4 where ∣𝛽∣ ≤ 1 relates to the correlation between parent and daughter.
B. Results The estimation results shown in Fig. 3 & 4 are from a simulation run with 500 particles for each target. Tracks are maintained by labelling each corresponding particle set. The state and error estimates are obtained by applying a similar clustering technique as demonstrated ∑ in [28] whereby the weights and means within the sum 𝜑∈𝝅 in (27) are (𝑛∣𝑖) (𝑛∣𝑖) re-indexed as 𝜔𝜑,𝑘 and m𝜑,𝑘 for 𝑛 = 1, . . . , 𝑁𝜑 where (𝑖) (𝑗∣𝑖) 𝑁𝜑 = 𝐽𝑘∣𝑘−1 × ∣𝜑∣ × 𝐽𝝃 . Then, for each cluster subset 𝜑, ∑𝑁𝑘−1 ∑𝑁𝜑 (𝑛∣𝑖) we compute 𝑊𝜑,𝑘 = 𝑖=1 𝑛=1 𝜔𝜑,𝑘 , and if 𝑊𝜑,𝑘 ≥ 𝑇 , for some threshold 𝑇 , we calculate the position and shape
1117
estimates respectively as 𝑁𝑘−1 𝑁𝜑
ˆ 𝜑,𝑘 = m
∑ ∑ 𝑖=1 𝑛=1
(𝑛∣𝑖) 𝜔𝜑,𝑘
(𝑛∣𝑖) m𝜑,𝑘 ,
𝑁𝑘−1 𝑁𝜑
ˆs𝜑,𝑘 =
∑ ∑ 𝑖=1 𝑛=1
(𝑛∣𝑖)
(𝑖)
𝜔𝜑,𝑘 ˜s𝑘 , (47)
and the corresponding errors are calculated as ˆ 𝜑,𝑘 = P
𝑁𝑘−1 𝑁𝜑
∑ ∑
(𝑛∣𝑖)
𝜔𝜑,𝑘
𝑖=1 𝑛=1 (𝑛∣𝑖) + (m𝜑,𝑘 𝑁𝑘−1 𝑁𝜑
ˆserr 𝜑,𝑘 =
∑ ∑ 𝑖=1 𝑛=1
(
(𝑛∣𝑖)
P𝜑,𝑘
) (𝑛∣𝑖) ˆ 𝜑,𝑘 ) (m𝜑,𝑘 − m ˆ 𝜑,𝑘 )𝑇 , − m (𝑛∣𝑖)
(𝑖)
(48)
(𝑖)
𝜔𝜑,𝑘 (˜s𝑘 − ˆs𝜑,𝑘 )(˜s𝑘 − ˆs𝜑,𝑘 )𝑇 ,
From the early simulation trials, it can be seen in Fig. 3 & 4 that the extracted estimates for both position and shape parameters are accurate, even when the uncertainty is high. The exception being when the targets approach the perimeter of the observation region towards the end of their existence, in which case the extents are only partly visible. V. C ONCLUSION The paper presented a hierarchical point process approach to tracking an extended target which gives rise to a structured set of measurements per time-step in the form of a geometric shape. The implementation of the corresponding PHD filter for the compound state, consisting of the target’s shape parametric and kinematic state, was based on a particle representation and a Gaussian mixture formulation respectively. The novelty of the approach is that it’s non-shape specific, and is applicable for structured sets of target generated measurements which take the form of any geometric shape that can be parameterised in a two-dimensional observation space. Demonstrated on a multiple extended target example where the extents are elliptical in shape and variable in size, early simulation results are promising. VI. ACKNOWLEDGEMENTS Anthony Swain is funded by an EPSRC Industrial CASE Award PhD Studentship with SELEX Galileo Ltd. Daniel Clark is a Royal Academy of Engineering/EPSRC Research Fellow. The authors would like to thank Neil Cade (SELEX Galileo Ltd) for his advice in contribution to this paper. R EFERENCES [1] R. P. S. Mahler, “Multitarget Bayes filtering via first-order multitarget moments,” IEEE Trans. AES, vol. 39, no. 4, pp. 1152–1178, 2003. [2] B. N. Vo, S. Singh, and A. Doucet, “Sequential monte carlo methods for multitarget filtering with random finite sets,” IEEE Trans. AES, vol. 41, no. 4, pp. 1224–1245, 2005. [3] D. E. Clark, B. T. Vo, and B. N. Vo, “Gaussian particle implementations of probability hypothesis density filters,” in Aerospace Conference, IEEE, 2007, pp. 1–11. [4] B. N. Vo and W. K. Ma, “The Gaussian mixture probability hypothesis density filter,” IEEE Trans. Signal Processing, vol. 54, no. 11, pp. 4091– 4104, 2006.
[5] D. E. Clark, B. N. Vo, and S. Godsill, “Gaussian mixture implementations of probability hypothesis density filters for non-linear dynamical models,” in Target Tracking and Data Fusion: Algorithms and Applications, IET Seminar on, 2008, pp. 21–28. [6] J. Houssineau and D. Laneuville, “PHD filter with diffuse partial prior on the birth process with applications to GM-PHD filter,” in 13th International Conference on Information Fusion, Edinburgh, UK, July 2010. [7] D. E. CLark, B. Ristic, B. N. Vo, and B. T. Vo, “Bayesian multi-object filtering with amplitude feature likelihood for unknown object SNR,” IEEE Trans. on Signal Processing, vol. 58, no. 1, pp. 26–37, 2010. [8] C. D. Haworth, Y. Saint-Pern, D. E. Clark, E.Trucco, and Y. Petillot, “Detection and tracking of multiple metallic objects in millimetre-wave images,” International Journal of Computer Vision, vol. 71, no. 2, pp. 183–196, 2007. [9] D. E. Clark, A. T. Cemgil, P. Peeling, and S. Godsill, “Multi-object tracking in sinusoidal components in audio with the Gaussian mixture probability hypothesis density filter,” in Applications of Signal Processing to Audio and Acoustics, IEEE Workshop, 2007, pp. 339–342. [10] R. P. S. Mahler, “PHD filters for nonstandard targets, I: Extended targets,” in 12th International Conference on Information Fusion, Seattle, WA, USA, July 6–9 2009, pp. 915–921. [11] K. Granstr¨ om, C. Lundquist, and U. Orguner, “A Gaussian mixture PHD filter for extended target tracking,” in 13th International Conference on Information Fusion, Edinburgh, UK, July 2010. [12] D. Daley and D. Vere-Jones, An introduction to the theory of point processes. Springer, 1988. [13] K. Granstr¨ om, C. Lundquist, and U. Orunger, “Tracking rectangular and elliptical extended targets using laser measurements,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, July 5–8 2011, pp. 592–599. [14] ——, “Estimating the shape of targets with a PHD filter,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, July 5–8 2011, pp. 49–56. [15] J. Mullane, B. N. Vo, M. Adams, and B. T. Vo, “A random finite set approach to Bayesian SLAM,” IEEE Trans. Robotics, vol. PP, no. 99, pp. 1–15, 2011. [16] M. Baum and U. D. Hanebeck, “Shape tracking of extended objects and group targets with star-convex RHMs,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, July 5–8 2011, pp. 338– 345. [17] N. Petrov, L. Mihayova, A. Gning, and D. Angelova, “A novel sequential Monte Carlo approach for extended object tracking based on border parameterisation,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, July 5–8 2011, pp. 306–313. [18] A. Swain and D. E. Clark, “Extended object filtering using spatial independent cluster processes,” in 13th International Conference on Information Fusion, Edinburgh, UK, July 2010. [19] ——, “First-moment filters for spatial independent cluster processes,” in Signal Processing, sensor fusion and target recognition XIX, SPIE Defense, Security and Sensing, I. Kadar, Ed., vol. 7697, Orlando, FL, USA, 2010. [20] ——, “The single-group PHD filter: an analytic solution,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, 2011, pp. 41–48. [21] C. S. Lee, D. E. Clark, and J. Salvi, “SLAM with single cluster PHD filters,” submitted to ICRA, 2012. [22] B. Ristic and D. E. Clark, “Particle filter for joint estimation of multiobject dynamic state and multi-sensor bias,” in IEEE International Conference on Acoustics, Speech and Signal Processing, March 25–30 2012. [23] D. E. Clark, “Bayesian filtering for multi-object systems with independently generated observations,” Arxiv preprint arXiv: 1202.0949, 2012. [24] ——, “Faa di Bruno’s formula for Gateaux differentials,” Arxiv preprint arXiv: 1202.0264, 2012. [25] D. E. Clark and R. P. S. Mahler, “Generalized PHD filters via a general chain rule,” submitted to Fusion, 2012. [26] R. P. S. Mahler, Statistical multisource-multitarget information fusion. Norwood, MA: Artech House, 2007. [27] Y. Bar-Shalom and T. E. Fortmann, Tracking and data association. San Diego: Academic Press, 1988. [28] B. Ristic, D. E. Clark, and B. N. Vo, “Improved SMC implementation of the PHD filter,” in 13th International Conference on Information Fusion, Edinburgh, UK, July 2010.
1118
Abstract—In extended target tracking, targets potentially produce more than one measurement per time step. In recent random finite set (RFS) approaches, the set of measurements obtained from an extended target is modelled as a point process. In this paper, we expand on the RFS approach to extended target tracking by considering a hierarchical point process representation of multiple extended target, more specifically a Poisson cluster process. This allows us to impose a geometric shape, in particular an ellipse, on each extended target. The set of target states, which are characterised by the kinematic variables and the shape parameters, represents the higher level (parent) process and the set of points on the boundary, from which measurements are generated, represents the lower level (daughter) process. We describe the PHD filter for multiple extended targets, whose extents vary in size, that estimates the shape parameters of the targets jointly with their positions and velocities. The main contribution of this paper is the practical implementation we propose, based on a particle-system representation for the targets’ shape and a Gaussian mixture formulation for the kinematic state per particle. The method is demonstrated on simulated data for multiple elliptical shaped extended targets.
Keywords: random finite sets, PHD filters, cluster processes, Gaussian mixture. I. I NTRODUCTION The random finite set (RFS) approach to multi-target tracking has proven to be a useful method that allows the problem of estimating multiple dynamic targets in the presence of false measurements and detection uncertainty to be cast in a Bayesian filtering framework [1]. By treating the collection of individual targets as a set-valued state and the collection of observation measurements as a set-valued observation, this theoretically optimal approach is an elegant generalisation of the single-target Bayes filter. However the propagation of the posterior probability distribution in the multi-target Bayes filter is often computationally unattainable due to the high dimensionality of the target RFS. To overcome this intractability, the Probability Hypothesis Density (PHD) filter was introduced [1], which propagates the first-order moment, otherwise known as the intensity. The PHD filter is typically implemented using a particle representation applying sequential Monte Carlo (SMC) techniques [2], [3] or with a Gaussian mixture formulation [4]– [6] and has applications in radar [7], computer vision [8] and acoustics [9], to name but a few. In most applications, including the PHD filter, it is assumed that each target produces at most one measurement per time-
step. In extended target tracking, targets potentially produce more than one measurement per time-step. Multiple measurements per target per time-step gives rise to the possibility of estimating the target’s shape and size in addition to its position and velocity. An extension of the PHD filter to handle extended targets was presented in [10], recently implemented using a Gaussian mixture formulation [11], which involved representing the measurements generated from targets as a spatial point process [12], specifically a Poisson point process. That is, at each time-step, a Poisson distributed number of measurements are generated, spatially distributed around the target. Such a measurement model implies the target generated measurements resemble a cluster of points, rather than a geometrically structured arrangement. A number of methods based on the RFS approach have been proposed for estimating the size and shape of a target [13], [14]. In [13], a direct application of the Gaussian mixture PHD filter for extended targets [11] is presented in which a method is proposed for calculating a likelihood function that relates a set of measurements, potentially, to a target whose structure resembles that of a rectangle or an ellipse. In [14], a partitioned state representation of an extended target is considered, which consists of linear (position & velocity) and non-linear (heading & target shape parameters) components. The PHD filter is applied for jointly estimating this target state and a set of measurement generating points defined on the boundary of each target’s geometrically structured shape (rectangle), and implemented using a Rao-Blackwellized particle filter as demonstrated for the Simulation Localisation and Mapping (SLAM) problem in [15]. Other proposed approaches for extended target size and shape estimation include a Random Hypersurface Model method [16] that assumes varying measurement sources which lie on scaled versions of shape boundaries, and a novel SMC approach [17] that considers target generated measurements bounded by a circular region which is estimated jointly with the target’s kinematic state. In this paper we consider an alternative approach whereby multiple extended targets are modelled as a hierarchical point process as proposed in [18]. Hierarchical point processes, known as cluster point processes, have been previously exploited in the group target tracking problem [19], [20], the SLAM problem [21], and more recently in sensor registration
1111
[22]. In this paper, we specifically consider a Poisson cluster point process representation of multiple extended targets, which allows us to impose a geometric shaped extent on each target, where the centres of each target’s extent represent the higher level (parent) process on which a dynamic model can be specified, and the set of points on the boundary of each target’s extent represents the lower level (daughter) process from which the measurements are generated. In the PHD filter for this approach to extended target tracking, the kinematic states and the shape parametric variables are estimated jointly via the first-order moment. The remainder of the paper is organised as follows: Sections II and III present the proposed methodology and implementation. The PHD recursion in Section II can easily be derived using a general chain rule and Gateaux differentials [23]–[25]. Section IV details a multiple extended target tracking example, whose extents are elliptical in shape and vary in size over time. The estimation results from the PHD filter implementation for this example using simulated data are then discussed, followed by concluding remarks in Section V. II. E XTENDED TARGET TRACKING USING CLUSTER PROCESSES
In this work, we model multiple extended targets, whose extents have a geometrically shaped structure and vary in size, as a doubly-stochastic point process. In particular, we suppose the multiple extended targets have a Poisson cluster process representation, defined by 𝕏𝑘 = {(X𝑘,1 , Ξ𝑘,1 ), . . . , (X𝑘,𝑛 , Ξ𝑘,𝑛 )} ]𝑇 [ where for 𝑖 = 1, . . . , 𝑛 the state X𝑘,𝑖 = x𝑘,𝑖 s𝑘,𝑖 consists of the random vector x𝑘,𝑖 defining the kinematic state and the random vector s𝑘,𝑖 defining the shape parameter variables for target 𝑖. The RFS Ξ𝑘,𝑖 = {𝝃𝑘,1 , . . . , 𝝃 𝑘,𝑛𝑖 } represents the points on the boundary of the extent for target 𝑖. The parent process is represented by the RFS of target states X𝑘,𝑖 with which there corresponds a daughter process, represented by the RFS Ξ𝑘,𝑖 conditioned on X𝑘,𝑖 , for 𝑖 = 1, . . . , 𝑛. In addition, we consider a measurement model, defined by the RFS 𝑍𝑘 , that takes into account detection uncertainty and false measurements (clutter) and is formed by the union of target generated measurements Θ𝑘 (𝕏𝑘 ) and clutter 𝐾𝑘 , i.e. 𝑍𝑘 = 𝐾𝑘 ∪ Θ𝑘 (𝕏𝑘 ).
(1)
In recent RFS approaches [10], [11], [13], [14], the set of false measurements 𝐾𝑘 is modelled as a Poisson point process, the probability density for which is ( ∫ ) ∏ 𝜆(z), (2) 𝑝𝑘 (𝐾𝑘 ) = exp − 𝜆(z) dz z∈𝐾𝑘
where 𝜆(z) is the intensity of the clutter process. An integral part of extended target tracking with the PHD filter is the partitioning of the measurement set. A partition 𝜋 is defined as a division of the RFS 𝑍𝑘 into subsets, denoted ∪ as 𝜑, such that 𝜑∈𝜋 𝜑 = 𝑍𝑘 . We denote the set of all possible partitions of 𝑍𝑘 as Π𝑍𝑘 . To assist the partitioning operation, instead of the false measurement model given by the
probability density in (2), we model 𝐾𝑘 as a Poisson cluster point process, the probability density for which is )∏ ( ∫ 𝜆1 (u𝜑𝜅 ) 𝑝𝑘 (𝐾𝑘 ) = exp − 𝜆1 (u𝜑𝜅 ) du𝜑𝜅 𝜑
)𝜅 ∏ ( ∫ 𝜆2 (z ∣ u𝜑𝜅 ), × exp − 𝜆2 (z ∣ u𝜑𝜅 ) dz z∈𝜑𝜅
(3) where 𝜆1 (u𝜑𝜅 ) is the intensity of the parent process whose state u𝜑𝜅 is the centroid of the measurements in subset 𝜑𝜅 and 𝜆2 (z ∣ u𝜑𝜅 ) is the intensity of the daughter process. With the clutter process modelled in this way, the assumption is that the measurements appear in clusters. Let 𝑝𝑘 (𝕏𝑘 ∣ 𝑍1:𝑘 ) be the posterior probability distribution of the Poisson cluster process representation of the multiextended target state 𝕏𝑘 , given a time sequence of measurement sets denoted by 𝑍1:𝑘 = 𝑍1 , . . . 𝑍𝑘 . The Bayesian filter recursion for estimating the posterior 𝑝𝑘 (𝕏𝑘 ∣ 𝑍1:𝑘 ) involves computing set integrals [26] due to the RFS of target states X𝑘,𝑖 representing the parent process as well as the RFS definition of Ξ𝑘,𝑖 for 𝑖 = 1, . . . , 𝑛. The set integrals in this recursion involve multiple infinite sums over all possible posterior distributions at different cardinalities in the parent and daughter processes which makes implementing the Bayes filter computationally intractable for variable numbers of targets X and corresponding boundary points 𝝃. Instead we propagate the first-order moment approximation of the posterior 𝑝𝑘 (𝕏𝑘 ∣ 𝑍1:𝑘 ), an approach proposed for multi-target tracking under the name of the Probability Hypothesis Density (PHD) filter in [1]. The PHD filter for multiple extended targets modelled as a Poisson cluster process, with geometric shaped target extents, jointly propagates the kinematic state and shape parametric via the first-order moment, or intensity. Throughout the PHD filter recursion, the propagation of the kinematic state will depend on the shape parametric since the RFS Ξ of boundary points, from which measurements are generated, updating the kinematic state, are partly determined by the shape parameters. Therefore the joint prior PHD can be factorized as follows 𝑀𝑘−1 (s′ , x′ ) = 𝑀𝑘−1 (s′ ) × 𝑀𝑘−1 (x′ ∣ s′ ),
(4)
where 𝑀𝑘−1 (s′ ) = 𝑀𝑘−1 (s𝑘−1 ) is the intensity of the shape parametric and 𝑀𝑘−1 (x′ ∣ s′ ) = 𝑀𝑘−1 (x𝑘−1 ∣ s𝑘−1 ) is the intensity of the kinematic state. Now suppose that the targets’ positions and shapes evolve independently of one another so that dynamics can be factorized as 𝑓𝑘∣𝑘−1 (X ∣ X′ ) = 𝑓𝑘∣𝑘−1 (s ∣ s′ )𝑓𝑘∣𝑘−1 (x ∣ x′ ), where 𝑓𝑘∣𝑘−1 (s ∣ s′ ) is the Markov transition density for the shape parametric and 𝑓𝑘∣𝑘−1 (x ∣ x′ ) is the Markov transition density for the kinematic state. This is representative of a scenario in which the targets’ shape parametrics do not include the orientations of the shapes, which are instead determined by the targets’ velocities, that are incorporated into the kinematic state. The PHD prediction equation is then
1112
𝑀𝑘∣𝑘−1 (s, x) = 𝑀𝑘∣𝑘−1 (s) × 𝑀𝑘∣𝑘−1 (x ∣ s),
(5)
where the predicted intensity of the shape parametric is ∫ 𝑀𝑘∣𝑘−1 (s) = 𝑀𝑘−1 (s′ ) 𝑓𝑘∣𝑘−1 (s ∣ s′ ) ds′ ,
very computationally demanding. If a single partition, denoted as 𝝅, is known the joint pseudo-likelihood reduces to (6) 𝐿𝑍𝑘 (s, x) = e−𝜇[1 ∣ s,x] +
where the predicted intensity of the kinematic state is
𝜑∈𝝅
𝑀𝑘∣𝑘−1 (x ∣ s) = 𝛾𝑘 (x ∣ s)+ ∫ 𝑝𝑆 (x′ ) 𝑓𝑘∣𝑘−1 (x ∣ x′ ) 𝑀𝑘−1 (x′ ∣ s) dx′ , (7) and 𝛾𝑘 (x ∣ s) is the intensity for birth targets. The probability of survival 𝑝𝑆 (x′ ) is also assumed to be shape independent. Given a new set of measurements 𝑍𝑘 and under the assumption that the daughter process can be approximated with a Poisson point process, the PHD update equation is 𝑀𝑘 (s, x) = ∫∫
𝐿𝑍𝑘 (s, x) 𝑀𝑘∣𝑘−1 (s) 𝑀𝑘∣𝑘−1 (x ∣ s) , 𝐿𝑍𝑘 (s, x) 𝑀𝑘∣𝑘−1 (s) 𝑀𝑘∣𝑘−1 (x ∣ s) ds dx (8)
where the joint pseudo-likelihood is ∑ ∑ 𝐿𝑍𝑘 (s, x) = e−𝜇[1 ∣ s,x] + 𝜔𝜋 𝜋∈Π𝑍𝑘
𝜑∈𝜋
Υ𝜑 (s, x) , Λ(𝜑) + 𝑀𝑘∣𝑘−1 [Υ𝜑 ] (9)
with partition weight given as ∏( ) Λ(𝜑) + 𝑀𝑘∣𝑘−1 [Υ𝜑 ] 𝜔𝜋 =
𝜑∈𝜋
∑
∏ (
),
Λ(𝜑′ ) + 𝑀𝑘∣𝑘−1 [Υ𝜑′ ]
(10)
𝜋 ′ ∈Π𝑍𝑘 𝜑′ ∈𝜋 ′
given the following denotations ∫ 𝜇[1 ∣ s, x] = 𝜇(𝝃 ∣ s, x) d𝝃, ∫ 𝑀𝑘∣𝑘−1 [Υ𝜑 ] = Υ𝜑 (s, x) 𝑀𝑘∣𝑘−1 (x ∣ s) dx, ( ∫ ) Υ𝜑 (s, x) = exp − 𝜇(𝝃 ∣ s, x) d𝝃 ) ∏ (∫ × 𝜇(𝝃 ∣ s, x) 𝑙z (𝝃, x) d𝝃 ,
∑
Υ𝜑 (s, x) . Λ(𝜑) + 𝑀𝑘∣𝑘−1 [Υ𝜑 ]
(13)
The study into appropriate partitioning methods is beyond the scope of this paper. However we assume, for the remainder of the paper, that such a partition 𝝅 exists for each measurement set. Finally, the presence of the lone exponential term in (13) can be accounted for by the Poisson point process approximation of the daughter process and the possible occurrence of an empty measurement set, attributing to missed detected targets. Hence this term can be regarded as the probability of missed detection, which for a daughter process with Poisson rate greater than 3 is less than 0.05, and can be interpreted in the following way, that the more boundary points on a target’s extent, the more measurements are likely to be generated resulting in a low probability of missed detection. III. I MPLEMENTATION We implement the PHD filter for the multiple extended target model, described in Section II, using a Dirac mixture model for the intensity of the target shape. Each component of this mixture model has associated with it a Gaussian mixture formulation for the intensity of the kinematic state conditioned on that particular shape component. At each iteration of the filter, we start with the following prior intensities 𝑁𝑘−1
𝑀𝑘−1 (s) =
∑ 𝑖=1
(𝑖)
(𝑖)
𝜛𝑘−1 𝛿(s − s𝑘−1 ),
(14)
(𝑖)
(𝑖) 𝑀𝑘−1 (x ∣ s𝑘−1 )
𝐽𝑘−1
=
(11)
z∈𝜑
for daughter process intensity 𝜇(𝝃 ∣ s, x) and singlemeasurement likelihood 𝑙z (𝝃, x). The clutter term is given by ∏ Λ(𝜑) = 𝜆1 (u𝜑 ) e−𝜆2 [1 ∣ u𝜑 ] 𝜆2 (z ∣ u𝜑 ), (12) z∈𝜑
∑ 𝑗=1
(𝑗∣𝑖)
(𝑖)
(𝑗∣𝑖)
(𝑗∣𝑖)
𝜔𝑘−1 𝒩 (x ∣ s𝑘−1 ; m𝑘−1 , P𝑘−1 ). (15)
The notation 𝒩 (x ; m, P) is used to denote a Gaussian distribution with mean vector m and covariance matrix P, and 𝛿(s) denotes the Dirac delta distribution centred at s. The mixture in (14) defines a particle representation of 𝑀𝑘−1 (s), which can be expressed as the set of weighted particles 𝑁𝑘−1 (𝑖) (𝑖) . {𝜛𝑘−1 , s𝑘−1 }𝑖=1 A. Gaussian mixture formulation
for false measurements modelled as a Poisson cluster process whose probability density is defined by (3). The likelihood 𝐿𝑍𝑘 (s, x) updates both the shape parametric s and kinematic state x and involves the summation over all possible partitions Π𝑍𝑘 of the measurement set. The number of partitions ∣Π𝑍𝑘 ∣ for a measurement set of size ∣𝑍𝑘 ∣ = 𝑛 equates to the 𝑛th Bell number (𝐵𝑛 = 1, 2, 5, 15, 52, 203, 877, 4140, 21147, 115975, . . . for 𝑛 = 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, . . . respectively). Consequently the operation of summing over all possible partitions in (9) is
Following the derivation of the Gaussian mixture PHD filter for single measurement targets in [4] and the single-group PHD filter in [20], a Gaussian mixture formulation can be attained for the predicted intensity of the kinematic state in (7) and the updated intensity of the kinematic state defined by (𝑖)
(𝑖)
(𝑖)
𝑀𝑘 (x ∣ s𝑘 ) = 𝐿𝑍𝑘 (s𝑘 , x) 𝑀𝑘∣𝑘−1 (x ∣ s𝑘 ), (𝑖)
(16)
where 𝐿𝑍𝑘 (s𝑘 , x) is the joint pseudo-likelihood given in (9) (𝑖) for each particle s𝑘 .
1113
(𝑖)
For the predicted intensity 𝑀𝑘∣𝑘−1 (x), we assume a linear Gaussian dynamic model given by 𝑓𝑘∣𝑘−1 (x ∣ x′ ) = 𝒩 (x ; Fx′ , Q),
(17)
where F is the state transition matrix and Q is the process noise covariance matrix. In addition suppose the probability of survival is state independent, i.e. 𝑝𝑆 (x′ ) = 𝑝𝑆 , and the intensity for the birth targets is a Gaussian mixture of the form (𝑖)
(𝑖) 𝛾𝑘 (x ∣ s𝑘 ) (𝑖)
∑
(𝑗∣𝑖) 𝜔𝛾,𝑘
𝑗=1 (𝑗∣)
(𝑖) 𝒩 (x ∣ s𝑘
(𝑗∣𝑖)
(𝑗∣𝑖) (𝑗∣𝑖) ; m𝛾,𝑘 , P𝛾,𝑘 ),
(18)
(𝑗∣𝑖)
(𝑖)
where 𝐽𝛾,𝑘 , 𝜔𝛾,𝑘 , m𝛾,𝑘 and P𝛾,𝑘 for 𝑗 = 1, . . . , 𝐽𝛾,𝑘 are given model parameters that determine the shape of the birth intensity for a particular particle 𝑖. The prediction for the kinematic state is then the same as that for the standard Gaussian mixture PHD filter (𝑖)
𝐽𝑘∣𝑘−1
(𝑖)
∑
𝑀𝑘∣𝑘−1 (x ∣ s𝑘 ) =
(𝑗∣𝑖)
(𝑖)
(𝑗∣𝑖)
(𝑗∣𝑖)
𝜔𝑘∣𝑘−1 𝒩 (x ∣ s𝑘 ; m𝑘∣𝑘−1 , P𝑘∣𝑘−1 ),
𝑗=1
(19) (𝑗∣𝑖)
(𝑗∣𝑖)
(𝑗∣𝑖)
where for birth targets we have 𝜔𝑘∣𝑘−1 = 𝜔𝛾,𝑘 , m𝑘∣𝑘−1 = (𝑗∣𝑖)
(𝑗∣𝑖)
(ℓ∣𝑖,𝑗)
and
(𝑖) 𝐽𝑘∣𝑘−1
(𝑗∣𝑖) 𝑝𝑆 𝜔𝑘−1 , (𝑗∣𝑖) Fm𝑘−1 ,
= =
⎡
(𝑖) 𝐽𝛾,𝑘
+
(ℓ∣𝑖,𝑗)
)𝑇
(ℓ∣𝑖,𝑗)
P𝝃,12
(𝑗∣𝑖)
P𝑘∣𝑘−1
(𝑖)
⎤ ⎦, (𝑗∣𝑖)
(𝑗∣𝑖)
(𝑗∣𝑖)
(ℓ∣𝑗,𝑖)
(ℓ∣𝑗,𝑖)
m𝝃 , P𝝃,11 are the means and covariances from the corresponding marginal Gaussian for the daughter process, (ℓ∣𝑗,𝑖) and P𝝃,12 are the covariances that describe the relationship between parent and daughter. As a consequence of the Gaussian mixture formulation for the daughter intensity, in the joint pseudo-likelihood given in (𝑖) (13), we have e−𝜇[1 ∣ s𝑘 ,x] = e−𝛼 , denoting the expected ∑𝐽𝝃(𝑗∣𝑖) (ℓ) number of boundary points as 𝛼 = ℓ=1 𝜐𝝃 , and in Υ𝜑 (s, x) we have ⎛ ∏ ∏ ⎜ (𝑖) ˆ R2 ) 𝜇[𝑙z ∣ s𝑘 , x] = ⎝ 𝒩 (z ; Hx, z∈𝜑
z∈𝜑
(𝑖)
(21)
ℓ=1
(ℓ)
(𝑖)
(ℓ∣𝑖,𝑗)
⎞
ℓ=1
(ℓ) (𝑖) (ℓ∣𝑖,𝑗) ˜ (ℓ∣𝑖,𝑗) )⎟ 𝜐𝝃 𝒩 (z ∣ s𝑘 ; Hm𝝃,𝑘∣𝑘−1 , R ⎠,
¯ 𝚤=1
(¯ 𝚥1 ,...,¯ 𝚥∣𝜑∣ )
(𝑖)
where the mean and covariance are given by ( )−1 ( ) (ℓ∣𝑖,𝑗) (ℓ∣𝑖,𝑗) (ℓ∣𝑖,𝑗) (𝑗∣𝑖) (𝑗∣𝑖) x − m𝑘∣𝑘−1 , + P𝝃,12 P𝑘∣𝑘−1 m𝝃,𝑘∣𝑘−1 = m𝝃 ( )−1 ( )𝑇 (ℓ∣𝑖,𝑗) (ℓ∣𝑖,𝑗) (ℓ∣𝑖,𝑗) (𝑗∣𝑖) (ℓ∣𝑖,𝑗) P𝝃,12 . P𝝃,𝑘∣𝑘−1 = P𝝃,11 − P𝝃,12 P𝑘∣𝑘−1 (23)
(¯ 𝚥 ∣𝑖,𝑗)
(26)
𝚥1 , . . . , 𝚥¯∣𝜑∣ ), then Now for further brevity, we denote 𝚥¯1:∣𝜑∣ = (¯ the measurement update for the kinematic state is (𝑖)
(𝑖)
𝑀𝑘 (x ∣ s𝑘 ) = e−𝛼 𝑀𝑘∣𝑘−1 (x ∣ s𝑘 )+
𝜑∈𝝅 𝑗=1 𝚥¯1:∣𝜑∣
(22)
⎞
¯ 𝚤 ˜ (¯𝚥¯𝚤 ∣𝑖,𝑗) )⎠ . ,R × 𝒩 (z¯𝚤 ∣ s𝑘 ; Hm𝝃,𝑘∣𝑘−1
∑ ∑ ∑
(ℓ∣𝑖,𝑗)
𝜐𝝃 𝒩 (𝝃 ∣ s𝑘 ; m𝝃,𝑘∣𝑘−1 , P𝝃,𝑘∣𝑘−1 ),
(25)
˜ (ℓ∣𝑖,𝑗) = HP(¯𝚥¯𝚤 ∣𝑖,𝑗) H𝑇 + R1 . where, for brevity, we denote R 𝝃,𝑘∣𝑘−1 This product of Gaussian mixtures can be expanded for 𝚥¯¯𝚤 = (𝑗∣𝑖) 1, . . . , 𝐽𝝃 and ¯𝚤 = 1, . . . , ∣𝜑∣ to give the following sum of Gaussian products ⎛ ∣𝜑∣ ∏ ∑ ˆ R2 ) ⎝ 𝜐 (¯𝚥¯𝚤 ) 𝒩 (z¯𝚤 ; Hx, 𝝃
(𝑖) 𝐽𝑘∣𝑘−1
(𝑗∣𝑖)
∑
∑
(20)
where 𝒩 (z ; H𝝃, R1 ) is the likelihood that the measurement z is related to the point 𝝃 on the boundary of the target’s shape, with projection matrix H and observation noise covariance ˆ R2 ) is the likelihood that the matrix R1 , and 𝒩 (z ; Hx, measurement z relates to the state vector x, with projection ˆ and observation noise covariance matrix R2 . In matrix H addition suppose the intensity of the daughter process is a mixture of conditional Gaussian functions of the form 𝐽𝝃
(𝑗∣𝑖)
, where m𝑘∣𝑘−1 ,
P𝑘∣𝑘−1 are the predicted means and covariances from (19),
×
P𝑘∣𝑘−1 = FP𝑘−1 F𝑇 + Q,
(𝑖) 𝐽𝑘−1 .
(24)
(𝑗∣𝑖)
ˆ R2 ), 𝑙z (𝝃, x) = 𝒩 (z ; H𝝃, R1 ) × 𝒩 (z ; Hx,
=
(ℓ∣𝑖,𝑗)
P𝝃,11
for 𝑗 = 1, . . . , 𝐽𝑘∣𝑘−1 and ℓ = 1, . . . , 𝐽𝝃
𝐽𝝃
For the updated intensity 𝑀𝑘 (x ∣ s𝑘 ) given in (16), we assume a separable linear Gaussian single-measurement likelihood given by
(𝑖) 𝜇(𝝃 ∣ s𝑘 , x)
and covariance given by the block
P𝝃,12
(𝑗∣𝑖)
=
𝑇
⎣(
m𝛾,𝑘 , P𝑘∣𝑘−1 = P𝛾,𝑘 , for persistent targets (𝑗∣𝑖) 𝜔𝑘∣𝑘−1 (𝑗∣𝑖) m𝑘∣𝑘−1
(𝑗∣𝑖)
m𝑘∣𝑘−1
m𝝃 matrix
(𝑗∣𝑖)
𝐽𝛾,𝑘
=
The Gaussian components in (22) are conditional Gaussian realisations from joint Gaussians with mean ] [
(¯ 𝚥
1:∣𝜑∣ 𝜔𝜑,𝑘
,𝑗∣𝑖)
(27) (¯ 𝚥
(𝑖)
𝒩 (x ∣ s𝑘 ; m𝑘 1:∣𝜑∣ (¯ 𝚥
,𝑗∣𝑖)
,𝑗∣𝑖)
(¯ 𝚥
, P𝑘 1:∣𝜑∣
,𝑗∣𝑖)
),
and covariances where for 1 ≤ ¯𝚤 ≤ ∣𝜑∣ the means m𝑘 1:¯𝚤 (¯ 𝚥 ,𝑗∣𝑖) P𝑘 1:¯𝚤 in the Gaussian components are ( ) (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) ˆm ˜ 𝑘 1:¯𝚤 ˜ 𝑘 1:¯𝚤 z¯𝚤 − H , =m + K𝑘 1:¯𝚤 m𝑘 1:¯𝚤 ( ) (¯ 𝚥1:¯𝚤 ,𝑗∣𝑖) (¯ 𝚥1:¯𝚤 ,𝑗∣𝑖) ˆ (¯ 𝚥 ,𝑗∣𝑖) ˜ 1:¯𝚤 P𝑘 = I − K𝑘 , H P 𝑘 (28)
1114
( )−1 (¯ 𝚥 ,𝑗∣𝑖) ˜ (¯𝚥1:¯𝚤 ,𝑗∣𝑖) H ˆ 𝑇 S(¯𝚥¯𝚤 ∣𝑖,𝑗) and K𝑘 1:¯𝚤 = P . The weights of 2 𝑘 the Gaussian components are (¯ 𝚥
1:∣𝜑∣ 𝜔𝜑,𝑘
,𝑗∣𝑖)
(𝑗∣𝑖) 𝜔𝑘∣𝑘−1
= Λ(𝜑) +
∑
(𝑖) 𝐽𝑘∣𝑘−1
𝑗=1
−𝛼
e ∑
𝚥¯1:∣𝜑∣
(𝑗∣𝑖) (𝑖) 𝐿𝚥¯1:∣𝜑∣ (s𝑘 )
(𝑗∣𝑖) 𝜔𝑘∣𝑘−1
e
−𝛼
(𝑗∣𝑖) (𝑖) 𝐿𝚥¯1:∣𝜑∣ (s𝑘 )
,
B. Particle representation To implement the Gaussian mixture formulation in Section III-A, we sample 𝑁𝑘−1 particles from the Markov transition density for the particle representation of 𝑀𝑘 (s), i.e. for 𝑖 = (𝑖) (𝑖) 1, . . . , 𝑁𝑘−1 , sample ˜s𝑘 ∼ 𝜋𝑘∣𝑘−1 (s ∣ s𝑘−1 ). The weights of the Dirac mixture can be updated then as follows (𝑖)
(29)
𝐿𝑍 (˜s ) (𝑖) (𝑖) 𝜛 ˜ 𝑘 = ∑𝑁 𝑘 𝑘 (𝑖) 𝜛𝑘−1 , 𝑘−1 s𝑘 ) 𝑖=1 𝐿𝑍𝑘 (˜
where (𝑗∣𝑖)
(𝑖)
∣𝜑∣ ∏
(¯ 𝚤,¯ 𝚥1:¯𝚤 ,𝑗∣𝑖)
𝜐𝝃 ¯𝚤 𝑞1
(𝑖)
(𝑖)
(𝑖) (z¯𝚤 ∣ s𝑘 ), where 𝐿𝑍𝑘 (˜s𝑘 ) can be determined from the updated intensity ¯ 𝚤=1 for the process (30) (𝑖) (𝑖) 𝐽𝑘∣𝑘−1 𝐽𝑘∣𝑘−1 ∑ ∑ ∑ (¯𝚥1:∣𝜑∣ ,𝑗∣𝑖) ∑ in which we have (𝑖) (𝑗∣𝑖) 𝜔𝑘∣𝑘−1 + 𝜔𝜑,𝑘 , 𝐿𝑍𝑘 (˜s𝑘 ) = e−𝛼 (¯ 𝚤,¯ 𝚥1:¯𝚤 ,𝑗∣𝑖) (𝑖) (𝑖) (¯ 𝚥1:¯𝚤 ∣𝑖,𝑗) (¯ 𝚥1:¯𝚤 ∣𝑖,𝑗) 𝑞1 (z¯𝚤 ∣ s𝑘 ) = 𝒩 (z¯𝚤 ∣ s𝑘 ; b1 , S1 ), 𝜑∈𝝅 𝑗=1 𝚥¯1:∣𝜑∣ 𝑗=1 (¯ 𝚤,¯ 𝚥 ,𝑗∣𝑖) (𝑖) (𝑖) ˆ (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ∣𝑖,𝑗) (36) ˜ 𝑘 1:¯𝚤 (z¯𝚤 ∣ s𝑘 ) = 𝒩 (z¯𝚤 ∣ s𝑘 ; H m , S2 1:¯𝚤 ), 𝑞2 1:¯𝚤 (31) (¯ 𝚥1:∣𝜑∣ ,𝑗∣𝑖) is as defined in (29). A resampling such that 𝜔𝜑,𝑘 ( )−1 (¯ 𝚥¯𝚤 ∣𝑖,𝑗) (𝑗∣𝑖) (¯ 𝚥¯𝚤 ∣𝑖,𝑗) procedure is then applied, which makes 𝑁𝑘 copies of particles P𝑘∣𝑘−1 = HP𝝃,12 , and, denoting A (𝑖) (𝑖) 𝑁𝑘−1 {𝜛 ˜ 𝑘 , ˜s𝑘 }𝑖=1 and eliminates low-weighted particles to give ( ) (¯ 𝚥 ∣𝑖,𝑗) (¯ 𝚥 ∣𝑖,𝑗) (¯ 𝚥 ∣𝑖,𝑗) (𝑗) (𝑖) (𝑖) 𝑘 = Hm𝝃 ¯𝚤 + A(¯𝚥¯𝚤 ∣𝑖,𝑗) 𝜼 𝑘 1:¯𝚤 − m𝑘∣𝑘−1 , b1 1:¯𝚤 {𝜛𝑘 , s𝑘 }𝑁 𝑖=1 . ( )𝑇 (¯ 𝚥 ∣𝑖,𝑗) (¯ 𝚥 ,𝑗∣𝑖) IV. S IMULATED E XAMPLE ˜ (¯𝚥¯𝚤 ∣𝑖,𝑗) , A(¯𝚥¯𝚤 ∣𝑖,𝑗) + R = A(¯𝚥¯𝚤 ∣𝑖,𝑗) Σ𝑘 1:¯𝚤 S1 1:¯𝚤 The methodology and implementation detailed in Sections (¯ 𝚥 ∣𝑖,𝑗) ˆP ˜ (¯𝚥1:¯𝚤 ,𝑗∣𝑖) H ˆ 𝑇 + R2 . =H S2 1:¯𝚤 𝑘 II & III apply to extended targets with any structural shape (32) in general. In this section we present a multiple extended target tracking example in which the extents are, specifically, Furthermore, for 1 ≤ ¯𝚤 ≤ ∣𝜑∣ ( ) elliptical in shape, that the proposed implementation has (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ∣𝑖,𝑗) ˜ (¯𝚥1:¯𝚤 ,𝑗∣𝑖) z¯𝚤 − b(¯𝚥1:¯𝚤 ∣𝑖,𝑗) , ˜ 𝑘 1:¯𝚤 = 𝜼 𝑘 1:¯𝚤 +K m 1 𝑘 been tested on, using simulated data. The generated target ) ( (33) trajectories are shown in Fig. 1 along with the elliptical shapes, (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) (¯ 𝚥 ,𝑗∣𝑖) ˜ 1:¯𝚤 ˜ 1:¯𝚤 = I−K A(¯𝚥¯𝚤 ∣𝑖,𝑗) Σ𝑘 1:¯𝚤 , P 𝑘 𝑘 parameterized by the major and minor axes, which gradually ( )−1 ( ) reduce in size over 100 iterations, for each target. The target 𝑇 (¯ 𝚥 ∣𝑖,𝑗) ˜ (¯𝚥1:¯𝚤 ,𝑗∣𝑖) = Σ(¯𝚥1:¯𝚤 ,𝑗∣𝑖) A(¯𝚥¯𝚤 ∣𝑖,𝑗) S1 1:¯𝚤 and where K 𝑘 𝑘 trajectories are also shown plotted against time, along with the { (𝑗) simulated measurements, in Fig. 2. for ¯𝚤 = 1, m𝑘∣𝑘−1 (¯ 𝚥 ∣𝑖,𝑗) In the following subsection we first provide precise details 𝜼 𝑘 1:¯𝚤 = (¯ 𝚥 ,𝑗∣𝑖) m𝑘 1:¯𝚤−1 for 1 < ¯𝚤 ≤ ∣𝜑∣, of the simulation set-up before presenting the simulation { (𝑗) (34) results in Section IV-B. for ¯𝚤 = 1, P𝑘∣𝑘−1 (¯ 𝚥 ,𝑗∣𝑖) Σ𝑘 1:¯𝚤 = (¯ 𝚥1:¯𝚤−1 ,𝑗∣𝑖) P𝑘 for 1 < ¯𝚤 ≤ ∣𝜑∣. A. Set-up
𝐿𝚥¯1:∣𝜑∣ (s𝑘 ) =
(¯ 𝚥 )
(35)
(¯ 𝚤,¯ 𝚥1:¯𝚤 ,𝑗∣𝑖)
(z¯𝚤 ∣ s𝑘 ) 𝑞2
(𝑖)
(𝑖)
The resulting mixture in (27) has 𝐽𝑘∣𝑘−1 + ∣𝝅∣ × 𝐽𝑘∣𝑘−1 × (𝑗∣𝑖) 𝐽𝝃
∣𝜑∣ × components and left unchecked the size of the parent process mixture would grow at an approximately exponential rate with every time step. Many of these come from low-likelihood measurement associations and contribute little to the updated intensity in (27). There are a couple of Gaussian mixture reduction procedures we employ to then eliminate and merge such low-weight components. The first is a measurement gating procedure as described in [27] which we apply to determine how many boundary points 𝝃 are related to measurements z ∈ 𝜑 for each 𝜑 ∈ 𝝅, hence reducing the number of components in the mixture within the product given in (25). The other procedure is the pruning and merging one presented in [4]. Applying both procedures help maintain a more manageable approximation of the kinematic state intensity.
The target state X𝑘 consists of the kinematic state variable ]𝑇 [ x𝑘 = 𝑥𝑘,1 𝑥𝑘,2 𝑥𝑘,3 𝑥𝑘,4 , (37) ]𝑇 [ is the position vector (centre of the where 𝑥𝑘,1 𝑥𝑘,3 ]𝑇 [ ellipse) and 𝑥𝑘,2 𝑥𝑘,4 is the velocity vector at time-step 𝑘, and shape parametric variable ]𝑇 [ (38) s𝑘 = 𝑠𝑘,1 𝑠𝑘,2 𝑠𝑘,3 𝑠𝑘,4 , where 𝑠𝑘,1 is the magnitude of the major axis and 𝑠𝑘,3 is the magnitude of the minor axis of the ellipse, while 𝑠𝑘,2 & 𝑠𝑘,4 are the rates of change in the major and minor axes’ magnitudes respectively. Targets follow the linear Gaussian dynamic model (17) with ] [ F𝑘 02 , Q = 𝜎x2 Γ𝑘 Γ𝑇𝑘 , (39) F= 02 F𝑘
1115
500
400
400
400
300
200
100
y coordinate (in m)
500
y coordinate (in m)
y coordinate (in m)
500
300
200
100
0 0
100
200 300 x coordinate (in m)
400
200
100
0 0
500
300
100
(a) Trajectory 1
200 300 x coordinate (in m)
400
0 0
500
100
(b) Trajectory 2
200 300 x coordinate (in m)
400
500
(c) Trajectory 3
Figure 1: The multiple target trajectories (line) and size varying elliptical extents. Starting positions are indicated by the blue ’filled diamonds’ at times (a) 𝑘 = 1, (b) 𝑘 = 26, (c) 𝑘 = 51 and terminal positions are indicated by the blue ’filled squares’ at times (a) 𝑘 = 100, (b) 𝑘 = 125, (c) 𝑘 = 150. 500
x (in m)
400 300 200 100 0 0
50
100
150
500
y (in m)
400 300 200 100 0 0
50
100
150
Time−step
Figure 2: Measurements (green ‘×’) and target trajectories (line).
such that F𝑘 =
[
]
1 𝜏 , 0 1
[ Γ𝑘 =
𝜏
/ 2 0
2
𝜏 0
0/ 𝜏2 2
0 𝜏
]𝑇 ,
(40)
where 02 denotes the √ 2 × 2 zero matrix, 𝜏 = 1s is the sampling period and 𝜎x = 2 (m/s2 ) is the standard deviation of the process noise. The probability of survival is 𝑝𝑆 = 0.9. ]𝑇 [ ∈ 𝑍𝑘 reThe point measurements z = 𝑧1 𝑧2 late to points 𝝃 on the ellipse according to the likelihood 𝒩 (z ; √ H𝝃, R1 ) in (21) with H = I2 and R1 = 𝜎12 I2 where 𝜎1 = 5m. They relate to the target state x according to the ˜ R2 ) in (21) with likelihood 𝒩 (z ; Hx, [ ] 1 0 0 0 ˆ H= , (41) 0 0 1 0 √ and R2 = 𝜎22 I2 where 𝜎2 = 50m. The average number of observed points on the ellipse is 𝛼 = 5. The clutter term in the PHD update is given by (12) with 𝜆1 (u𝜑 ) = 𝜅1 𝑈 (u𝜑 ) and
𝜆2 (z ∣ u𝜑 ) = 𝜅2 𝒩 (z ; u𝜑 , R𝜅 ) where 𝑈 (u𝜑 ) is the uniform density over the observation region, 𝜅1 = 20 is the average number of false measurement clusters over the region, 𝜅2 = 1 is the average number of measurements per cluster and R𝜅 = 5 × I2 . (𝑖) The filter is initialised with the birth intensity 𝛾𝑘 (x ∣ s𝑘 ) at time-step 𝑘 = 1. Birth targets are measurement driven, a technique applied for the PHD filter in [28]. That is, in (18), for measurements in each cluster subset 𝜑 ∈ 𝝅 and (𝑖) (𝑖) 𝑗 = 1, . . . , 𝐽𝛾,𝑘 where 𝐽𝛾,𝑘 = ∣𝝅∣, we have [
(𝑗∣𝑖) m𝛾,𝑘
1 ∑ 𝑧1 = ∣𝜑∣ z∈𝜑 (𝑗∣𝑖)
0
1 ∑ 𝑧2 ∣𝜑∣ z∈𝜑
]𝑇 0 (𝑗∣𝑖)
,
(42)
with covariance P𝛾,𝑘 = 𝜎22 I4 and weight 𝜔𝛾,𝑘 = 0.1, for each particle 𝑖. The sampling distribution for the particle representation of a target’s shape is a linear truncated Gaussian. That is
1116
600
400
400
200
x (m)
600
400 x (m)
x (m)
600
200
200
0
0
0
−200
−200
−200
50
100
25
75
125
50 600
400
400
400
200 0
y (m)
600
y (m)
y (m)
0 600
200
50 Time−step
100
150
100 Time−step
150
200
0
0
100
0
25
75 Time−step
125
50
Figure 3: Position estimates (blue ‘circles’), uncertainty (grey shaded area) and target trajectories (line).
20 10
30 20
0
50
0 25
100
5
50 Time−step
100
100
150
100 Time−step
150
20
15 10
15 10 5
5
0
20
0 50
125
Minor axis (m)
Minor axis (m)
10
0
75
20
15
30
10
10
20 Minor axis (m)
Major axis (m)
30
0
40
40 Major axis (m)
Major axis (m)
40
0 25
75 Time−step
0 50
125
Figure 4: Shape estimates (blue ‘circles’), uncertainty (grey shaded area) and true shape variation (line). (𝑖)
(𝑖)
for persistent targets, we sample ˜s𝑘 ∼ 𝑓𝑘∣𝑘−1 (s ∣ s𝑘−1 ) = (𝑖) ˜ such that ˜s(𝑖) lies within the interval (a, b) 𝒩 (s ; Fs𝑘−1 , Q) 𝑘 where [ ]𝑇 [ ]𝑇 a = 19 −0.2 14 −0.1 , b = 32 0 21 0 , (43) F is as defined in (39) and ˜ = Q
2 2 2 2 Σs = diag([𝜎s,1 , 𝜎s,2 , 𝜎s,3 , 𝜎s,4 ]), (44) √ √ √ √ for 𝜎s,1 = 2m, 𝜎s,2 = 0.2m, 𝜎s,3 = 1m, 𝜎s,4 = 0.1m (𝑖) ˜𝑘 = and Γ𝑘 as defined in (40). For new targets, we sample c ]𝑇 [ (𝑖) (𝑖) (𝑖) ∼ 𝒩 (c ; bnew , Σnew ) such that c𝑘 lies within 𝑐𝑘,1 𝑐𝑘,2 [ ]𝑇 the interval (anew , bnew ), where anew = 19 14 , bnew = [ ]𝑇 (𝑖) 2 2 31 21 & Σnew = diag([𝜎s,1 , 𝜎s,3 ]), so that we have ˜s𝑘 = [ ]𝑇 (𝑖) (𝑖) 𝑐𝑘,1 0 𝑐𝑘,2 0 . Finally, the daughter process for each particle 𝑖 and kinematic state 𝑗 has the conditional Gaussian mixture form given (𝑗∣𝑖) (ℓ) = 𝛼, 𝜐𝝃 = 1, in (22) with 𝐽𝝃 (ℓ∣𝑖,𝑗)
(ℓ∣𝑖,𝑗)
P𝝃,12
ˆ = Hm P𝝃,11 = R1 , 𝑘∣𝑘−1 + t𝑘∣𝑘−1 , ⎡√ (𝑗∣𝑖) P𝑘∣𝑘−1 (1, 1) 0 0 √ = 𝛽𝜎1 ⎣ (𝑗∣𝑖) 0 0 P𝑘∣𝑘−1 (3, 3) (𝑗∣𝑖)
(ℓ∣𝑖,𝑗)
(ℓ∣𝑖,𝑗) t𝑘∣𝑘−1
=
, where
] [ (𝑖) (𝑗∣𝑖) (𝑖) (𝑗∣𝑖) 𝑠˜𝑘,1 cos(𝜃ℓ ) cos(𝜙𝑘∣𝑘−1 ) + 𝑠˜𝑘,3 sin(𝜃ℓ ) sin(𝜙𝑘∣𝑘−1 ) (𝑖)
(𝑗∣𝑖)
(ℓ∣𝑖,𝑗)
⎤ 0 ⎦ 0 (45)
(𝑖)
(𝑗∣𝑖)
𝑠˜𝑘,1 cos(𝜃ℓ ) sin(𝜙𝑘∣𝑘−1 ) − 𝑠˜𝑘,3 sin(𝜃ℓ ) cos(𝜙𝑘∣𝑘−1 ) (46) (𝑗∣𝑖)
Γ𝑘 Σs Γ𝑇𝑘 ,
m𝝃
(𝑗∣𝑖)
for all ℓ = 1, . . . , 𝐽𝝃
(𝑗∣𝑖)
(𝑗∣𝑖)
given 𝜃ℓ = 2𝜋ℓ/𝛼, 𝜙𝑘∣𝑘−1 = arctan(𝑚𝑘∣𝑘−1,4 /𝑚𝑘∣𝑘−1,2 ), (𝑗∣𝑖)
(𝑗∣𝑖)
denoting 𝑚𝑘∣𝑘−1,𝑛 as the 𝑛th element of mean vector m𝑘∣𝑘−1 , (𝑗∣𝑖)
and P𝑘∣𝑘−1 (𝑛, 𝑚) as the (𝑛, 𝑚) element of the covariance (𝑗∣𝑖)
matrix P𝑘∣𝑘−1 . Finally, we set 𝛽 = −0.4 where ∣𝛽∣ ≤ 1 relates to the correlation between parent and daughter.
B. Results The estimation results shown in Fig. 3 & 4 are from a simulation run with 500 particles for each target. Tracks are maintained by labelling each corresponding particle set. The state and error estimates are obtained by applying a similar clustering technique as demonstrated ∑ in [28] whereby the weights and means within the sum 𝜑∈𝝅 in (27) are (𝑛∣𝑖) (𝑛∣𝑖) re-indexed as 𝜔𝜑,𝑘 and m𝜑,𝑘 for 𝑛 = 1, . . . , 𝑁𝜑 where (𝑖) (𝑗∣𝑖) 𝑁𝜑 = 𝐽𝑘∣𝑘−1 × ∣𝜑∣ × 𝐽𝝃 . Then, for each cluster subset 𝜑, ∑𝑁𝑘−1 ∑𝑁𝜑 (𝑛∣𝑖) we compute 𝑊𝜑,𝑘 = 𝑖=1 𝑛=1 𝜔𝜑,𝑘 , and if 𝑊𝜑,𝑘 ≥ 𝑇 , for some threshold 𝑇 , we calculate the position and shape
1117
estimates respectively as 𝑁𝑘−1 𝑁𝜑
ˆ 𝜑,𝑘 = m
∑ ∑ 𝑖=1 𝑛=1
(𝑛∣𝑖) 𝜔𝜑,𝑘
(𝑛∣𝑖) m𝜑,𝑘 ,
𝑁𝑘−1 𝑁𝜑
ˆs𝜑,𝑘 =
∑ ∑ 𝑖=1 𝑛=1
(𝑛∣𝑖)
(𝑖)
𝜔𝜑,𝑘 ˜s𝑘 , (47)
and the corresponding errors are calculated as ˆ 𝜑,𝑘 = P
𝑁𝑘−1 𝑁𝜑
∑ ∑
(𝑛∣𝑖)
𝜔𝜑,𝑘
𝑖=1 𝑛=1 (𝑛∣𝑖) + (m𝜑,𝑘 𝑁𝑘−1 𝑁𝜑
ˆserr 𝜑,𝑘 =
∑ ∑ 𝑖=1 𝑛=1
(
(𝑛∣𝑖)
P𝜑,𝑘
) (𝑛∣𝑖) ˆ 𝜑,𝑘 ) (m𝜑,𝑘 − m ˆ 𝜑,𝑘 )𝑇 , − m (𝑛∣𝑖)
(𝑖)
(48)
(𝑖)
𝜔𝜑,𝑘 (˜s𝑘 − ˆs𝜑,𝑘 )(˜s𝑘 − ˆs𝜑,𝑘 )𝑇 ,
From the early simulation trials, it can be seen in Fig. 3 & 4 that the extracted estimates for both position and shape parameters are accurate, even when the uncertainty is high. The exception being when the targets approach the perimeter of the observation region towards the end of their existence, in which case the extents are only partly visible. V. C ONCLUSION The paper presented a hierarchical point process approach to tracking an extended target which gives rise to a structured set of measurements per time-step in the form of a geometric shape. The implementation of the corresponding PHD filter for the compound state, consisting of the target’s shape parametric and kinematic state, was based on a particle representation and a Gaussian mixture formulation respectively. The novelty of the approach is that it’s non-shape specific, and is applicable for structured sets of target generated measurements which take the form of any geometric shape that can be parameterised in a two-dimensional observation space. Demonstrated on a multiple extended target example where the extents are elliptical in shape and variable in size, early simulation results are promising. VI. ACKNOWLEDGEMENTS Anthony Swain is funded by an EPSRC Industrial CASE Award PhD Studentship with SELEX Galileo Ltd. Daniel Clark is a Royal Academy of Engineering/EPSRC Research Fellow. The authors would like to thank Neil Cade (SELEX Galileo Ltd) for his advice in contribution to this paper. R EFERENCES [1] R. P. S. Mahler, “Multitarget Bayes filtering via first-order multitarget moments,” IEEE Trans. AES, vol. 39, no. 4, pp. 1152–1178, 2003. [2] B. N. Vo, S. Singh, and A. Doucet, “Sequential monte carlo methods for multitarget filtering with random finite sets,” IEEE Trans. AES, vol. 41, no. 4, pp. 1224–1245, 2005. [3] D. E. Clark, B. T. Vo, and B. N. Vo, “Gaussian particle implementations of probability hypothesis density filters,” in Aerospace Conference, IEEE, 2007, pp. 1–11. [4] B. N. Vo and W. K. Ma, “The Gaussian mixture probability hypothesis density filter,” IEEE Trans. Signal Processing, vol. 54, no. 11, pp. 4091– 4104, 2006.
[5] D. E. Clark, B. N. Vo, and S. Godsill, “Gaussian mixture implementations of probability hypothesis density filters for non-linear dynamical models,” in Target Tracking and Data Fusion: Algorithms and Applications, IET Seminar on, 2008, pp. 21–28. [6] J. Houssineau and D. Laneuville, “PHD filter with diffuse partial prior on the birth process with applications to GM-PHD filter,” in 13th International Conference on Information Fusion, Edinburgh, UK, July 2010. [7] D. E. CLark, B. Ristic, B. N. Vo, and B. T. Vo, “Bayesian multi-object filtering with amplitude feature likelihood for unknown object SNR,” IEEE Trans. on Signal Processing, vol. 58, no. 1, pp. 26–37, 2010. [8] C. D. Haworth, Y. Saint-Pern, D. E. Clark, E.Trucco, and Y. Petillot, “Detection and tracking of multiple metallic objects in millimetre-wave images,” International Journal of Computer Vision, vol. 71, no. 2, pp. 183–196, 2007. [9] D. E. Clark, A. T. Cemgil, P. Peeling, and S. Godsill, “Multi-object tracking in sinusoidal components in audio with the Gaussian mixture probability hypothesis density filter,” in Applications of Signal Processing to Audio and Acoustics, IEEE Workshop, 2007, pp. 339–342. [10] R. P. S. Mahler, “PHD filters for nonstandard targets, I: Extended targets,” in 12th International Conference on Information Fusion, Seattle, WA, USA, July 6–9 2009, pp. 915–921. [11] K. Granstr¨ om, C. Lundquist, and U. Orguner, “A Gaussian mixture PHD filter for extended target tracking,” in 13th International Conference on Information Fusion, Edinburgh, UK, July 2010. [12] D. Daley and D. Vere-Jones, An introduction to the theory of point processes. Springer, 1988. [13] K. Granstr¨ om, C. Lundquist, and U. Orunger, “Tracking rectangular and elliptical extended targets using laser measurements,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, July 5–8 2011, pp. 592–599. [14] ——, “Estimating the shape of targets with a PHD filter,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, July 5–8 2011, pp. 49–56. [15] J. Mullane, B. N. Vo, M. Adams, and B. T. Vo, “A random finite set approach to Bayesian SLAM,” IEEE Trans. Robotics, vol. PP, no. 99, pp. 1–15, 2011. [16] M. Baum and U. D. Hanebeck, “Shape tracking of extended objects and group targets with star-convex RHMs,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, July 5–8 2011, pp. 338– 345. [17] N. Petrov, L. Mihayova, A. Gning, and D. Angelova, “A novel sequential Monte Carlo approach for extended object tracking based on border parameterisation,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, July 5–8 2011, pp. 306–313. [18] A. Swain and D. E. Clark, “Extended object filtering using spatial independent cluster processes,” in 13th International Conference on Information Fusion, Edinburgh, UK, July 2010. [19] ——, “First-moment filters for spatial independent cluster processes,” in Signal Processing, sensor fusion and target recognition XIX, SPIE Defense, Security and Sensing, I. Kadar, Ed., vol. 7697, Orlando, FL, USA, 2010. [20] ——, “The single-group PHD filter: an analytic solution,” in 14th International Conference on Information Fusion, Chicago, Illinois, USA, 2011, pp. 41–48. [21] C. S. Lee, D. E. Clark, and J. Salvi, “SLAM with single cluster PHD filters,” submitted to ICRA, 2012. [22] B. Ristic and D. E. Clark, “Particle filter for joint estimation of multiobject dynamic state and multi-sensor bias,” in IEEE International Conference on Acoustics, Speech and Signal Processing, March 25–30 2012. [23] D. E. Clark, “Bayesian filtering for multi-object systems with independently generated observations,” Arxiv preprint arXiv: 1202.0949, 2012. [24] ——, “Faa di Bruno’s formula for Gateaux differentials,” Arxiv preprint arXiv: 1202.0264, 2012. [25] D. E. Clark and R. P. S. Mahler, “Generalized PHD filters via a general chain rule,” submitted to Fusion, 2012. [26] R. P. S. Mahler, Statistical multisource-multitarget information fusion. Norwood, MA: Artech House, 2007. [27] Y. Bar-Shalom and T. E. Fortmann, Tracking and data association. San Diego: Academic Press, 1988. [28] B. Ristic, D. E. Clark, and B. N. Vo, “Improved SMC implementation of the PHD filter,” in 13th International Conference on Information Fusion, Edinburgh, UK, July 2010.
1118
