2017 IEEE Intelligent Vehicles Symposium (IV)

June 11-14, 2017, Redondo Beach, CA, USA

Pedestrian tracking using Velodyne data – stochastic optimization for

extended object tracking
Karl Granström1 , Stephan Reuter2 , Maryam Fatemi3 and Lennart Svensson1

Abstract— Environment perception is a key enabling tech- [6], the GIW approach was extended to additionally estimate
nology in autonomous vehicles, and multiple object tracking the Poisson measurement rate of each object, resulting in the
is an important part of this. High resolution sensors, such as Gamma GIW (GGIW) model.
automotive radar and lidar, leads to the so called extended
target tracking problem, in which there are multiple detec- The most common approach to multiple extended target
tions per tracked object. For computationally feasible multiple tracking is to model the problem using random finite sets
extended target tracking, the data association problem must (RFS) [7]. The GIW and GGIW models have been integrated
be handled. Previous work has relied on the use of clustering into RFS based filters, e.g. Probability Hypothesis Density
algorithms, together with assignment algorithms, to achieve (PHD) filter [8], [9], Cardinalized PHD (CPHD) filter [6],
this. In this paper we present a stochastic optimisation method
that directly maximises the desired likelihood function, and Labeled Multi-Bernoulli filters (GLMB and LMB) [10] as
solves the problem in a single step, rather than two steps well as the Poisson Multi-Bernoulli Mixture (PMBM) filter
(clustering+assignment). The proposed method is evaluated [11], [12]. The PHD and CPHD filters are examples of
against previous work in an experiment where Velodyne data moment approximations of the multi-object density, while
is used to track pedestrians, and the results clearly show that the GLMB and PMBM are examples of multi-object conjugate
the proposed method achieves the best performance, especially
in challenging scenarios. priors. Common to all mentioned multiple extended target
tracking filters is that they have to deal with the unknown
I. I NTRODUCTION measurement origin—it is unknown if a measurement’s
The realization of highly automated driving functions, as source is clutter or a target—something that leads to the
well as advanced driver assistance systems, requires a precise combinatorially complex data association problem.
and consistent environment model that incorporates dynamic In previous work [6], [8]–[12] the data association problem
as well as static obstacles. Object-based representations are is handled in two stages: first, clustering methods are used to
typically used for dynamic obstacles whose state is estimated find a set of different ways in which the measurements can be
over time using multi-target tracking (MTT) approaches. The clustered, and second, assignment methods are used to assign
main challenge of MTT is the simultaneous estimation of measurement clusters to targets. While these approaches have
the number of objects and their individual states, based on a produced good tracking results, they have difficulty handling
sequence of noisy measurements including missed detections dense scenarios with close targets, and rely on heuristics
and false alarms. rather than directly maximising the multi-target likelihood
Modern lidar and radar sensors typically deliver multiple function.
detections per object, since the object’s extent is not neg- In this paper we present a stochastic optimisation (SO)
ligible in comparison to the sensor resolution. During the method to handle the data association problem. The proposed
last decade, extended object tracking approaches, capable SO method can be used in any of the mentioned extended
of handling several measurements per object, have been target tracking filters, PHD, CPHD, GLMB, or PMBM. The
proposed. A single extended object measurement model has recently presented extended target PMBM filter has shown
to model the number of measurements per object, as well an interesting capability of integrating occlusion modelling
as the distribution of each measurement. The Poisson Point into the tracking, something that is of interest in dense and
Process (PPP) model [1] is a very popular model, where the cluttered urban environments. Hence, we have chosen to
individual measurements are modelled as spatially distributed integrate SO into the PMBM filter. The resulting tracking
around the target. Two popular spatial distribution models are filter is evaluated using Velodyne data from an autonomous
the Gaussian inverse Wishart (GIW) approach [2], [3] and the vehicle, and it is shown that using SO yields a tracking filter
Random Hypersurface Models [4]. The GIW model models that outperforms previous work.
the spatial distribution as Gaussian with unkown mean and II. P ROBLEM FORMULATION AND CONTRIBUTIONS
covariance; this implies elliptically shaped objects. In [5],
A. Background
1 Karl Granström and Lennart Svensson are with the Department of Let xik denote the state of the ith target at discrete time
Signals and Systems, Chalmers University of Technology, Gothenburg,
Sweden [email protected] step k, and let the target set be denoted Xk . The target set is
2 Stephan Reuter is with the Institute of Measurement, modeled as a random finite set (RFS) [7], meaning that the
Control and Microtechnology, Ulm University, Ulm, Germany number of targets, i.e., the target set cardinality is a time-
[email protected]
3 Maryam Fatemi is with Zenuity, Gothenburg, Sweden varying discrete random variable, and each target state xk ∈
[email protected] Xk is a random variable. The set of measurements obtained

978-1-5090-4804-5/17/$31.00 ©2017 IEEE 39

at time step k is also modeled as an RFS, and is denoted Zk . DBSCAN [16] are two examples of, will then return a single
Further, Zk denotes all measurement sets Zm from m = 0 measurement cluster, resulting in a single target.
up to, and including, m = k. The MTT problem consists of To improve the tracking results for close targets, two
using the information in the sequence of measurement sets different extensions based on k-means clustering and ex-
Zk to estimate the set of targets Xk , i.e., we are interested pectation maximisation for Gaussian mixtures, called sub-
in both the number of targets, as well as the states of the partition (SP) and EM-partition (EMP), respectively, were
individual targets. proposed in [8], [9]. Both simulations, as well as experiments
The multi-object density is denoted fk|k (Xk |Zk ). This where 2D lidar is used to track pedestrians, have shown
distribution captures what is known about the set of targets, that handling of close targets is successful when DP is used
given the sets of measurements. In this work we use the together with SP and EMP, see [9].
PMBM conjugate prior [11], [13] to represent the multi-
object distribution. The extended target PMBM filter [11],
C. Problems, and proposed approach
[12] propagates in time the target set PMBM density using a
Bayes update and a Chapman-Kolmogorov prediction. The Both simulation studies and experiments have shown that
focus in this work is on the Bayes update, defined as clustering algorithms can be an efficient part in handling
fk (Zk |Xk )fk|k−1 (Xk |Zk−1 ) the data association problem in extended target tracking,
fk|k (Xk |Zk ) =  , (1) however, a few drawbacks can be identified:
fk (Zk |Xk )fk|k−1 (Xk |Zk−1 )δXk
• To handle close targets multiple clustering algorithms
where fk (Zk |Xk ) is the multitarget measurement set density.
are necessary, see, e.g., [9].
Because of the unknown measurement origin, the up-
• Performance depends on the clustering parameters. In
date includes consideration of all possible data associa-
the worst case, incorrect parameter values lead to poor
tions A, i.e., all possible ways to assign measurements to
clustering, which leads to poor tracking results.
measurement sources. This has combinatorial complexity,
• The clustering does not consider the likelihood, which is
and approximations are necessary to enable computationally
the quantity that we ultimately would like to maximise
tractable tracking algorithms. The approximation consists of
with the computed association.
finding a subset of associations A that are highly probable,
i.e., that have high likelihoods LA k|k−1 , and then truncating
Even though intuition about the tracking problems can be
the associations that are unlikely. Finding a subset of highly used as guidance when selecting and parameterising the
probable associations in multiple extended target tracking is clustering algorithms, there is no guarantee that the computed
the problem considered in this paper. clusterings will actually yield highly probable associations.
Instead, one has to rely on empirical evidence that confirms
B. Previous work: clustering + assignment that this is the case.
Data association in extended target tracking can be split In this paper, a stochastic optimisation (SO) approach
into two parts, called partitioning and assignment, see, e.g., to extended object data association, and its integration in
[14]. The partitioning divides Zk into non-empty disjoint a GGIW-PMBM filter, is presented. The main idea of the
subsets called cells—this specifies which measurements are proposed approach is to start with an initial data association
from the same target. An illustrative example of partition- for the measurements and then randomly sample actions that
ing is given in [14, Sec. IV.C]. Given a partitioning, an change the association. By basing the probability of selecting
assignment then assigns the cells to measurement sources, an action on its impact on the multi-object likelihood—the
either clutter or targets. Previous work, see, e.g., [8]–[11], higher the resulting likelihood, the higher the probability of
has dealt with the data association problem by first using selecting the action—the approach efficiently finds highly
combinations of different clustering algorithms to compute probable associations. From the obtained sequence of asso-
a set of partitions, and then using, for each partition, assign- ciations, a subset of associations with high likelihoods is
ment algorithms, e.g., Murty’s algorithm [15], to assign the taken. The SO algorithm is inspired by work in Bayesian
cells to different targets. This efficiently reduces the number nonparametrics, see, e.g., [17], and some recent work on
of data associations, and empirical results are good. extended object mapping [18].
In early multiple extended target tracking, a clustering The benefits of the proposed SO method, in comparison
scheme called distance partitioning (DP) was proposed [8]. to previous work based on clustering, can be summarised as
DP builds upon a single linkage hierarchical clustering al- follows:
gorithm, and given an upper and a lower threshold for
the spatial proximity of measurements, it finds a subset of • It works equally well for both close and distant targets.
partitions. DP has been shown to work well when the targets • It only has one parameter: the number of iterations.
are spatially separated, however it has also been shown to • It works directly on the likelihood.
result in missed targets when two or more targets are spatially The proposed method is applied to pedestrian tracking
close [8]. The reason is that the detections from multiple using real sensor data from a Velodyne lidar, and the results
spatially close targets are also spatially close, and clustering are compared to the ones obtained using DP alone, or DP
algorithms based on spatial proximity, which DP and also together with SP/EMP.

40
 III. R EVIEW OF MODELLING detected, is
i
A. Random set modelling qD (xik ) = 1 − pD (xik ) + pD (xik )e−γ(xk ) . (3)
Two types of RFSs are important in this work: the PPP and Note that qD (xik ) is the conditional likelihood for an empty
the Bernoulli process. A PPP is an RFS whose cardinality set of measurements, i.e., ∅ (xik ) = qD (xik ) (cf. (2)).
is Poisson distributed, and each target is independent and C. The PMBM conjugate prior
identically distributed (i.i.d.). A PPP has two parameters, the
The PMBM model was developed for extended target MTT
Poisson rate μ and the spatial distribution f (x), and the PPP
in [11], [12], and for point target MTT in [13]. The PPP
intensity function is defined as D(x) = μf (x).
describes the distribution of the targets that are thus far
A Bernoulli RFS X is an RFS that is empty with probability
undetected, while the MBM describes the distribution of
1 − r or, with probability r, contains a single element with
the targets that have been detected at least once. Thus,
distribution f (x). In other words, the cardinality is Bernoulli
the set of targets can be divided into two disjoint subsets,
distributed with parameter r. A typical assumption in MTT
Xk = Xuk Xdk , corresponding to undetected targets Xuk and
is that the targets are independent. A multi Bernoulli (MB)
detected targets Xdk . The PMBM density is defined entirely
RFS X is the union of a fixed number of independent
by the parameters
Bernoulli RFSs Xi , X = i∈I Xi , where I is an index set.
j j,i j,i
The MB distribution is defined entirely by the parameters Dk|k
u
, {(Wk|k , {(rk|k , fk|k )}i∈Ij )}j∈Jk|k , (4)
k|k
{ri , f i (·)}i∈I of the involved Bernoulli RFSs. Here |I| is the
where
maximum number of targets that the MB RFS can represent.
• Dk|k
u
(·) is the PPP intensity for the set of undetected
Lastly, an MB mixture (MBM) is an RFS whose multi-object
targets Xuk . Higher/lower intensity in a location implies
density is a normalized weighted sum of MB densities, where
a higher/lower probability that a yet undetected target is
the components correspond to, e.g., different data association
located there. This can be used to model target occlusions,
sequences. An MBM density is defined entirely by the set
see [12] for an example.
of parameters {(W j , {rj,i , f j,i (·)}i∈Ij )}j∈J , where J is an
• j is an index for the MB components, and Jk|k is an index
index set for the MBs in the MBM (also called components
set. There are |Jk|k | MB components. Each MB component
of the MBM), W j is the weight of the jth MB, and Ij is an
corresponds to a unique global hypothesis for the detected
index set for the Bernoullis in the jth MB.
targets, i.e., a particular history of data associations for all
detected targets.
B. Standard extended target measurement model j
• The probability of the jth MB component is Wk|k .
The set of measurements obtained at time step k is the • For the jth MB component, i is an index for the Bernoulli
union of targetgenerated
 measurements and clutter measure- estimates, and Ijk|k is an index set. The jth component
ments, Zk = ∪i Wki ∪ Kk , where Kk denotes the set of has |Ijk|k | Bernoulli estimates. In the jth global association
clutter measurements, and Wki denotes the set of measure- hypothesis, the ith Bernoulli estimate has probability of
ments from the ith target. Standard MTT assumptions are existence rk|kj,i j,i
and target state pdf fk|k (·).
that the sets are all independent, and that the measurement
origin is unknown, i.e., for the measurement set Zk it is IV. DATA ASSOCIATION IN THE PMBM FILTER
not known which measurements are clutter, nor is it known Let the predicted multi-target density be a PMBM density,
which measurements originated from which target. with parameters
The set of clutter measurements Kk is modelled as a PPP u
Dk|k−1 j
, {(Wk|k−1 j,i
, {(rk|k−1 j,i
, fk|k−1 )}i∈Ij )}j∈Jk|k−1 , (5)
with rate λ and spatial distribution c(z), and the clutter PPP k|k−1

intensity is κ(z) = λc(z). An extended target with state let the measurements in the set Zk be indexed by m ∈ Mk ,
xik is detected with state dependent probability of detection Zk = {zm k }m∈Mk , and let A be the space of all data
j

pD (xik ), and, if it is detected, the target measurement set associations A for the jth predicted hypothesis. A data
Wki is modelled as a PPP with state dependent Poisson rate association A ∈ Aj is an assignment of each measurement
γ(xik ) and spatial distribution φ(·|xik ). For a non-empty set in Zk to a source, either to the background (clutter or new
of measurements (|Wki | > 0) the conditional extended target target) or to one of the existing targets indexed by Ijk|k−1 .
measurement set likelihood is denoted Note that Mk ∩ Ijk|k−1 = ∅ for all j.
i  Formally a data association A ∈ Aj consists of a partition
Wki (xik ) =pD (xik )e−γ(xk ) γ(xik )φ(z|xik ). (2)
i
of Mk ∪ Ijk|k−1 into non-empty disjoint subsets called index
z∈Wk
cells, denoted C ∈ A. The meaning of a cell is that all
Note that this is the product of the probability of detection elements in the cell are associated together1 . Due to the stan-
and the PPP density. The effective probability of detection 1 For example, let M = 1, 2, 3 and I = A, B, i.e., three measure-
i
for an extended target with state x is pD (xik )(1 − e−γ(xk ) ), ments and two targets. A valid partition, i.e., a possible association, is
i
where 1 − e−γ(xk ) is the Poisson probability of generating {1, 2, A}, {3}, {B}. The meaning of this is that measurements 1, 2 are
associated to target A, target B is not detected, and measurement 3 is not
at least one detection. Accordingly, the effective probability associated to any previously detected target, i.e., measurement 3 is either
of missed detection, i.e., the probability that the target is not clutter or from a new target.

41
dard MTT assumption that the targets generate measurements is the likelihood for measurement cell CC under association
independent of each other, an index cell contains either no A ∈ Aj .
target index, or one target index, i.e., C ∩ Ijk|k−1 = ∅ ⇒ The updated weight resulting from updating predicted
|C ∩ Ijk|k−1 | = 1 for all C ∈ A. If the index cell C contains hypothesis j with association A ∈ Aj , see (7), can thus
a target index, then let iC denote the corresponding target be written as follows

index. Further, let CC denote the measurement cell that j,A
Wk|k j
∝ Wk|k−1 Lj∅ Lj,A (12)
CC
corresponds to the index cell C, i.e., the set of measurements C∈A:
C∩Mk =∅
CC = ∪m∈C zm
k . (6)
Notice that, for predicted hypothesis j, the predicted weight
j
By [12, Theorem 1] the updated multi-target density is also Wk|k−1 and the likelihood of all targets being missed Lj∅ are
a PMBM. Given a predicted set of PMBM parameters, for each constant and independent of the data association A ∈ Aj . In
predicted global hypothesis there are multiple possible data other words, the difference lies in the product over the cell
associations, each of which will result in a MB component likelihoods Lj,A
CC .
in the updated MB mixture. The weight for the component As noted above, the size of the association space Aj
in the updated PMBM, that resulted from updating predicted is extremely large, and it is not possible to consider all
global hypothesis j ∈ Jk|k−1 with association A ∈ Aj , is associations in the tracking update. In the next section we
[12] present a stochastic optimisation method that finds a subset
j of associations with large likelihoods Lj,ACC .
j,A
Wk|k−1 Lj,A
k|k−1
Wk|k =  j j  ,A
(7) V. S TOCHASTIC OPTIMISATION
j  ∈Jk|k−1 A ∈Aj Wk|k−1 Lk|k−1

In this section we present a novel approach to handling
j
where Wk|k−1 is the predicted weight, and Lj,A
k|k−1 is the
the extended target data association problem, inspired by
likelihood of predicted hypothesis j and association A ∈ Aj . previous work [17], [18]. The Gibbs sampling algorithm [18]
The likelihood Lj,A and the Metropolis Hastings (MH) algorithm [17] are two
k|k−1 can be expressed as
examples of Markov Chain Monte Carlo (MCMC) methods,
  
Lj,A
k|k−1 = LbCC
j,i
LC C
C
j,iC
L∅ however, the presented algorithm does not have all the
C∈A: C∈A: C∈A: theoretical properties of an MCMC method. We chose to not
j j j
C∩Ik|k−1 =∅ C∩Ik|k−1 =∅ C∩Ik|k−1 =∅
implement an MCMC algorithm because we are interested in
C∩Mk =∅ C∩Mk =∅ C∩Mk =∅
(8a) finding probable data associations, not in generating sam-
⎧
 ples from the posterior distribution over data associations.
⎨ κ + DC
k|k−1 ; C if |C| = 1
u
LbC =
 (8b) We prioritise that the algorithm mixes well, rather than
⎩ Du
k|k−1 ; C if |C| ≥ 1 attributing value to the stationary distribution being equal

 to the posterior distribution. The method is called stochastic
Lj,i
C = r j,i
k|k−1 f j,i
k|k−1 ;  C (8c) optimisation (SO) because it takes stochastic steps towards

 maximising the likelihood (8a).
Lj,i j,i j,i
∅ = 1 − rk|k−1 + rk|k−1 fk|k−1 ; qD
j,i
(8d)
 A. Assignment variable
where
a; b = a(x)b(x)dx. The three products in (8a) We start by defining an assignment variable ϕ; this vari-
correspond to able facilitates the description, as well as the implementation,
• cells that are associated to the background, of the SO algorithm. Let ϕ be a vector of length |Zk |, with
• cells that are associated to previously detected targets, entries ϕm that indicate which sources the measurements are
• previously detected targets that are miss-detected. associated to. If ϕm ∈ Ijk|k−1 , then the measurement with
By multiplication by 1 in (8a) we get index m is associated to Bernoulli estimate with index ϕm ,
j,i
and if ϕm ∈ Ijk|k−1 , then the measurement with index m
  LC C 
Lj,A LbCC C
Lj,i is associated to a background object (clutter or new target).
k|k−1 = j,i ∅
(9)
C∈A: C∈A: L∅ C j
i∈Ik|k−1
Further, if ϕm1 = ϕm2 , then measurements zm k
1
and zm
k
2
j j
C∩Ik|k−1 =∅ C∩Ik|k−1 =∅
are associated to the same source, and if ϕm1 = ϕm2 , then
C∩Mk =∅ C∩Mk =∅
 measurements are associated to different sources. Note that
=Lj∅ Lj,A
CC (10) for ϕm ∈ Ijk|k−1 , it does not matter what value ϕm takes,
C∈A:
C∩Mk =∅
it only matters which measurements are associated together
 (i.e., for which m we have equal ϕm ). A set of measurements
where Lj∅ = i∈Ij Lj,i
∅ is the likelihood that none of the
associated to the same source is, analogously to (6), denoted
k|k−1
previously detected targets are detected in this time step, and Cc = ∪m:ϕm =c zm
k . (13)
⎧
⎨ LbCC if C ∩ Ijk|k−1 = ∅ Given an assignment vector ϕ we can obtain the equivalent
j,A
L CC =  −1 (11)
⎩ Lj,i C
Lj,i C
if C ∩ Ijk|k−1 = {iC } association A(ϕ) by simply forming subsets of the measure-
CC ∅
ment indices that have equal assignments, and including the

42
 TABLE I
target index if ϕm ∈ Ijk|k−1 . Note that the associations A
S TOCHASTIC OPTIMISATION ACTIONS
are unique, however, the assignments are not, i.e., two as-
signments ϕ = ϕ can have the same equivalent association, 1) Do nothing: ϕ(t+1) = ϕ(t) .
A(ϕ) = A(ϕ ). (t) (t)
2) If |Cc1 | > 1, move zr to cell Cc2 , where c2 ∈ S(t) \c1 :

B. Sampling (t+1) c2 if m = r
ϕm = (t) (14)
Given ϕ(t) , where superscript t denotes the iteration, ϕm otherwise
the next assignment ϕ(t+1) is obtained as follows. First, a There are |S(t) \c1 | = |S(t) | − 1 other cells to which the measure-
(t)
measurement index r is randomly sampled; let Cc1 denote ment can be moved, i.e., |S(t) | − 1 possible actions.
(t) (t) (t+1)
the corresponding cell, i.e., ϕr = c1 . By performing an 3) If |Cc1 | > 1, move zr to a new cell Cc , c ∈ S(t) :

action α that involves the measurement we can obtain a (t+1) c if m = r
ϕm = (15)
different assignment. Thus, starting from the assignment ϕ(t) ϕm
(t)
otherwise
and performing a randomly sampled action α we get the next This corresponds to one possible action.
(t)
assignment ϕ(t+1) = ϕα . (t) (t)
4) Merge cell Cc1 with cell Cc2 , where c2 ∈ S(t) \c1 :
1) Actions: The possible actions affect either the selected 
(t)
(t+1) c2 if ϕm = c1
measurement, or all measurements with the same assignment. ϕm = (t) (16)
ϕm otherwise
The selected measurement can be moved to another cell,
either an existing or a new cell. Alternatively, the selected There are |S(t) \c1 | = |S(t) | − 1 other cells with which it is possible
cell can be merged with an existing cell, or split into two to merge, i.e., |S(t) | − 1 possible actions.
5) If c1 ∈ Ijk|k−1 , change assignments to c ∈ S(t) :
parts. In Table I the actions are given, where S(t) = Ijk|k−1 ∪
(t) 
{ϕm } is used for notational brevity. (t+1) c
(t)
if ϕm = c1
ϕm = (t) (17)
In total, this gives the following number of possible actions ϕm otherwise
to choose from, This corresponds to one possible action.
⎧ (t) (t) (t) (t),1 (t),2
(t) 6) If |Cc1 | > 1, first split Cc1 into Cc1 and Cc1 . Next,
⎨ |S | + 1
⎪ |Cc1 | = 1
(t) a) if c1 ∈ Ijk|k−1 then
2|S(t) | + 2 if |Cc1 | > 1, c1 ∈ Ijk|k−1
⎪
⎩ (t) 
2|S(t) | + 3 |Cc1 | > 1, c1 ∈ Ijk|k−1 (t+1) c if zm
(t),2
k ∈ C c1
ϕm = (t) (18)
ϕm otherwise
The actions above that change the assignment only for the
selected measurements—cf. (14), (15)—correspond to the b) else, if c1 ∈ Ijk|k−1 , then select one of the two alternatives:
Gibbs sampling steps taken in the extended object mapping 
(t),2
algorithm presented in [18]. In the SO algorithm, the Gibbs (t+1) c if zm
k ∈ C c1
ϕm = (t) (19a)
sampling steps are complemented by merging and splitting— ϕm otherwise

(t),1
cf. (16), (17), (18) and (19)—inspired by the merge/split (t+1) c if zm
k ∈ C c1
ϕm = (t) (19b)
Metropolis Hastings algorithm presented in [17]. Adding ϕm otherwise
split/merge steps speeds up the process, i.e., fewer iterations
are required to find highly probable assignments, which is where c ∈ S(t) . Given a cell split, this corresponds to either one
action, (18), or two actions, (19). There are many ways to split the
important especially when there are multiple targets in close selected cell into two; here we used the k-means++ algorithm [19].
vicinity.
2) Sampling probabilities: Given an assignment ϕ(t) , the
probability for the next assignment resulting from an action j,A(t)
As shown in (10) each likelihood Lk|k−1 α
is proportional
α is given by the relative likelihood (cf. (7)), to a product over the cells resulting from the association.
(t)
   j,Aα
Lk|k−1 However, for each action α, only a subset of the cells given
(t+1) 
P ϕ = ϕ (α) ϕ(t) =
(t)
(20) by A will be changed. This fact can be utilized to simplify
 (t)
j,A(ϕα )
α Lk|k−1
the computations of the probabilities (20). For example,
(t)
(t) (t)
given the assignment ϕ(t) with equivalent association Aα =
where Aα = A(ϕα ) is the equivalent association that (t)
A(ϕα ), and an action α of type 2 in Table I, i.e., one that
(t)
 (t) α to assignment ϕ , and
follows (t)
  from applying action affects cells c1 and c2 , we obtain a new assignment ϕα with
(t+1) (t) 
αP ϕ = ϕ (α) ϕ = 1. (t)
equivalent association Aα and likelihood
For the actions (14), (15), also used in [18], the sampling
(t) −1 (t) −1
probabilities (20) correspond to Gibbs sampling, but with j,A(t) (t) A(t) A(t)
Lk|k−1
α
= Lj,A L
k|k−1 Cc
α
L A
Cc1 L α
Cc LACc2 (21)
the additional actions (16)–(19) the algorithm is not a proper 1 2

MCMC algorithm. However, by using (20) data associations

j,A(t)
with high posterior probabilities will be sampled more often, For the general case, we express this as Lk|k−1
α
=
(t)
which is a key property for our purposes. We are explicitly (t) (t)
Lj,A
k|k−1 Lα , where Lα
represents the product of the like-
looking for data associations with high posterior probabili- lihoods of the cells that are affected by the action. It is
j,A(t) (t)
ties, i.e., large Lk|k−1
α
. straightforward to calculate what Lα is for the different

43
 15 -0.5
1000

10 -1

5
100 -1.5

y [m]
y [m]

0
-2

-5 10
-2.5
k=919 k=911 k=903
k=895
-10
-3
10 10.5 11 11.5 12 12.5 13 13.5 14 14.5
-15 x [m]
-20 -15 -10 -5 0 5 10 15 20
x [m]
-0.5

Fig. 1. Visualisation of Velodyne data. The 2D world was gridded, and

-1
for each grid the number of times a detection fell inside was recorded.
The logarithmic colorscale corresponds to the number of times the grid cell
was detected. Stationary structures can be seen as yellow, the traces left by -1.5

y [m]
moving objects can be seen as green/light-blue. The data that was used to
evaluate pedestrian tracking was taken from the area indicated by the red -2
rectangle.
-2.5
k=919 k=911 k=903
k=895

types of actions. Inserting this into (20) and simplifiying -3

10 10.5 11 11.5 12 12.5 13 13.5 14 14.5
gives x [m]

   Lα
(t)
 -0.5
P ϕ(t+1) = ϕ(t) (α) ϕ(t) =  (t)
(22)
α L α -1

which is cheaper to compute than (20) is. -1.5

y [m]

C. Initialisation -2
The algorithm can be initialised with any valid partition;
-2.5
two simple initialisations are starting with a partition with k=919 k=911 k=903
k=895
all the measurement in individual cells, or starting with a -3
partition with all measurements in a single cell. Empirically 10 10.5 11 11.5 12 12.5 13 13.5 14 14.5
x [m]
we have confirmed that given enough iterations the algorithm
will find associations with high likelihood from any possible Fig. 2. Measurements (gray), PMBM-DP (orange), PMBM-SP / EMP (red),
starting point, however, by using the predicted PMBM pa- and PMBM-SO (blue). The target position is shown for each time step. For
rameters, it is possible to find an initialisation that allows the clarity, measurements and ellipses are shown for a subset of time steps. As
indicated by the time steps (k), the three targets move left to right.
algorithm to run in fewer iterations. The assignment variables
are initialised as

îm if îm ∈ Ijk|k−1 In the experiment presented below, the number of iterations
(0)
ϕm = j , (23a) was set adaptively to 5(|Mk |+|Ijk|k−1 |) and the threshold was
m + |Ik|k−1 | otherwise
set to 99.99%. The same threshold was used to truncated the
îm = arg max m,init
j,i , (23b) number of associations in the other filters in the experimental
i∈Ijk|k−1 ∪{0}

 comparison.
m,init
j,0 = λc(z m
k ) + p D D u
; 
k|k−1 {zk } ,
m (23c) VI. E XPERIMENTAL RESULTS
m,init
j,i = j,i
pD rk|k−1 γ(x̂j,i m j,i
k|k−1 )φ(zk |x̂k|k−1 ), (23d) The data for the experimental results is recorded with the
test vehicle of Ulm University. For the evaluation, the data
where x̂j,i
k|k−1 = Efk|k−1 [xk ]. In other words, if the max-
j,i of four Velodyne Puck 16 is used. The sensors are mounted
imum PPP likelihood corresponds to a target, then the on the roof, one in the front, one over each of the B-pillars
measurement is associated to that target. Otherwise the and one at the back. The raw measurements are projected
measurements initial association is a background object. to the ground plane and discretized in the x-y plane using
grid cells of 0.1 m in order to reduce the total number
D. Finding a subset of associations of measurements. The data for the evaluation was recorded
After the algorithm is run for T iterations, a set of with the car parked on the campus of Ulm University. A
highly probable unique associations is obtained by sorting the histogram of the discretized measurements over 10 minutes
unique equivalent associations A(t) in order of descending is visualized in Figure 1.
(t)
likelihood LA
k|k−1 , and then taking the associations whose We have compared three different variants of the GGIW-
cumulative sum of the relative likelihood exceeds a threshold. PMBM extended target tracking filter. Due to page length

44
 1
k =1111 k =1112 k =1113
y [m] 0
-0.4 -0.4 -0.4

-0.6 -0.6 -0.6

-1 -0.8 -0.8 -0.8

k=223 k=245
-1 -1 -1

-2 k=311

y [m]

y [m]

y [m]
k=267 k=289 k=333
-1.2 -1.2 -1.2

-1.4 -1.4 -1.4

8 10 12 14 16 18 -1.6 -1.6 -1.6

x [m] -1.8

-2
-1.8

-2
-1.8

-2

-2.2 -2.2 -2.2

1 -2.4

13.5 14 14.5 15 15.5

-2.4

13.5 14 14.5 15 15.5

-2.4

13.5 14 14.5 15 15.5

x [m] x [m] x [m]

0
y [m]

k =1114 k =1115 k =1116

-1
-0.4 -0.4 -0.4

k=223 k=245 -0.6 -0.6 -0.6

-2 k=267 k=289 k=311 k=333 -0.8 -0.8 -0.8

-1 -1 -1
8 10 12 14 16 18

y [m]

y [m]

y [m]
x [m]
-1.2 -1.2 -1.2

-1.4 -1.4 -1.4

-1.6 -1.6 -1.6

-1.8 -1.8 -1.8

1 -2 -2 -2

-2.2 -2.2 -2.2

0 -2.4 -2.4 -2.4

y [m]

13.5 14 14.5 15 15.5 13.5 14 14.5 15 15.5 13.5 14 14.5 15 15.5

x [m] x [m] x [m]

-1
k=223 k=245 k =1117 k =1118 k =1119
-2 k=267 k=289 k=311 k=333 -0.4 -0.4 -0.4

8 10 12 14 16 18 -0.6 -0.6 -0.6

x [m] -0.8

-1
-0.8

-1
-0.8

-1

y [m]

y [m]

y [m]
-1.2 -1.2 -1.2

-1.4 -1.4 -1.4

Fig. 3. Measurements (gray), PMBM-DP (orange), PMBM-SP / EMP (red), -1.6 -1.6 -1.6

and PMBM-SO (blue). The target position is shown for each time step. For -1.8 -1.8 -1.8

clarity, measurements and ellipses are shown for a subset of time steps. As -2

-2.2
-2

-2.2
-2

-2.2

indicated by the time steps (k), the three targets move left to right. -2.4 -2.4 -2.4

13.5 14 14.5 15 15.5 13.5 14 14.5 15 15.5 13.5 14 14.5 15 15.5

x [m] x [m] x [m]

k =1120 k =1121 k =1122

constraints the GGIW-PMBM filter details cannot be given; -0.4 -0.4 -0.4

the reader is referred to [11], [12] for details. All three filters -0.6

-0.8
-0.6

-0.8
-0.6

-0.8

are identical except for the different approximations used to -1 -1 -1

y [m]

y [m]

y [m]
-1.2 -1.2 -1.2

handle the data association in the update: PMBM-DP uses DP; -1.4

-1.6
-1.4

-1.6
-1.4

-1.6

PMBM - SP / EMP uses DP, SP and EMP ; and PMBM - SO uses SO . -1.8 -1.8 -1.8

-2 -2 -2

For most parts of the data the three compared tracking -2.2

-2.4
-2.2

-2.4
-2.2

-2.4

filters have identical performance; this is expected when the 13.5 14 14.5

x [m]
15 15.5 13.5 14 14.5

x [m]
15 15.5 13.5 14 14.5

x [m]
15 15.5

targets are well separated because all three filters are based
on the same single object models. Therefore we highlight Fig. 4. Measurements (gray), PMBM-DP (orange), PMBM-SP / EMP (red),
results from parts of the data that proved to be challenging and PMBM-SO (blue).
due to spatially close targets. In this data the pedestri-
ans move such that their respective measurements change
between forming one measurement cluster per pedestrian, as it causes estimates that are significantly erroneous.
and forming joint measurement clusters (i.e., two or more Figures 4 and 5 show scenarios in which targets pass each
pedestrians per cluster). other at close distance, while moving in opposite directions.
Figure 2 shows two pedestrians moving in the same For increased clarity, we have plotted each time step on its
direction. As soon as the targets become close, PMBM-DP own, rather than all time steps in one plot as in Figures 2 and
merges the targets, while PMBM-SP / EMP and PMBM-SO can 3. Velocity vectors indicate the direction the target is heading;
maintain both targets. Note how PMBM-DP maintains a single for increased clarity the length of the vectors are tripled in
target, even after the data clearly forms two clusters. Because the plots. Note that in the first figures, time steps k = 1111
the separation happens slowly, the target estimate is able and k = 748, because the targets have been well separated
to slowly adapt. A similar scenario, but with three targets, all three filters have the same estimates. When the targets
is shown in Figure 3. In this case PMBM-DP merges the become close, PMBM-SO manages to keep track of all three
close targets again. PMBM-SP / EMP can maintain three targets, targets, while both PMBM-DP and PMBM-SP / EMP produce
however, the estimated tracks are not as smooth as the results significantly worse estimates of the targets. As the targets
from PMBM-SO. In these two scenarios, because the targets separate again, PMBM-DP and PMBM-SP / EMP can recover
are moving in the same direction and with the same speed, all three targets, however, the estimated extents are too large
the merging can be seen as a benign error—the tracked compared to the results from PMBM-SO, see time steps k =
object represents a group of pedestrians. However, merging 1122 and k = 757. Note also that PMBM-SO produces better
of targets moving in different direction is a worse problem, estimates of the velocity vector than the other two filters do.

45
 k =748 k =749 k =750 k =751 k =752
-0.5 -0.5 -0.5 -0.5 -0.5

-1 -1 -1 -1 -1
y [m]

y [m]

y [m]

y [m]

y [m]
-1.5 -1.5 -1.5 -1.5 -1.5

-2 -2 -2 -2 -2

-2.5 -2.5 -2.5 -2.5 -2.5

14.5 15 15.5 16 16.5 14.5 15 15.5 16 16.5 14.5 15 15.5 16 16.5 14.5 15 15.5 16 16.5 14.5 15 15.5 16 16.5

x [m] x [m] x [m] x [m] x [m]

(a) (b) (c) (d) (e)

k =753 k =754 k =755 k =756 k =757

-0.5 -0.5 -0.5 -0.5 -0.5

-1 -1 -1 -1 -1
y [m]

y [m]

y [m]

y [m]

y [m]
-1.5 -1.5 -1.5 -1.5 -1.5

-2 -2 -2 -2 -2

-2.5 -2.5 -2.5 -2.5 -2.5

14.5 15 15.5 16 16.5 14.5 15 15.5 16 16.5 14.5 15 15.5 16 16.5 14.5 15 15.5 16 16.5 14.5 15 15.5 16 16.5

x [m] x [m] x [m] x [m] x [m]

(f) (g) (h) (i) (j)
Fig. 5. Measurements (gray), PMBM-DP (orange), PMBM-SP / EMP (red), and PMBM-SO (blue).

VII. C ONCLUDING REMARKS [9] K. Granström and U. Orguner, “A PHD filter for tracking multiple
extended targets using random matrices,” IEEE Transactions on Signal
This paper presented a sampling based method for data Processing, vol. 60, no. 11, pp. 5657–5671, Nov. 2012.
association approximation in multiple extended target track- [10] M. Beard, S. Reuter, K. Granström, V. B.-T., B.-N. Vo, and A. Scheel,
ing, called stochastic optimisation. The sampling method “Multiple extended target tracking with labelled random finite sets,”
IEEE Transactions on Signal Processing, vol. 64, no. 7, pp. 1638–
was successfully integrated into the PMBM extended target 1653, Apr. 2016.
conjugate prior, and an experiment with Velodyne data [11] K. Granström, M. Fatemi, and L. Svensson, “Gamma Gaussian
showed that the resulting filter outperforms previous methods inverse-Wishart Poisson multi-Bernoulli Filter for Extended Target
Tracking,” in Proceedings of the International Conference on Infor-
for data association approximation. In addition to yielding mation Fusion, Heidelberg, Germany, Jul. 2016.
superior results, the sampling method does not rely on the [12] ——, “Poisson multi-Bernoulli conjugate prior for multiple extended
use of multiple different algorithms, which previous methods object estimation,” arXiv pre-print, 2016. [Online]. Available:
arxiv.org/abs/1605.06311
do, and it works directly with the desired likelihood, which [13] J. Williams, “Marginal multi-Bernoulli filters: RFS derivation of
previous methods do not. MHT, JIPDA, and association-based MeMBer,” IEEE Transactions
on Aerospace and Electronic Systems, vol. 51, no. 3, pp. 1664–1687,
R EFERENCES Jul. 2015.
[14] K. Granström, M. Baum, and S. Reuter, “Extended Object Tracking:
[1] K. Gilholm and D. Salmond, “Spatial distribution model for tracking
Introduction, Overview and Applications,” Journal of Advances in
extended objects,” IEE Proceedings of Radar, Sonar and Navigation,
Information Fusion.
vol. 152, no. 5, pp. 364–371, Oct. 2005.
[15] K. Murty, “An algorithm for ranking all the assignments in order of
[2] W. Koch, “Bayesian approach to extended object and cluster tracking
increasing cost,” Operations Research, vol. 16, no. 3, pp. 682–687,
using random matrices,” IEEE Transactions on Aerospace and Elec-
1968.
tronic Systems, vol. 44, no. 3, pp. 1042–1059, Jul. 2008.
[16] M. Ester, H.-P. Kriegel, J. Sander, and X. Xu, “A density-based
[3] M. Feldmann, D. Fränken, and J. W. Koch, “Tracking of extended
algorithm for discovering clusters in large spatial databases with
objects and group targets using random matrices,” IEEE Transactions
noise.” AAAI Press, 1996, pp. 226–231.
on Signal Processing, vol. 59, no. 4, pp. 1409–1420, Apr. 2011.
[17] S. Jain and R. M. Neal, “A split-merge Markov chain Monte Carlo
[4] M. Baum and U. Hanebeck, “Extended object tracking with random
procedure for the Dirichlet process mixture model,” Journal of Com-
hypersurface models,” IEEE Transactions on Aerospace and Electronic
putational and Graphical Statistics, vol. 13, no. 1, pp. 158–182, 2004.
Systems, vol. 50, no. 1, pp. 149–159, Jan. 2013.
[18] M. Fatemi, K. Granström, L. Svensson, F. Ruiz, and L. Hammarstrand,
[5] K. Granström and U. Orguner, “Estimation and Maintenance of
“Poisson multi-bernoulli radar mapping using gibbs sampling,” IEEE
Measurement Rates for Multiple Extended Target Tracking,” in Pro-
Transactions on Signal Processing.
ceedings of the International Conference on Information Fusion,
[19] D. Arthur and S. Vassilvitskii, “k-means++: The Advantages of Careful
Singapore, Jul. 2012, pp. 2170–2176.
Seeding,” in Proceedings of the ACM-SIAM symposium on Discrete
[6] C. Lundquist, K. Granström, and U. Orguner, “An extended target
algorithms, Philadelphi, PA, USA, Jan. 2007, pp. 1027–1035.
CPHD filter and a gamma Gaussian inverse Wishart implementation,”
IEEE Journal of Selected Topics in Signal Processing, Special Issue
on Multi-target Tracking, vol. 7, no. 3, pp. 472–483, Jun. 2013.
[7] R. Mahler, Statistical Multisource-Multitarget Information Fusion.
Norwood, MA, USA: Artech House, 2007.
[8] K. Granström, C. Lundquist, and U. Orguner, “Extended Target
Tracking using a Gaussian Mixture PHD filter,” IEEE Transactions
on Aerospace and Electronic Systems, vol. 48, no. 4, pp. 3268–3286,
Oct. 2012.

46