A Random Finite Set Approach
for Dynamic Occupancy Grid Maps
Dominik Nuß
Band 20
Schriftenreihe des Instituts für Mess-, Regel- und Mikrotechnik
Dominik Stefan Nuß
A Random Finite Set Approach for
Dynamic Occupancy Grid Maps
Schriftenreihe des
Instituts für Mess-, Regel- und Mikrotechnik
Universität Ulm
Herausgeber:
Prof. Dr.-Ing. Klaus Dietmayer
Band 20
Dominik Stefan Nuß
A Random Finite Set Approach for
Dynamic Occupancy Grid Maps
Bibliografische Information der Deutschen Nationalbibliothek
Die Deutsche Nationalbibliothek verzeichnet diese Publikation in der
Deutschen Nationalbibliografie; detaillierte bibliografische Daten sind
im Internet über http://dnb.dnb.de abrufbar.
Dissertation, Universität Ulm,
Fakultät für Ingenieurwissenschaften, Informatik und Psychologie, 2016
Impressum
Universität Ulm
Institut für Mess-, Regel- und Mikrotechnik
Prof. Dr.-Ing. Klaus Dietmayer
Albert-Einstein-Allee 41
89081 Ulm
http://www.uni-ulm.de/mrm
Eine Übersicht über alle Bände der Schriftenreihe finden Sie unter
http://www.uni-ulm.de/mrmschriften.
Diese Veröffentlichung ist im Internet auf dem Ulmer Volltextserver (http://vts.uni-ulm.de) verfügbar
und dort unter der Lizenz “B - Standard (ohne Print-On-Demand)” publiziert.
Details zur Lizenz sind unter http://www.uni-ulm.de/index.php?id=50392 zu finden.
Institut für Mess-, Regel- und Mikrotechnik der Universität Ulm 2017
Print on Demand
ISBN 978-3-941543-28-7
e-ISBN 978-3-941543-29-4
A Random Finite Set Approach for
Dynamic Occupancy Grid Maps
DISSERTATION
zur Erlangung des akademischen Grades eines
DOKTOR-INGENIEURS
(Dr.-Ing.)
der Fakultät für Ingenieurwissenschaften, Informatik
und Psychologie der Universität Ulm
von
Dominik Stefan Nuß
aus Balingen
Gutachter:
Prof. Dr.-Ing. Klaus Dietmayer
Priv.-Doz. Dr. Wolfgang Koch
Amtierender Dekan:
Prof. Dr. rer. nat. Frank Kargl
Ulm, 16.12.2016
The acknowledgments were removed in the online version for privacy reasons.
Preamble
A short version of the thesis has been submitted for publication in The International
Journal of Robotics Research and has been made available to the public via arXiv:
Nuss, Dominik; Reuter, Stephan; Thom, Markus; Yuan, Ting; Krehl, Gunther; Maile,
Michael; Gern, Axel; Dietmayer, Klaus: A random finite set approach for dynamic
occupancy grid maps with real-time application. In: ArXiv e-prints, 2016. Available
online at http://arxiv.org/abs/1605.02406.
Abstract
Reliable vehicle environment perception is a basic prerequisite for advanced driver
assistant systems and autonomously driving cars. A common environment representation form is an occupancy grid map. It divides the environment into single grid
cells and estimates for each cell whether the space it represents is occupied or free,
assuming grid cells are independent of each other. The mathematical framework is
based on the binary Bayes filter (BBF), which combines sensor measurements from
different sensors and potentially from different points in time. Since an occupancy
grid map does not employ a concept of individual objects, it is able to represent
arbitrarily shaped obstacles.
A classical occupancy grid map is not eligible for estimating dynamic environments,
because it does not apply a process model. A much-noticed extension to a static
occupancy grid map is the Bayesian occupancy filter (BOF). In contrast to a classical
occupancy grid, the BOF estimates a velocity distribution for the occupancy of each
grid cell based on a histogram filter. Since the BOF is computationally extremely
demanding, recent publications suggest to represent the dynamic state of grid cells
with particles. This allows to calculate dynamic grid maps in real-time applications
with increased grid cell size and resolution. Today, dynamic occupancy grid maps are
still a younger research area and not as well-studied as object-tracking approaches
are. Up to now, the BOF has been addressed as a research field with little connection
to other tracking methods.
This work presents a new concept of dynamic grid mapping as an approximation
of a random finite set (RFS) filter. A random finite set is a general, probabilistic
representation of a random but limited number of objects and their states. The finite
set statistics (FISST) describe Bayesian filtering of random finite sets and are basis
for a number of multi-object tracking approaches like the probability hypothesis
density (PHD) filter. Describing the grid as a random finite set allows transferring
advanced concepts from the well-established field of random finite set filtering to
the field of dynamic grid mapping. The thesis derives a filter called probability
hypothesis density / multi-instance Bernoulli (PHD/MIB) filter, which represents
and propagates the dynamic grid map in alternating forms as a PHD and as multiple
instances of Bernoulli filters.
Additionally, the thesis presents a sequential Monte Carlo (SMC) realization of
the PHD/MIB filter and an approximation in the Dempster-Shafer domain called
Dempster-Shafer PHD/MIB (DS-PHD/MIB) filter, which requires a smaller number
of particles than the original PHD/MIB filter. The thesis describes in detail an
efficient, massively parallel implementation of the DS-PHD/MIB filter and outlines
the algorithm in pseudo code.
Finally, the thesis describes characteristics of the DS-PHD/MIB filter and discusses
its advantages and disadvantages compared to object-based tracking approaches
using practical application examples. A quantitative evaluation with real-world data
shows that the DS-PHD/MIB filter provides consistent state estimation results and
that it appropriately models the stochastic multi-object transition process and the
stochastic multi-object observation process. Furthermore, the evaluation confirms
the real-time capability of the parallelized implementation of the DS-PHD/MIB
filter and its usefulness for state estimation of a dynamic vehicle environment.
Kurzfassung
Die zuverlässige Wahrnehmung des Fahrzeugumfelds ist eine Grundvoraussetzung
für fortgeschrittene Fahrerassistenzsysteme und autonom fahrende Fahrzeuge. Eine
weit verbreitete Repräsentation der Fahrzeugumgebung ist eine Belegungskarte,
welche die Umgebung in einzelne Rasterzellen unterteilt und für jede Zelle die
Belegungswahrscheinlichkeit schätzt. Dies erfolgt meist unter der Annahme, dass
Rasterzellen untereinander statistisch unabhängig sind. Die Basis hierfür bildet das
binary Bayes filter (BBF), welches Sensordaten kombiniert, die von unterschiedlichen
Sensoren und unterschiedlichen Zeitpunkten stammen können. Da Rasterkarten auf
Objektkonzeptionen verzichten, sind sie in der Lage beliebig geformte Hindernisse
abzubilden.
Klassische Rasterkarten sind nicht geeignet, den Zustand dynamischer Umgebungen zu schätzen, da sie keine dynamischen Prozessmodelle berücksichtigen. Eine
viel beachtete Erweiterung statischer Rasterkarten ist das Bayesian occupancy filter (BOF). Im Gegensatz zu klassischen Belegungskarten schätzt das BOF eine
Geschwindigkeitsverteilung für jede Rasterzelle auf Basis eines Histogrammfilters.
Da das BOF rechentechnisch aufwändig ist, empfehlen jüngste Veröffentlichungen, den dynamischen Zustand der Rasterzellen durch Partikel abzubilden, was
die Echtzeit-Anwendung von Belegungskarten für einen größeren Bereich und mit
höherer Auflösung ermöglicht. Nach wie vor sind dynamische Rasterkarten ein junges
Forschungsfeld und weniger ausgereift als objektbasierte Zustandsschätzer. Bislang
wurden in diesem Forschungsfeld kaum Verbindungen zu anderen Bereichen der
Zustandsschätzung untersucht.
Diese Arbeit legt ein Konzept vor, welches die dynamische Zustandsschätzung mit
Rasterkarten als Approximation eines random finite set (RFS) Filters beschreibt.
Ein random finite set beschreibt eine Menge von Objekten und deren dynamischen
Zustand. Die Anzahl der Objekte ist hierbei probabilistisch verteilt aber limitiert.
Die sogenannten finite set statistics (FISST) behandeln Bayes’sche Filter für random
finite sets und bilden die Grundlage für viele Multi-Objekt-Zustandsschätzer wie das
probability hypothesis density (PHD) Filter.
Die Belegungskarte als random finite set auszudrücken erlaubt es, fortgeschrittene
und bewährte Konzepte aus dem umfangreich untersuchten Forschungsfeld der
random finite set Filter auf dynamische Rasterkarten zu übertragen. Die Arbeit
leitet daraus ein Filter ab, genannt probability hypothesis density / multi-instance
Bernoulli (PHD/MIB) Filter, welches den Zustand dynamischer Rasterkarten schätzt
und dabei in wechselnder Form als PHD und durch mehrere Instanzen von Bernoulli
random finite sets abbildet.
Zudem schlägt die Arbeit eine sequential Monte Carlo (SMC) Realisierung vor
und eine Approximation auf Basis der Dempster-Shafer Evidenztheorie, die als
Dempster-Shafer PHD/MIB (DS-PHD/MIB) Filter bezeichnet wird. Diese erlaubt die Anwendung mit einer reduzierten Anzahl an Partikeln gegenüber des
ursprünglichen PHD/MIB Filters. Die Arbeit beschreibt eine effiziente, stark parallelisierte Implementierung des DS-PHD/MIB Filters und erläutert diese anhand von
Pseudo-Quellcode.
Abschließend beschreibt die Arbeit charakteristische Eigenschaften des DS-PHD/MIB
Filters und diskutiert dessen Vor- und Nachteile gegenüber objektbasierten Zustandsschätzern anhand praktischer Bespiele. Eine quantitative Evaluation zeigt,
dass das DS-PHD/MIB Filter einen konsistenten Zustandsschätzer darstellt, der
den stochastischen Multi-Objekt-Zustand der Umgebung und dessen stochastischen
Multi-Objekt-Beobachtungsprozess berücksichtigt. Die Evaluierung bestätigt außerdem die Echtzeit-Fähigkeit der parallelen Implementierung und die Eignung des
DS-PHD/MIB Filters für die Zustandsschätzung des dynamischen Fahrzeugumfelds.
Contents
1 Introduction
1
2 Object Tracking
2.1 Single-Object Tracking . . . . . . . . . . . . . . . . . . .
2.1.1 Bayesian Filter . . . . . . . . . . . . . . . . . . .
2.1.2 Kalman Filter . . . . . . . . . . . . . . . . . . . .
2.1.3 Particle Filter . . . . . . . . . . . . . . . . . . . .
2.2 Multi-Object Tracking and Random Finite Set Statistics
2.2.1 Random Finite Set Statistics . . . . . . . . . . .
2.2.2 Multi-Object Probability Distributions . . . . . .
2.2.3 Multi-Object Filtering . . . . . . . . . . . . . . .
2.2.4 Bernoulli Filter . . . . . . . . . . . . . . . . . . .
2.2.5 Probability Hypothesis Density Filter . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
5
5
5
7
9
11
12
14
16
19
20
3 Grid Mapping
3.1 Static Grid Mapping . . . . . . . . . . . . . . . . . . . .
3.1.1 Ego Motion Compensation with Dead Reckoning
3.1.2 Measurement Grid Calculation . . . . . . . . . .
3.1.3 Grid Map Update . . . . . . . . . . . . . . . . .
3.2 Dynamic Grid Mapping . . . . . . . . . . . . . . . . . .
3.2.1 State of the Art in Dynamic Grid Mapping using
3.2.2 Discussion . . . . . . . . . . . . . . . . . . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
. . . . . .
Particles .
. . . . . .
.
.
.
.
.
.
.
25
25
26
28
30
32
33
35
.
.
.
.
.
.
.
.
37
37
38
39
40
40
43
52
55
5 Particle Realization and Dempster-Shafer Approximation
5.1 Particle Realization of the PHD/MIB Filter . . . . . . . . . . . . . .
5.1.1 Particle Prediction Step . . . . . . . . . . . . . . . . . . . . .
57
57
58
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4 Dynamic Grid Mapping as a Random Finite Set Problem
4.1 Problem Setting . . . . . . . . . . . . . . . . . . . . . . . . . . .
4.1.1 Environment Definition . . . . . . . . . . . . . . . . . . .
4.1.2 Filter Recursion Outline . . . . . . . . . . . . . . . . . . .
4.2 Probability Hypothesis Density / Multi-Instance Bernoulli Filter
4.2.1 Prediction Step of the MIB/PHD Filter . . . . . . . . . .
4.2.2 Update Step of the PHD/MIB Filter . . . . . . . . . . . .
4.3 Relation between PHD/MIB Filter and Binary Bayes Filter . . .
4.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
xvi
Contents
5.1.2 Particle Update Step . . . . . . . . . . . . . . . . . . . . . . .
5.1.3 Resampling . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Real-time Approximation using Dempster-Shafer Theory of Evidence
5.2.1 Dempster-Shafer Theory of Evidence . . . . . . . . . . . . . .
5.2.2 State Representation with Dempster-Shafer Approximation .
5.2.3 Prediction Step with Dempster-Shafer Approximation . . . .
5.2.4 Update Step with Dempster-Shafer Approximation . . . . . .
5.2.5 Practical Example: Radar Doppler Measurements . . . . . .
Summary of Approximations . . . . . . . . . . . . . . . . . . . . . .
60
62
63
63
71
72
72
77
79
6 Parallel Implementation
6.1 Parallelization Challenges . . . . . . . . . . . . . . . . . . . . . . . .
6.2 Parallel Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . .
83
83
84
7 Evaluation
7.1 Experiment Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.1.1 Test Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.1.2 Filter configuration . . . . . . . . . . . . . . . . . . . . . . . .
7.2 Qualitative Evaluation . . . . . . . . . . . . . . . . . . . . . . . . . .
7.2.1 Free-form Obstacle Representation in Dynamic Environments
7.2.2 Heterogeneous Sensor Data Fusion . . . . . . . . . . . . . . .
7.2.3 Systemic Limitations of Dynamic Grid Maps . . . . . . . . .
7.2.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7.3 Quantitative Evaluation . . . . . . . . . . . . . . . . . . . . . . . . .
7.3.1 Velocity Estimation of a Moving Object . . . . . . . . . . . .
7.3.2 Consistency of the DS-PHD/MIB filter . . . . . . . . . . . .
7.3.3 Separation of Moving and Stationary Obstacles . . . . . . . .
7.3.4 Computation Time . . . . . . . . . . . . . . . . . . . . . . . .
93
93
93
95
96
96
100
101
103
104
105
106
108
111
8 Conclusion and Outlook
113
Acronyms
115
List of Symbols
117
Bibliography
121
Publications
127
Supervised Theses
129
5.2
5.3
Chapter 1
Introduction
Self-driving cars are a technical revolution that will change our everyday life. One
of the most challenging problems in this field is the reliable perception of the vehicle
environment. Autonomous vehicles need to be able to detect arbitrarily shaped
static and dynamic obstacles and to distinguish between free space and occluded
areas. A basic approach investigated in the robotics domain are occupancy grid
maps [Elf89; TBF05]. A classic occupancy grid map divides the environment into
single grid cells and estimates the occupancy probability for each cell. Since several
measurements occur over time, the grid map combines these measurements with a
Bayesian filter. A commonly used filter for this application is the binary Bayes filter
(BBF), which combines measurements to estimate the binary state of a grid cell: free
or occupied [DRN15]. A restrictive assumption of the common BBF application is
that the environment is stationary. Furthermore, a common assumption of grid maps
is the independence of individual grid cells which facilitates a fast implementation
at the cost of approximation errors.
Today, grid maps are used in many automated vehicles [KNW+ 15; NWW+ 13;
Urm+ 08]. Due to their explicit free-space estimation and their ability to represent
arbitrarily shaped objects, grid maps are an important tool for collision avoidance.
Moreover, the spatial grid structure provides a convenient fusion layer for data
from different range finding sensors [LPP+ 11; NTDD14]. In vehicle environment
perception, the assumption of a stationary environment is obviously not fulfilled due
to moving road users like vehicles or pedestrians.
Recently, several approaches have been presented to combine grid mapping and
multi-object tracking. A well-known example is an approach called simultaneous
localization, mapping and moving object tracking (SLAMMOT) [WTT+ 07], which
retains a grid map and multiple object tracks at the same time and assigns object
detections either to the grid map or to a tracked object. Other publications suggest
associating grid cells directly to object tracks [BH10] or detecting object movement
2
Introduction
in multiple time frames of grid maps with a data driven Markov chain Monte Carlo
(DDMCMC) approach [VA09]. Further approaches combine grid mapping and object
tracking in a modular way [NSD14; VDN15].
However, some of these approaches imply complicated environment perception
architectures and are therefore not an appropriate choice for many applications.
In 2006, Coué et al. proposed the Bayesian occupancy filter (BOF) [CPL+ 06] as
an alternative architecture, which uses a four-dimensional grid to estimate a twodimensional environment. Here, two grid dimensions represent the spatial position
and two grid dimensions represent the two-dimensional velocity of the obstacles.
Thus, the BOF estimates object movement and explicitly considers it in its process
model. Although the BOF motivated further ideas [GBSD09; LPP+ 11], its area of
application is limited by its high computational load. This is caused by the large
number of grid cells necessary to represent the environment appropriately.
An important improvement by Danescu et al. [DON11] suggested to represent the
dynamic state of a grid cell with particles resulting in a significant reduction of
the computational load. In subsequent publications, Nègre et al. [NRL14] and
Tanzmeister et al. [TTWB14] independently proposed to represent only the dynamic
part of a grid map with particles instead of all occupied grid cells. In 2015, the
author of this thesis suggested to use the dynamic grid map as a fusion layer for laser
and radar measurements [NYK+ 15], which improves the overall performance of the
dynamic grid map, especially the separation between moving and static obstacles.
In summary, previous work on dynamic grid maps based on particles shows promising
results. Unfortunately, the proposed filters lack a stochastically rigorous definition
of the multi-object state estimation problem. As such, they describe evolutionary
algorithms (survival of the fittest) rather than Bayesian filters.
This thesis models the dynamic state estimation of grid cells as a random finite
set (RFS) problem. Finite set statistics (FISST) [Mah07a] provide a mathematical
framework for the state estimation of multiple dynamic objects in a Bayesian sense.
Well-established techniques from the field of FISST like the probability hypothesis
density (PHD) filter [Mah03] and the Bernoulli filter [Ris13] are applied to estimate
the dynamic state of grid cells. The resulting filter is called probability hypothesis
density / multi-instance Bernoulli (PHD/MIB) filter. Modeling the estimation
problem of a dynamic grid map in the random finite set domain yields substantial
advantages. It gives every filter parameter a physical meaning and allows a generic
and stochastically rigorous filter design for various estimation problems.
1 Introduction
3
The key contributions of the thesis are:
1. The definition of the dynamic state estimation of grid cells as an RFS problem
and the derivation of the PHD/MIB filter, which takes into account the special
form of measurement grids as they are common for grid mapping approaches.
2. The realization of the PHD/MIB filter with particles and an approximation in
the Dempster-Shafer domain.
3. A detailed pseudo code description of a massively parallel, real-time capable
approximation of PHD/MIB filter.
4. Results of experiments with real-world data and evaluation of estimation error
and consistency of the approximated PHD/MIB filter.
The remainder of the thesis is structured as follows. Chapter 2 gives an introduction
to object tracking and random finite set statistics. Chapter 3 outlines basics of
static grid maps and compares dynamic grid mapping techniques as presented by
Danescu et al. [DON11], Nègre et al. [NRL14], Tanzmeister et al. [TTWB14] and
the author [NYK+ 15]. The PHD/MIB filter is derived in Chapter 4. A particle-based
realization and an approximation in the Dempster-Shafer domain is presented in
Chapter 5. Chapter 6 provides a detailed description of a highly-efficient parallel
implementation, followed by the evaluation in Chapter 7. Chapter 8 presents the
conclusion of the thesis.
Chapter 2
Object Tracking
The goal of object tracking is the state estimation of dynamic objects based on
sensor measurements. A core approach is the Bayesian filter, which will be outlined
in Section 2.1.1. The Kalman filter [Kal60] is an optimal solution of the Bayes
filter applicable under certain conditions, as introduced in 2.1.2. An important
approximation for non-linear systems and multimodal state distributions is the
particle filter [RAG04; TBF05]. This sequential Monte Carlo (SMC) approach is
described in Section 2.1.3.
A further challenge is tracking multiple objects as outlined in Section 2.2. Finite set
statistics (FISST) [Mah07a] offer a fundamental framework for multi-object tracking
and are introduced in Section 2.2.1. Section 2.2.2 presents relevant examples of
multi-object distributions and Section 2.2.3 outlines the multi-object Bayes filter. A
special case is the Bernoulli filter [Ris13], presented in Section 2.2.4. Finally, Section
2.2.5 introduces the probability hypothesis density (PHD) filter [Mah03; Ris13],
which is a well-known, real-time capable approximation of the multi-object Bayes
filter.
2.1 Single-Object Tracking
2.1.1 Bayesian Filter
The state vector xk ∈ X describes the nx -dimensional state of an object at time
step k in the state space X = Rnx . The Bayesian filter considers nz -dimensional
observations zk ∈ Z in the measurement space Z = Rnz . All observations up to time
6
Object Tracking
step k are denoted by z1:k = {z1 , ..., zk }. The posterior density
pk (xk |z1:k )
(2.1)
is the probability density function (PDF) of the state xk at time step k given all
observations z1:k . For a shorter notation, the conditioning on the measurements will
be omitted and the posterior state density will be denoted pk (xk ). Note that this
notation does not distinguish between a random variable and its realization.
The model of a Markov process defines the transition density
f+ (xk+1 |xk ) = f+ (xk+1 |xk , z1:k ).
(2.2)
A Markov transition density is the probability density of a transition to the state
xk+1 given the former state xk . Due to the Markov assumption, all information
about the estimated process at time step k is contained in its current state xk . For
a shorter notation, the index "+" expresses a prediction step from time step k to
time step k + 1, often noted as k + 1|k.
The likelihood function
gk+1 (zk+1 |xk+1 )
(2.3)
is the probability density of the observation zk+1 at time step k + 1 given the state
xk+1 .
The Bayesian filter can be divided into two steps: prediction and update. The
first step predicts the state at time step k + 1 based on the process model and the
posterior density from time step k. The solution for the prediction is provided by
the Chapman-Kolmogorov equation:
Z
p+ (xk+1 ) = f+ (xk+1 |xk )pk (xk )dxk .
(2.4)
The second step updates the estimation with the observation at time step k + 1:
pk+1 (xk+1 ) = R
gk+1 (zk+1 |xk+1 )p+ (xk+1 )
gk+1 (zk+1 |xk+1 )p+ (xk+1 )dxk+1 .
(2.5)
With these two steps the Bayesian filter recursively estimates the dynamic state of
an object assuming the initial probability density p0 (x0 ) is known.
2.1 Single-Object Tracking
7
The Bayesian filter is the foundation of state estimators used in a variety of applications and will also be basis for all state estimators in this thesis.
2.1.2 Kalman Filter
The Kalman filter [Kal60] is the exact, explicit solution of the Bayes filter under
certain conditions, which are outlined below. The result of these conditions is that
the predicted as well as the posterior state distribution are analytically represented
by a Gaussian distribution [HL64]. It is therefore possible to directly calculate
the first and second statistical moment of the state distribution (mean vector and
covariance matrix) in both steps of the Bayes filter, which uniquely define the state
distribution.
The first condition is that the initial state estimation is given by a Gaussian distribution
1
1
p0 (x0 ) = N (x0 ; x̂0 , P0 ) := p
exp − (x0 − x̂0 )T P0−1 (x0 − x̂0 ) , (2.6)
2
det(2πP0 )
with mean x̂0 and covariance matrix P0 .
The process model is given by
xk+1 = F xk + ξk ,
(2.7)
with the state transition matrix F and the process noise ξk . The process noise is
white and follows a Gaussian distribution with zero mean. Equation (2.7) represents
a linear process model with additive process noise.
The measurement model is given by
zk+1 = Hxk+1 + ζk ,
(2.8)
with the observation matrix H and the observation noise ζk . The observation noise
is also white and follows a Gaussian distribution with zero mean. Equation (2.8)
represent a linear measurement model with additive measurement noise. Finally, the
last condition is that process noise and measurement noise are uncorrelated.
8
Object Tracking
From (2.6) and (2.7) it follows that the predicted state PDF p+ (xk+1 ) is a Gaussian
distribution with mean x̂+ and covariance matrix P+ given by
x̂+ = F x̂,
(2.9)
T
P+ = F Pk F + Qk ,
(2.10)
where Qk is the covariance matrix of the process noise: Qk = E{ξk ξkT }. Equations
(2.9) and (2.10) represent the Kalman filter prediction step.
The Kalman filer update step considers the measurement zk+1 . The measurement
residual γ k+1 is the difference between the measurement zk+1 and the predicted
measurement ẑ+ = H x̂+ :
γ k+1 = zk+1 − ẑ+ .
(2.11)
From the given conditions it follows that the PDF of the measurement residual γ k+1
is Gaussian with zero mean and covariance matrix Sk+1 given by
Sk+1 = HP+ H T + Rk ,
(2.12)
where Rk is the covariance matrix of the measurement noise: Rk = E{ζk ζkT }.
Finally, the PDF of the posterior state pk+1 (xk+1 ) is also a Gaussian distribution.
Therefore, the update step is complete by calculating mean x̂k+1 and covariance
matrix Pk+1 of the posterior state distribution with
x̂k+1 = x̂+ + Kγ k+1 ,
(2.13)
T
Pk+1 = P+ − Kk+1 Sk+1 Kk+1 ,
(2.14)
where Kk+1 is the Kalman gain, given by
Kk+1 = P+ H T Sk+1 .
(2.15)
With the update step, one recursion of the Kalman filter is complete.
In practice, process models and measurement models are often not linear. The two
most important approximations of the Kalman filter for non-linear systems are the
extended Kalman filter (EKF) [SSM62] and the unscented Kalman filter (UKF)
[JUD95].
2.1 Single-Object Tracking
9
2.1.3 Particle Filter
The particle filter [RAG04; TBF05] is a non-parametric realization of the Bayes
filter and therefore suitable for modeling non-linear systems and non-Gaussian state
distributions. It is also called sequential Monte Carlo (SMC) method.
The particle filter represents the posterior PDF pk (xk ) with a set of ν particles and
(i)
(i)
their weights {xk , wk }νi=1 by
pk (xk ) =
ν
X
(i)
(i)
wk δ(xk − xk ),
(2.16)
i=1
(i)
(i)
where xk are particle states and wk are particle weights. Here, δ is the Dirac delta
function which satisfies
Z∞
f (x)δ(x)dx = f (0)
(2.17)
−∞
for any test function f : X → R.
The goal of the particle filter as a Bayesian filter is to evolve the particle set to
represent the posterior state distribution of the next time step pk+1 (xk+1 ) considering
the process model, the measurement model and the observation zk+1 .
Importance Sampling
A prerequisite for the realization and also for the derivation of the particle filter is
the ability to represent arbitrary PDFs with particles and weights. This is possible
with the importance sampling method.
The goal is to represent a PDF p : Rnx → R with particles and weights according to
equation (2.16), even if is not possible to directly draw particles from p. Assume it
is possible to draw particles from the PDF q : Rnx → R which is similar to p:
p(x) > 0 ⇒ q(x) > 0 ∀ x ∈ Rnx .
(2.18)
The PDF q is called proposal density function. Usually, a uniform or Gaussian
distribution is chosen for q. If a number of ν particles are being drawn from q, their
weights have to be adapted to represent p.
10
Object Tracking
The unnormalized importance weights are calculated by
w
e(i) =
p(x(i) )
,
q(x(i) )
(2.19)
and the normalized weights are given by
w
e(i)
w(i) = Pν
.
e(j)
j=1 w
(2.20)
Sequential Importance Sampling
The general implementation of the particle filter is realized by the sequential importance sampling approach. Assume the posterior state distribution pk (xk ) of time
(i)
(i)
step k is represented by the particle set {xk , wk }νi=1 according to equation (2.16).
An observation zk+1 at time step k + 1 occurs. Also given is the state transition
probability f+ (xk+1 |xk ) and the measurement likelihood gk+1 (zk+1 |xk+1 ), known as
process model and measurement model. The first step of the particle filter draws ν
(i)
new particles. Each new particle xk+1 is drawn by sampling the proposition function
(i)
(i)
qk+1 (xk+1 |xk , zk+1 )
(2.21)
(i)
conditioning on the corresponding particle state of the last time step xk and the
measurement zk+1 .
One can show that the posterior state pk+1 (xk+1 ) at time step k + 1 is represented
by the newly drawn particles if the corresponding updated unnormalized weights
satisfy
(i)
(i)
(i)
(i)
(i) gk+1 (zk+1 |xk+1 )f+ (xk+1 |xk )
,
(i)
(i)
qk+1 (xk+1 |xk , zk+1 )
w
ek+1 = wk
(2.22)
and the normalized particle weights are
(i)
w
e
(i)
wk+1 = Pν k+1(j) .
ek+1
j=1 w
The derivation can be found in [RAG04] or [TBF05].
(2.23)
2.2 Multi-Object Tracking and Random Finite Set Statistics
11
Resampling
If the sequential importance sampling method is carried out several times, the
particles will degenerate. This means, the weight will concentrate on a small number
of particles, while most particles carry weights near zero. This can be avoided by a
resampling step. The resampling step draws ν particles from a set of ν particles. A
particle can be drawn several times or not at all, so duplicates are possible in the
new particle set. The probability of a particle to be chosen is proportional to its
weight. After the resampling step, all particles are assigned the same weight ν1 .
Sequential Importance Resampling
Different implementations of the particle filter can be distinguished by the applied
(i)
(i)
proposition density qk+1 (xk+1 |xk , zk+1 ). One of the most common approaches is
the sequential importance resampling (SIR) method. In this case, the proposition
density is chosen as
(i)
(i)
(i)
(i)
qk+1 (xk+1 |xk , zk+1 ) = f+ (xk+1 |xk ).
(2.24)
This is not the optimal choice in terms of the variance of particle weights, but it
is advantageous for practical reasons. From (2.22) and (2.23) it follows, that the
update step of the particle filter is carried out by recalculating the unnormalized
particle weights with
(i)
(i)
(i)
w
ek+1 = wk gk+1 (zk+1 |xk+1 )
(2.25)
and normalization afterwards. The SIR can carry out the resampling step only after
the update step.
2.2 Multi-Object Tracking and Random Finite Set
Statistics
The Bayes filter discussed so far always assumes an observation z can unambiguously
be associated with an object and its state x. For many object tracking problems
this is not the case. In a vehicle environment, a number of traffic participants like
pedestrians, trucks or cars generate a number of measurements. Further, missed
detections, clutter measurements or occluded objects make the association problem
even more challenging.
12
Object Tracking
Generally, two major groups of multi-object tracking approaches can be distinguished.
One group describes tracking of multiple objects with multiple instances of singleobject Bayes filters. This can be seen as bottom-up modeling. One of the main
challenges here is the association between measurements and objects, also referred to
as data association problem. Exemplary approaches to the problem are probabilistic
data association (PDA) [BF88; BT75], integrated probabilistic data association
(IPDA) [MES94], joint probabilistic data association (JPDA) [BWT11; FBS83],
joint integrated probabilistic data association (JIPDA) [ME04], or multi-hypothesis
tracking (MHT) [Rei79].
The other group uses a top-down modeling approach based on random finite sets
(RFS) [Mah07a]. Finite set statistics (FISST) provide a mathematical framework
for a Bayesian filter estimating the state of an RFS. An RFS inherently describes
the existence and the state of a random but limited set of objects on a probabilistic
basis. Section 2.2 summarizes the introduction to RFSs presented in [Ris13] and
follows its notation system.
2.2.1 Random Finite Set Statistics
With finite set statistics (FISST), it is possible to formally express the Bayes filter
for a random finite set (RFS). An RFS X represents a random but limited number
of n unordered elements and their random states x ∈ X = Rnx :
X = {x(1) , . . . , x(n) }.
(2.26)
It is also possible that the number of elements is zero, in which case X = ∅.
The cardinality distribution of an RFS is given by the discrete distribution ρ(n),
n ∈ N0 . This means that with a probability of ρ(n), the RFS consists of n elements.
The distribution of an RFS is uniquely defined by the cardinality distribution ρ(n)
and a set of PDFs
{fn (x(1) , . . . , x(n) ), n ∈ N | ρ(n) > 0}.
(2.27)
Each PDF is the joint distribution of the states of n objects
fn : Rnx ·n → R.
(2.28)
Since the order of the objects is irrelevant, fn is symmetric, i.e. it is equal for all n!
possible permutations of the objects {x(1) , . . . , x(n) }.
2.2 Multi-Object Tracking and Random Finite Set Statistics
All functions fn are standard PDFs in a sense that
Z
fn (x(1) , . . . , x(n) )dx(1) . . . dx(n) = 1.
13
(2.29)
Rn·nx
These functions represent the probability density of all joint object states under the
assumption of a certain cardinality.
Within the framework of FISST, the PDF π(X) of an RFS X is defined as [Mah07a]
(
ρ(0)
if X = ∅,
(1)
(n)
π(X = {x , . . . , x }) =
(2.30)
(1)
(n)
n! · ρ(n) · fn (x , . . . , x ) else.
The function π is also called multi-object probability density function (MPDF)
[Mah13]. The set integral of an MPDF is defined as
Z
π(X)δX = π(∅) +
Z
∞
X
1
π(x(1) , . . . , x(n) )dx(1) . . . dx(n) .
n!
n=1
(2.31)
It follows from (2.30) that the cardinality distribution ρ(n) can be expressed as
Z
1
π(x(1) , . . . , x(n) )dx(1) . . . dx(n) .
(2.32)
ρ(n) =
n!
The first statistical moment of an RFS is
(
) Z
X
X
D(x) = E
δ(x − w) =
δ(x − w)π(X)δX.
w∈X
(2.33)
w∈X
It is called probability hypothesis density (PHD). The integral over a PHD equals
the expected number of objects N̂ of an RFS:
Z
N̂ = D(x)dx.
(2.34)
14
Object Tracking
2.2.2 Multi-Object Probability Distributions
Depending on the application, different kinds of RFSs are suited to represent the
multi-object environment to be estimated. This section introduces RFSs with
important properties and which will be used in the course of this thesis.
Bernoulli Random Finite Set
A Bernoulli RFS is empty with probability 1 − r or occupied by one object with
probability r [Mah07a; Ris13]. Consequently, r is referred to as existence probability.
If the Bernoulli RFS is occupied, the spatial distribution of the object is given by
the PDF p(x). Accordingly, the MPDF of a Bernoulli RFS is defined by
(
1−r
if X = ∅,
π(X) =
(2.35)
r · p(x) if X = {x}.
The PHD D of the given Bernoulli RFS is
D(x) = r · p(x).
(2.36)
Independent Identically Distributed Cluster Random Finite Set
The MPDF of an independent identically distributed (i.i.d.) cluster RFS with
X = {x(1) , . . . , x(n) } is given by [Mah07a; Ris13]
π(X) = n! · ρ(n) · p(x(1) ) . . . p(x(n) ),
(2.37)
with the cardinality probability distribution ρ(n), n ≥ 0 and the PDF p(x(i) ), which
represents the spatial distribution of the ith object.
Poisson Random Finite Set
A Poisson RFS is an i.i.d. cluster RFS with Poisson distributed cardinality distribution [Mah07a]
ρ(n) = e−λ ·
λn
,
n!
(2.38)
2.2 Multi-Object Tracking and Random Finite Set Statistics
15
where λ is the expected cardinality. The Poisson RFS is commonly used to model
multi-object birth processes or multi-object clutter measurements as will be shown
in Section 2.2.3.
Multi-Bernoulli Random Finite Set
A multi-Bernoulli RFS X describes a set of objects which are independent of each
other [Mah07a]. It is uniquely defined by the parameter set {(r(i) , p(i) )}M
i=1 , where r
is the existence probability of an object and p is the PDF of its dynamic state, also
called spatial distribution. The parameter M is the maximum possible number of
objects in the multi-Bernoulli RFS.
The MPDF π(X) is defined as follows. The probability π(∅) for the special case that
no object exists is
π(∅) =
M
Y
(1 − r(i) ).
(2.39)
i=1
The MPDF π for the RFS {x(1) , . . . , x(n) } is given by [Mah07a]
π({x(1) , . . . , x(n) }) =
M Y
1 − r(j)
X
1≤i1 6=...6=in
j=1
n
Y
r(ij ) p(ij ) (x(j) )
.
1 − r(ij )
≤M j=1
(2.40)
Here, n ∈ {1, . . . , M } is the cardinality of the the specific RFS {x(1) , . . . , x(n) }, while
M describes the maximum possible number of objects.
The cardinality distribution ρ(n) is given by
ρ(n) =
M Y
j=1
1 − r(j)
X
1≤i1 6=...6=in
n
Y
r(ij )
1 − r(ij )
≤M j=1
(2.41)
for n ∈ {1, . . . , M }, while the probability for the cardinality zero is given by (2.39).
The sum in equation (2.41) considers every possible combination of n out of M
objects. The probability that a specific set of n objects exist is calculated by multiplying their existence probabilities, since the objects are independent of each other.
The probability that none of the other possible objects exists is calculated by multiplying their non-existence probabilities, which is achieved by the combination of the
denominator and the first product in equation (2.41). Equation (2.40) additionally
considers the spatial PDFs of the existing objects.
16
Object Tracking
The PHD of a multi-Bernoulli RFS is defined by [Mah07a]
D(x) =
M
X
r(i) p(i) (x)
(2.42)
i=1
and the expected cardinality of a multi-Bernoulli RFS is given by
N̂ =
M
X
r(i) .
(2.43)
i=1
2.2.3 Multi-Object Filtering
The multi-object Bayes filter represents the dynamic state of multiple objects at
time step k with the RFS
(1)
(n)
Xk = {xk , . . . , xk }.
(2.44)
Note, that both the number of objects nk and the object states xk at time step k
are random and vary over time. The underlying Markov process is defined by the
initial MPDF π0 (X0 ) and the multi-object state transition f+ (Xk+1 |Xk ).
Standard Multi-Object Markov Density
The standard multi-object transition was proposed in [Mah07a]. It is valid under
the following assumptions. Transitions of objects are independent of each other
and are defined by single-object Markov transition densities f+ (xk+1 |xk ). Further,
each object survives independently of all other objects with the state dependent
persistence probability pS (x). The birth model is a Poisson process with the spatial
distribution pb (x) and an expected number λB of new-born objects. The birth
process is independent of the persistent objects. Then, the multi-object transition
density is given by
(i)
f+ (Xk+1 |Xk ) = πb (Xk+1 )πk+1 (∅|Xk )
X Y
(i)
θ
i:θ(i)>0
(θ(i))
(i)
pS (xk )f+ (xk+1 |xk )
(θ(i))
(1 − pS (xk )) · λB pb (xk+1 )
.
(2.45)
2.2 Multi-Object Tracking and Random Finite Set Statistics
17
All possible associations from the set Xk to the set Xk+1 are represented by
θ : {1, . . . , |Xk |} → {0, . . . , |Xk+1 |}. The association θ(i) = 0 stands for the disappearance of an object, whereas θ(i) > 0 means that an object survives.
The MPDF of the birth process is
|Xk+1 |
λB pb (xk ).
(i)
(2.46)
(1 − pS (xk )).
(i)
(2.47)
Y
πb (Xk+1 ) = e−λB
i=1
The probability that no object survives is
|Xk |
πk+1 (∅|Xk ) =
Y
i=1
Standard Multi-Object Likelihood Function
Analogously, [Mah07a] defines the standard multi-object likelihood function. It is
valid under the following assumptions. An object generates an observation at time
step k with the state dependent detection probability pD (xk+1 ). This event is called
a true positive event. For the case of a true positive event, the single-object spatial
likelihood function is denoted gk+1 (zk+1 |xk+1 ). One object can generate at most
one observation. The clutter model is a Poisson process with the spatial distribution
pcl (zk+1 ) and an expected number λcl of clutter observations. The clutter process is
independent of the persistent objects. Then, the standard multi-object likelihood
function is given by
(i)
γk+1 (Zk+1 |Xk+1 ) = πcl (Zk+1 )π(∅|Xk+1 )
X Y
(θ(i))
(i)
θ
i:θ(i)>0
(i)
pD (xk+1 )gk+1 (zk+1 |xk+1 )
(θ(i))
(1 − pD (xk+1 )) · λcl pcl (zk+1 )
.
(2.48)
All possible associations from the set Xk+1 to the set Zk+1 are represented by
θ : {1, . . . , |Xk+1 |} → {0, . . . , |Zk+1 |}. The association θ(i) = 0 represents a missed
detection, whereas θ(i) > 0 characterizes a true positive case.
The MPDF of the clutter process is
|Zk+1 |
−λcl
πcl (Zk+1 ) = e
Y
i=1
(i)
λcl pcl (zk+1 ).
(2.49)
18
Object Tracking
The probability, that none of the objects generates an observation is
|Xk+1 |
π(∅|Xk+1 ) =
Y
(i)
(1 − pD (xk+1 )).
(2.50)
i=1
Multi-Object Bayes Filter
Assume the multi-object density at time step k is given by πk (Xk ). Then, it follows
with the Chapman-Kolmogorov equation that [Mah07a]
Z
π+ (Xk+1 ) = f+ (Xk+1 |Xk )πk (Xk )δXk .
(2.51)
The multi-object transition density f+ (Xk+1 |Xk ) considers possible cardinality
changes as well as the dynamic process of objects. Recall that the short notation index "+" expresses a prediction step from time step k to time step k + 1, often
noted as k + 1|k.
Analogously to the single-object case, applying Bayes’ rule to update the state
estimation with the set of measurements Zk+1 leads to
πk+1 (Xk+1 |Zk+1 ) = R
γk+1 (Zk+1 |Xk+1 )π+ (Xk+1 )
,
γk+1 (Zk+1 |Xk+1 )π+ (Xk+1 )δXk+1
(2.52)
R
considering definition (2.31) of the set integral π(X)δX. The multi-object likelihood
function γk+1 (Zk+1 |Xk+1 ) models the spatial uncertainty of measurements as well
as possible missed detections and false alarms.
Several realizations of the multi-object Bayes filter have been presented in literature,
e.g. SMC methods [Mah07a; RWW+ 13; VSD05] or approaches based on parametric
state representation forms [RVVD14; VV13; VVP14].
Two forms of multi-object Bayes filters will be used in the thesis, which are the
Bernoulli filter [Ris13] and the probability hypothesis density (PHD) filter [Mah03;
Ris13; VM06]. Both will be outlined in the following section.
2.2 Multi-Object Tracking and Random Finite Set Statistics
19
2.2.4 Bernoulli Filter
The Bernoulli filter is the exact solution of the estimation of a Markov process
switching between occupied and free as described by the Bernoulli RFS (2.35)
[Ris13]. It propagates the existence probability rk and the spatial distribution pk (xk )
over discrete time steps k.
The transition MPDF is


if Xk+1 = ∅,
1 − pB
f+ (Xk+1 |Xk = ∅) = pB · pb (xk+1 ) if Xk+1 = {xk },


0
if |Xk+1 | ≥ 2,
(2.53)


if Xk+1 = ∅,
1 − pS
f+ (Xk+1 |Xk = {xk }) = pS · f+ (xk+1 |xk ) if Xk+1 = {xk },


0
if |Xk+1 | ≥ 2.
(2.54)
and
Here, pB is the probability for a new-born object, given no object existed in the
previous time step. The PDF pb (x) describes the spatial distribution of a new-born
object. The survival probability is pS and f+ (xk+1 |xk ) is the single-object Markov
transition density. The transition MPDF of the Bernoulli filter differs slightly from
the standard multi-object Markov density (2.45) since the Bernoulli filter does not
assume a Poisson birth process. The predicted Bernoulli RFS at time step k + 1
results in
r+ = pB · (1 − rk ) + pS · rk ,
(2.55)
R
pB (1 − rk ) · pb (xk+1 ) + pS · rk · f+ (xk+1 |xk )p(xk )dxk
.
p+ (xk+1 ) =
pB · (1 − rk ) + pS · rk
(2.56)
If pS = 1 and rk = 1, (2.56) degenerates to the standard Chapman-Kolmogorov
equation (2.4). The likelihood function of the Bernoulli filter can be directly derived
as a special case of the standard multi-object likelihood function (2.48):

πcl (Zk+1 ) if Xk+1 = ∅,
(i)
P|Zk+1 | gk+1 (zk+1
γk+1 (Zk+1 |Xk+1 ) =
|xk+1 )
πcl (Zk+1 ) 1−pD +pD i=1
if Xk+1 = {x}.
(i)
λcl pcl (zk+1 )
(2.57)
20
Object Tracking
Inserting (2.57) into (2.52) leads to
rk+1 =
1 − δk
r+
1 − δk · r+
(2.58)
1 − pD + pD
(i)
gk+1 (zk+1 |xk+1 )
P|Zk+1 |
pk+1 (xk+1 ) =
i=1
(i)
λcl pcl (zk+1 )
1 − δk
p+ (xk+1 |xk ),
(2.59)
where

δk = pD 1 −
|Zk+1 |
X
i=1
R

(i)
gk+1 (zk+1 |xk+1 )p+ (xk+1 )dxk+1
.
(i)
λcl pcl (zk+1 )
(2.60)
Again, if pD = 1 and the expected number of false alarms is zero, (2.59) reduces to
the standard single-object Bayes update (2.5).
2.2.5 Probability Hypothesis Density Filter
The PHD filter is an approximation of the general RFS multi-object Bayes filter
[Ris13; VM06]. It propagates the first order statistical moment Dk of an RFS.
The prediction step is
Z
D+ (xk+1 ) = γb (xk+1 ) + pS
f+ (xk+1 |xk )Dk (xk )dxk ,
(2.61)
where γb (xk+1 ) is the object birth PHD, pS is the survival probability and f+ (xk+1 |xk )
is the single-object Markov transition density.
The update step is
Dk+1 (xk+1 ) = (1 − pD (xk+1 ))D+ (xk+1 )
X
pD (xk+1 )gk+1 (zk+1 |xk+1 )D+ (xk+1 )
R
+
,
κk+1 (zk+1 ) + pD (xk+1 )gk+1 (zk+1 |xk+1 )D+ (xk+1 )dxk+1
zk+1 ∈Zk+1
(2.62)
where κk+1 (zk+1 ) is the clutter PHD at time step k + 1 and pD (xk+1 ) is the detection
probability.
2.2 Multi-Object Tracking and Random Finite Set Statistics
21
Probability Hypothesis Density Filter with Adaptive Birth Intensity
In practice, it is computationally demanding to model object birth PHDs over the
entire state space X. A helpful assumption proposed by [RCVV12] is that newly
born objects are always detected, which is equivalent to assuming pD (xk+1 ) = 1 for
new-born objects. Therefore, the PHD must be separated into two parts.
One part represents the predicted PHD of persistent objects Dp,+ and the other part
represents the predicted PHD of new-born objects Db,+ . The prediction step is then
Db,+ (xk+1 ) = γb (xk+1 ),
Z
Dp,+ (xk+1 ) = pS f+ (xk+1 |xk )Dk (xk )dxk ,
(2.64)
D+ = Db,+ + Dp,+ .
(2.65)
(2.63)
The update step can be divided into two parts analogously: Dk+1 = Db,k+1 + Dp,k+1 .
It is given by
Dp,k+1 (xk+1 ) = (1 − pD (xk+1 )) Dp,+ (xk+1 )
X
pD (xk+1 )gk+1 (zk+1 |xk+1 ) Dp,+ (xk+1 )
+
,
Q(zk+1 )
(2.66)
zk+1 ∈Zk+1
Db,k+1 (xk+1 ) =
X
zk+1 ∈Zk+1
gk+1 (zk+1 |xk+1 ) γb (xk+1 )
,
Q(zk+1 )
(2.67)
where
Z
Q(zk+1 ) = κk+1 (zk+1 ) + gk+1 (zk+1 |xk+1 )γb (xk+1 )dxk+1
Z
+ pD (xk+1 ) gk+1 (zk+1 |xk+1 ) Dp,+ (xk+1 )dxk+1 .
(2.68)
SMC Realization of the Probability Hypothesis Density Filter with
Adaptive Birth Intensity
A common realization of the PHD filter is the sequential Monte Carlo probability
hypothesis density (SMC-PHD) filter [Ris13; Sid03; VSD05]. It represents the
22
Object Tracking
(i)
(i)
k
by
posterior PHD Dk with a set of νk particles and their weights {xk , wk }νi=1
Dk =
νk
X
(i)
(i)
wk δ(xk − xk ).
(2.69)
i=1
Inserting (2.69) into (2.34) shows that the sum of particle weights equals the expected
number of objects N̂ of the estimated RFS:
νk
X
(i)
wk = N̂ .
(2.70)
i=1
Prediction
The sequential Monte Carlo probability hypothesis density (SMC-PHD) filter first
predicts the state of persistent objects at time step k + 1 by sampling a number
of νk particles based on the posterior particle set of the former time step k. The
particle set representing predicted persistent objects at time step k + 1 is denoted
(i)
(i)
k
{xp,+ , wp,+ }νi=1
.
(2.71)
The samples are drawn from the proposition density q. The following assumes it
is possible to sample from the single-object Markov transition density f+ (xk+1 |xk ),
which is chosen as proposition density here:
(i)
(i)
(i)
(i)
qk+1 (xk+1 |xk , zk+1 ) = f+ (xk+1 |xk ).
(2.72)
This corresponds to the prediction step of the SIR method of a particle filter in the
single-object case. In contrast to the single-object particle filter, the SMC-PHD
filter considers the persistence probability of objects in the prediction step, which is
carried out by adapting weights of the predicted particle set:
(i)
(i)
(i)
wp,+ = pS (xk )wk .
(2.73)
The SMC-PHD filter represents new-born objects with the particle set
(i)
(i)
ν
b,k+1
{xb,+ , wb,+ }i=1
.
(2.74)
This particle set is calculated taking the observation set Zk+1 into account. In order
to draw particles representing a new-born object, the SMC-PHD filter calculates
the object state distribution pb,zk+1 conditioned on a single-object observation
zk+1 ∈ Zk+1 , which is denoted pb,zk+1 (xk+1 |zk+1 ). Usually, assumptions about prior
2.2 Multi-Object Tracking and Random Finite Set Statistics
23
distributions are necessary in order to model pb,zk+1 (xk+1 |zk+1 ), especially if the
observation zk+1 is independent of at least some object state elements of xk+1 . For
further information, refer to [Ris13], p. 38.
When a measurement set Zk+1 arrives, the SMC-PHD filter draws the number of Mb
samples for each measurement zk+1 from the PDF pb,zk+1 (xk+1 |zk+1 ). This leads to
a total number of
νb,k+1 = Mb · |Zk+1 |
(2.75)
particles representing new-born objects. In order to weight these particles, the
SMC-PHD filter applies the design parameter N̂b,k+1 , which is the expected number
of object births at time step k + 1. The weights are then equally normalized to
(i)
wb,+ =
N̂b,k+1
.
νb,k+1
(2.76)
As a conclusion, the prediction step of the SMC-PHD filter creates two particle sets.
One represents persistent objects, the other represents new-born objects.
Update
The update step is carried out by recalculating the individual weights of both particle
sets. Resulting from (2.66), the updated weights of the particles representing the
persistent objects are given by
(i)
(i)
(i)
wp,k+1 = (1 − pD (xp,+ )) wp,+
(i)
+
X
zk+1 ∈Zk+1
(i)
(i)
pD (xp,+ )gk+1 (zk+1 |xp,+ ) wp,+
.
Q(zk+1 )
According to (2.67), the updated weights of the particles representing the new-born
objects are
(i)
(i)
wb,k+1 =
X
zk+1 ∈Zk+1
wb,+
Q(zk+1 )
,
24
Object Tracking
where (2.68) leads to
νb,k+1
Q(zk+1 ) = κk+1 (zk+1 ) +
X
(i)
wb,+
i=1
+
νk
X
(i)
(i)
(i)
pD (xp,+ )gk+1 (zk+1 |xp,+ ) wp,+ .
(2.77)
i=1
Resampling
The SMC-PHD filter resamples the particle set representing the persistent objects.
The estimated number of persistent objects at time step k + 1 is
N̂p,k+1 =
νk
X
(i)
wp,k+1 .
(2.78)
i=1
Let Mp be the design parameter for the number of particles the SMC-PHD filter
uses per persistent object. Then the SMC-PHD filter resamples a number of
νp,k+1 = bMp · N̂p,k+1 e
(2.79)
particles from the posterior persistent particle set. The operator b·e provides the
nearest integer. There is no need to resample the particle set representing new-born
objects.
Finally, both particle sets can be joined to one set, because in the transition from
time step k + 1 to k + 2 it will not make a difference, if an object was born at time
step k + 1 or persistent from time step k to k + 1. The joint set then holds a number
of νk+1 = νp,k+1 + νb,k+1 particles.
Chapter 3
Grid Mapping
An important ability of mobile robots is distinguishing between free space and
occupied areas. Occupancy grid maps separate the robot environment into single
grid cells and estimate the occupancy probability of each grid cell individually based
on sensor measurements [Elf89; TBF05]. Grid maps play an important role in vehicle
environment perception and for autonomous driving [DRN15; KNW+ 15; NWW+ 13;
Urm+ 08]. Classical grid maps assume a static environment, which simplifies the
estimation problem but leads to estimation errors in dynamic traffic scenarios.
During recent years, new grid mapping techniques emerged which consider a dynamic
environment [CPL+ 06; DON11; NRL14; NYK+ 15; TTWB14]. However, due to
a more complex process model, the approaches are also computationally more
demanding. This chapter gives an introduction to classical, static grid mapping
approaches in the first part (3.1) and to the state of the art in dynamic grid mapping
techniques in the second part (3.2).
3.1 Static Grid Mapping
Classical grid maps discretize space by dividing the surrounding area into single grid
cells. Generally, it is possible to store any collected information about a grid cell
like semantic information [NTDD14] or elevation [DN13]. Original occupancy grid
maps estimate the occupancy probability of each grid cell. Two strong assumptions
play an important role: the first assumption is that the environment is static, the
second assumption is that the state of a grid cell is independent of all other grid
cell states. Both assumptions are not met in most applications, but lead to strong
simplifications of the state estimation problem. A detailed introduction to static
grid mapping is given in [DRN15].
26
Grid Mapping
The process of generating a static grid map can be separated into three steps:
• ego motion compensation,
• measurement grid calculation,
• grid map update.
In the first step, the movement of the robot or vehicle over ground is compensated.
The second step extracts information about each individual grid cell based on sensor
measurements and the third step updates the state of each grid cell. All three steps
are outlined in the following sections.
3.1.1 Ego Motion Compensation with Dead Reckoning
Grid maps can relate to different coordinate systems, e.g. a vehicle coordinate
system (related to the ego vehicle), a global coordinate system (related to common,
global coordinates) or a local, stationary coordinate system. The introduced method
defines a local, stationary coordinate system as a basis for the grid map. It considers
a two-dimensional environment, but the approach is easily extendable to a threedimensional coordinate system. The method estimates the position of the vehicle in
the local, stationary coordinate system by dead reckoning. In the exemplary case
this means integrating the velocity and yaw rate of the vehicle as described in the
following.
Assume the vehicle pose p1 at time t1 is given by [x1 y1 ψ1 ]T as an arbitrary pose
in the local, stationary coordinate system. Here, x and y are the two-dimensional
coordinates and ψ is the heading orientation of the vehicle in a specific vehicle
reference point, e.g. the center of the rear axle as depicted in Figure 3.1. Assume
further that the vehicle’s heading direction and yaw angle are identical in the vehicle
reference point, which is equivalent to assuming the slip angle is zero in the vehicle
reference point. A sensor measurement is taken at time t2 . The system measures
the speed v and the yaw rate ψ̇.
Then the change of the vehicle pose is given by
  

ẋ
v · cos(ψ)
 ẏ  =  v · sin(ψ)  ,
ψ̇
ψ̇
(3.1)
3.1 Static Grid Mapping
27
y
ψ2
y2
ψ1
y1
x1
x2
x
Figure 3.1: Vehicle pose p1 at time t1 and vehicle pose p2 at time t2 [DRN15],
c 2015 Springer.
so the vehicle pose p2 at time t2 calculates to


Zt2 ẋ(t)
p2 = p1 +  ẏ(t)  dt.
ψ̇(t)
t1
(3.2)
If the vehicle travels on a circular path during the time interval ∆t = t2 − t1 > 0
6 0, then the integral (3.2) is
with constant velocity v and constant yaw rate ψ̇ =
solved by
 v

sin(ψ1 + ψ̇∆t) − sin(ψ1 )
ψ̇


p2 = p1 +  ψ̇v − cos(ψ1 + ψ̇∆t) + cos(ψ1 )  .
(3.3)
ψ̇∆t
If the vehicle moves straight with ψ̇ = 0, then the integral (3.2) is solved by


v∆t cos(ψ1 )
p2 = p1 +  v∆t sin(ψ1 )  .
(3.4)
0
Applying the dead reckoning equations (3.3) and (3.4) allows to estimate the vehicle
position in the grid map based on measurements of vehicle speed and yaw rate.
28
Grid Mapping
Depending on the application, dead reckoning is not necessarily a good choice for ego
motion compensation. Over the time, small measurement errors and the vehicle drift
integrate to a constantly growing error in the pose estimation. For map generation
of huge areas, the use of external positioning systems like global navigation satellite
system (GNSS) might be a better choice.
However, dead reckoning offers the advantage that the resulting position estimation
is smooth or in other words: free of position skips. For the estimation of the current
vehicle environment in a short range (usually smaller than 250 m), smoothness is
more important than avoiding drift effects. Therefore, dead reckoning is a common
choice for ego motion compensation.
3.1.2 Measurement Grid Calculation
Generally, two streams of grid mapping can be distinguished: grid mapping with
forward sensor models and grid mapping with inverse sensor models [Thr03]. The
following section first describes the use of a forward sensor model for occupancy grid
maps and outlines the difference to the inverse sensor model in the second part.
Forward Sensor Model for Occupancy Grid Maps
In a classical occupancy grid map, the binary grid cell state ok at time k is considered
either occupied or free: ok ∈ {O, F }. The forward sensor model estimates for each
grid cell the occupancy likelihood density gk+1 (zk+1 |ok+1 ) for the two feasible occupancy events ok+1 ∈ {O, F } [Thr03]. The occupancy and the free space likelihood
are density functions in regard to the measurement zk+1 , their physical unit is a
density in the measurement space. Modeling likelihoods is more complicated than
designing inverse sensor models and usually also computationally more expensive.
However, the forward sensor model directly leads to the common Bayesian update
step as will be outlined in Section 3.1.3.
Inverse Sensor Model for Occupancy Grid Maps
An inverse sensor model assigns each grid cell an individual, discrete binary occupancy
probability pzk+1 (ok+1 |zk+1 ) based on the measurement zk+1 at time k + 1. The
result is called a measurement grid.
3.1 Static Grid Mapping
29
Figure 3.2: Occupancy probabilities of two-dimensional grid cells reasoning
on a multi-beam laser range measurement. Grid cells with a
high probability of being occupied are colored black, free grid
cells are marked white. Grid cells with an unknown state (same
probability for both occupied and free) are displayed in gray
c 2015 Springer.
[DRN15], To give a practical example, consider a laser range measurement consisting of
several laser beams and the resulting measurement grid as depicted in Figure 3.2.
Grid cells between the sensor and a reflection are assigned a reduced occupancy
probability pzk+1 (ok+1 |zk+1 ) < 0.5, whereas grid cells near the reflection are assigned
an increased occupancy probability pzk+1 (ok+1 |zk+1 ) > 0.5. The state of grid
cells which are occluded or outside the field of view is considered unknown with
pzk+1 (ok+1 |zk+1 ) = 0.5.
Generally, inverse sensor models are much more complicated than the given example
depicted in Figure 3.2. Usually, they assign varying probability values to grid cells,
which depend on different factors. Figure 3.3 shows an example of an inverse sensor
model for a single laser beam, where the assigned occupancy probabilities depend
on the distance of the measurement. The inverse sensor model can be a heuristic
model or the result of a machine learning process [TBF05].
The measurement grid calculation step combines information from individual laser
beams of time step k + 1 and creates a joint measurement grid for time step k + 1.
Furthermore, the update step combines measurement grids from different time steps
to accumulate the gathered information about individual grid cells. A way to combine
information in grid cells is the binary Bayes filter (BBF), which will be outlined in
the next section.
30
Grid Mapping
p(o)
1
p(o)
1
0,5
r
0
0
Figure 3.3: Inverse sensor model for a single laser beam. The uncertainty of
the state estimation increases with the distance of the measurec 2015 Springer.
ment [DRN15], 3.1.3 Grid Map Update
Due to the assumption of a static environment, classical grid mapping does not
include an explicit prediction step. The predicted occupancy probability at time
k + 1 simply equals the last posterior occupancy probability po,k (ok ) at time k.
Some approaches consider the possibility that a grid cell switches its state [NWW+ 13],
sometimes referred to as fading memory. In this case, the prediction step discounts
the information in a grid cell depending on the time difference.
Basis for the update step of grid maps is the binary Bayes filter (BBF). It is the
special case of the Bayes filter where the state space consists of only two discrete
states. In case of an occupancy grid cell, these states are occupied and free. A basic
principle is that each grid cell applies the BBF individually, based on the assumption
that its state is independent of other grid cell states. Thus, the following update
equations relate to one individual grid cell.
The BBF differs slightly when applied to forward sensor models or inverse sensor
models. However, both cases can be expressed jointly with the generalized BBF,
which is a contribution of this thesis. The three variants of the BBF will be outlined
in the following sections.
3.1 Static Grid Mapping
31
Binary Bayes Filter for Forward Sensor Models
In case of a forward sensor model, the BBF can directly be expressed as a special
case of the Bayes update (2.5) with a discrete, binary state. The posterior occupancy
probability po,k+1 (ok+1 ) at time k + 1 of a grid cell results from its last posterior
occupancy probability po,k (ok ) at time k and its likelihood density gk+1 (zk+1 |ok+1 )
through
po,k+1 (ok+1 ) =
gk+1 (zk+1 |ok+1 ) · po,k (ok )
,
gk+1 (zk+1 |ok+1 ) · po,k (ok ) + gk+1 (zk+1 |ok+1 ) · po,k (ok )
(3.5)
where o denotes the counter event of the occupancy state o.
Binary Bayes Filter for Inverse Sensor Models
In case of an inverse sensor model, the update step combines two independent state
estimations instead of combining a state estimation with a measurement likelihood.
Then the occupancy probability po,k+1 (ok+1 ) at time k + 1 of a grid cell results from
its last posterior occupancy probability po,k (ok ) at time k and its measurement-based
estimate pzk+1 (ok+1 |zk+1 ) through [TBF05]
po,k+1 (ok+1 ) =
pzk+1 (ok+1 |zk+1 ) · po,k (ok )
.
pzk+1 (ok+1 |zk+1 ) · po,k (ok ) + pzk+1 (ok+1 |zk+1 ) · po,k (ok )
(3.6)
Equation (3.6) holds if the prior probabilities for occupied and free are equal,
measurements from different time steps are independent of each other, and the grid
cell state does not change over time.
Generalized Binary Bayes Filter
Equations (3.5) and (3.6) can be generalized to
po,k+1 (ok+1 ) =
αzk+1 · po,k (ok )
,
αzk+1 · po,k (ok ) + po,k (ok )
(3.7)
32
Grid Mapping
where αzk+1 is the single measurement-based occupancy probability ratio
αzk+1 =
pzk+1 (ok+1 |zk+1 )
, pzk+1 (ok+1 |zk+1 ) > 0,
pzk+1 (ok+1 |zk+1 )
(3.8)
gk+1 (zk+1 |ok+1 )
, gk+1 (zk+1 |ok+1 ) > 0,
gk+1 (zk+1 |ok+1 )
(3.9)
or the likelihood ratio
αzk+1 =
respectively. As a conclusion, the binary Bayes filter requires either a likelihood ratio
or a probability ratio for the update step. It will be shown later in Section 4.3 that
the binary Bayes filter (3.7) is a special case of the presented probability hypothesis
density / multi-instance Bernoulli (PHD/MIB) filter, namely for the assumption of
zero velocity in a deterministic process model.
3.2 Dynamic Grid Mapping
The assumption of a stationary environment is not realistic for typical traffic scenarios.
Therefore, Coué et al. proposed the Bayesian occupancy filter (BOF) [CPL+ 06] for
dynamic environments. The BOF considers a four-dimensional space, consisting of a
two-dimensional position and a two-dimensional velocity and separates this space
into grid cells. One grid cell represents a certain area and a certain velocity interval.
Consequently, the BOF estimates the dynamic state of the vehicle surrounding by
propagating the occupancy probability of every grid cell.
Since the BOF is computationally demanding, Gindele et al. suggested to propagate
the dynamic state of grid cells using a Gaussian mixture representation [GBSD09]. As
another alternative, Danescu et al. presented a particle-based approach for dynamic
grid maps [DON11]. Particles offer the advantage of a highly efficient parallel
implementation, as will be outlined in detail in Chapter 6. In 2014 and 2015, three
contributions of Tanzmeister et al. [TTWB14], Nègre et al. [NRL14] and the author
et al. [NYK+ 15] adopted Danescu’s approach and suggested variations. This section
gives an introduction to the state of the art and summarizes and compares all four
publications on particle-based dynamic grid maps.
3.2 Dynamic Grid Mapping
33
3.2.1 State of the Art in Dynamic Grid Mapping using
Particles
All mentioned publications on particle-based dynamic grid maps estimate the occupancy probability and the dynamic state of grid cells in the vehicle environment.
Further, all publications describe an algorithm consisting of a prediction and an
update step as depicted in Figure 3.4 and apply a resampling step to avoid degeneration.
State Representation
All mentioned publications represent the dynamic state of grid cells with particles,
but the interpretation of a particle differs: [DON11] and [NYK+ 15] directly use the
number of particles or the sum of particle weights in a grid cell as a measure for the
occupancy probability of the grid cell. In contrast, [NRL14] propagates an additional
discrete probability distribution for the events free, static occupancy and dynamic
occupancy for each grid cell. The particles then represent a velocity distribution for
the dynamic case. The same events are used in [TTWB14] within a Dempster-Shafer
framework [Dem68]. Details about the Dempster-Shafer theory of evidence (DST)
will be outlined in Section 5.2.1. To avoid aliasing problems, particles represent
velocity and position of an occupancy in a grid cell in all mentioned publications, so
the dynamic state of a grid cell is four-dimensional.
Prediction Step
All mentioned publications assume a process model with constant velocity and
constant direction and propagate each single particle accordingly. All particles that
are predicted into a certain grid cell represent the predicted dynamic state of the
grid cell. However, the exact quantitative reasoning about the resulting predicted
occupancy probability varies. Intuitively, the higher the number of particles or
particle weights predicted into a grid cell, the higher is the predicted occupancy
probability. An example is depicted in Figure 3.4b.
Update Step
Updating the occupancy probability of a grid cell with a measurement grid is
generally a binary Bayes problem and solved either by equation (3.6) or (3.5) or by
34
Grid Mapping
(a) Posterior at k
(b) Prediction for k + 1
(c) Measurement grid at k + 1
(d) Posterior at k + 1
Figure 3.4: Different states of the dynamic grid map estimation recursion.
equivalent update steps in the Dempster-Shafer framework [TTWB14], [NYK+ 15].
The estimated occupancy probability of a grid cell is used as a basis to normalize
the sum of weights of particles in one grid cell. This means that weights of particles
in one grid cell are all updated by the same normalization factor during one update
step.
An exception is [NYK+ 15], which suggests to fuse radar and laser measurements.
Radar Doppler measurements provide an individual likelihood density depending on
the velocity of a particle state. Consequently, [NYK+ 15] extends the update step by
individually updating particle weights even inside one grid cell. After the individual
particle weight update, [NYK+ 15] also normalizes the overall particle weight in one
grid cell represent the posterior occupancy probability of the cell.
3.2 Dynamic Grid Mapping
35
Resampling
All mentioned publications apply a resampling step to avoid degeneration. Similar
to classical particle filters, the resampling step chooses to eliminate some particles
and reproduce others instead, based on their weight. After the resampling step, all
particles are assigned the same weight.
Initialization
If a measurement grid cell provides a high occupancy probability (or occupancy
likelihood, in the forward case), but no particles were predicted into the corresponding
grid cell, new particles must be initialized to represent the dynamic state of the grid
cell. The initial distribution depends on the environment setting, but usually the
velocity of objects is limited, e.g. by the maximum speed of a vehicle.
Neither [DON11] nor [TTWB14] describe the initialization step to any further detail
than mentioned here. In a realistic scenario, a grid cell is not either empty or fully
populated, but mostly something in-between. Then, the question arises how to
divide the weight between predicted and initialized particles. Intuitively, the weight
for newly initialized particles should rise with increasing measurement occupancy
and decreasing predicted occupancy. Heuristic examples are provided by [NRL14]
and [NYK+ 15].
Occluded Areas
In practical applications, a grid map contains a high ratio of occluded and therefore
unobserved grid cells. Populating unknown areas of the grid map with particles
would result in a huge computational load. To avoid this, all mentioned publications
only initialize particles in grid cells with a certain measured occupancy probability.
3.2.2 Discussion
The discussed particle-based dynamic grid maps show promising results. However,
from a theoretical point of view many open questions remain. A prerequisite for
Bayesian state estimation is the definition of a state space, a stochastic process
describing the state transition and a stochastic observation process. All mentioned
papers directly describe the propagation of particles without defining the estimation
36
Grid Mapping
problem first. As a result, it is unclear what a particle represents. All mentioned
publications explain that a particle represents a hypothesis for the dynamic state
of an individual grid cell. However, during the prediction step, particles from
various cells are predicted into another grid cell and jointly represent the state of
the destination cell. The particles are not assigned to a specific object, instead they
represent a hypothesis for the existence and state of a whole group of objects.
In other words, the particles jointly represent a set of occupied grid cells, where
the number of occupied grid cells is a random process itself and must be estimated
too. This cannot be explained with single-object Bayesian estimation theory. As
a consequence, previous work cannot motivate prediction or update equations for
a well-defined estimation problem. Especially the initialization of new particles
remains unclear.
From a theoretical point of view, an environment containing a random but limited
number of objects is a random finite set (RFS). The next chapter presents a concept
which uses finite set statistics (FISST) to describe a grid map for state estimation in
dynamic environments.
Chapter 4
Dynamic Grid Mapping as a
Random Finite Set Problem
The chapter presents a new method which describes dynamic grid mapping as a
random finite set problem. The finite set theory facilitates the combination of the
Bernoulli filter [Ris13] and the PHD filter [Ris13; VM06] in order to recursively
estimate the state of dynamic grid cells. These filters cover fundamental problems
like object initialization or modeling of heterogeneous measurements in an elegant
way. The result is a recursion called probability hypothesis density / multi-instance
Bernoulli (PHD/MIB) filter.
The first part of the chapter defines the problem setting and outlines the PHD/MIB
filter recursion. The second part derives the filter equations and the third part
explains the relation to the classical binary Bayes filter (BBF) for static grid mapping.
A discussion at the end of the chapter summarizes the most important contributions.
4.1 Problem Setting
Intuitively, an object in a vehicle environment is a traffic participant, e.g. a pedestrian
or another vehicle. For static obstacles, it is unclear how one object is separated from
another. Static grid maps don’t facilitate a concept of objects. However, in order to
model the environment as a random finite set, an object definition is required. The
next section defines the multi-object state of a vehicle or robot environment as it is
estimated by the PHD/MIB filter.
38
Dynamic Grid Mapping as a Random Finite Set Problem
Occupied Grid Cell
Point Object
Free Grid Cell
Traffic Participants
c
c
c
Stationary Obstacle
c
c
c
Figure 4.1: Exemplary visualization of point objects. Left: Real-world
objects and corresponding point objects. Right: The number of
point objects per real-world object equals the number of grid
cells occupied by the real-world object.
4.1.1 Environment Definition
The PHD/MIB filter estimates a random finite set consisting of so-called point
objects. The relation between point objects and real-world objects depends on
an underlying grid map and is shown in Figure 4.1. A real-world object consists
of at least one but possibly several point objects. The number of corresponding
point objects per real-world object equals the number of grid cells occupied by the
real-world object.
To provide an example for the state x of a point object, let x be a two-dimensional
position and a two-dimensional velocity:
x = [px py vx vy ]T .
(4.1)
However, the object state can be arbitrarily extended and can include additional
attributes such as object height, color, semantic class, etc.
4.1 Problem Setting
39
PHD Representation of Joint
RFS of Point Objects
PHD Prediction Step for
Persistent Point Objects
Approximation as Bernoulli
RFS for Grid Cell 1
…
Approximation as Bernoulli
RFS for Grid Cell C
Bernoulli Prediction Step for
New-Born Object
…
Bernoulli Prediction Step for
New-Born Object
Bernoulli Update Step
…
Bernoulli Update Step
Transformation to PHD
Representation of Joint RFS
of Point Objects
Figure 4.2: Processing scheme of the PHD/MIB filter. Here, C is the overall
number of grid cells.
4.1.2 Filter Recursion Outline
The goal of the PHD/MIB filter is to estimate the multi-object state of the point
objects in a vehicle environment, which includes an estimate of the occupancy state
of grid cells, as will be explained below. In the course of the filter recursion, the
PHD/MIB filter represents the random finite set of point objects in different forms.
An overview of the filter recursion is depicted in Figure 4.2.
The filter represents the posterior multi-object state by its PHD. Thus, the prediction
step simply applies the standard PHD filter prediction. In order to update the
predicted RFS state with a measurement grid, the PHD/MIB filter approximates the
point object state in each grid cell as a Bernoulli RFS and carries out the update step
independently for each grid cell. Finally, the PHD/MIB filter transfers all instances
of Bernoulli sets to a joint PHD to represent the posterior state. The individual
filter steps are detailed in the following section.
40
Dynamic Grid Mapping as a Random Finite Set Problem
Point Object
Traffic Participants
Stationary Obstacle
Figure 4.3: Exemplary PHD of a traffic situation. Here, only the subspace
of the two-dimensional position is visualized.
4.2 Probability Hypothesis Density /
Multi-Instance Bernoulli Filter
The following sections derive the prediction and update equations of the PHD/MIB
filter.
4.2.1 Prediction Step of the MIB/PHD Filter
The PHD/MIB filter expresses the posterior multi-object state of time step k by its
PHD
Dk (xk ) : R4 → R.
(4.2)
In practical applications, PHDs are commonly represented by particles or Gaussian
mixtures. However, the following derivation is independent of its practical realization.
Figure 4.3 shows contour lines of an exemplary PHD of point objects in a typical
traffic situation.
4.2 Probability Hypothesis Density / Multi-Instance Bernoulli Filter
41
Multi-Object State Transition in PHD Representation Form
The birth process of point objects is defined by the birth PHD γb (xk+1 ). The
persistence probability of each point object is denoted by pS . The standard prediction
step of a PHD filter is then given by [Mah03]
Z
D+ (xk+1 ) = γb (xk+1 ) + pS f+ (xk+1 |xk )Dk (xk )dxk ,
(4.3)
where f+ (xk+1 |xk ) is the single-object transition density. An example for a process
model which defines a transition density is the constant velocity, constant orientation
(CVCO) model given by


1 0 T 0
0 1 0 T 

xk+1 = 
(4.4)
0 0 1 0  xk + ξk ,
0 0 0 1
where ξk is the process noise and T is the time interval between k and k + 1.
Since new-born objects will be handled in Bernoulli form, the prediction step of the
PHD/MIB filter is only required to consider persisting objects and consequently
simplifies to
Z
Dp,+ (xk+1 ) = pS f+ (xk+1 |xk )Dk (xk )dxk .
(4.5)
Here, the lower index p symbolizes the affiliation to persistent objects.
Approximation as Bernoulli Distributions
The PHD/MIB filter approximates the predicted PHD Dp,+ (xk+1 ) of persisting
objects by multiple instances of independent Bernoulli RFSs, one for each grid cell.
The interpretation is that each grid cell can either be occupied (a point object exists
in the grid cell) or free (no point object exists in the grid cell). Each Bernoulli RFS
instance models the possibility of object birth and the observation process including
clutter. The birth process model is identical to the standard Bernoulli filter. The
clutter process is different and will be derived for the special form of a measurement
grid. The following part describes the Bernoulli filter steps for a single grid cell
denoted by c.
42
Dynamic Grid Mapping as a Random Finite Set Problem
The predicted Bernoulli RFS for a persistent point object in grid cell c is given by

r(c)
if Xk+1 = ∅,
p,+
(c)
πp,+ (Xk+1 ) =
(4.6)
r(c) · p(c) (x
p,+
p,+ k+1 ) if Xk+1 = {xk+1 },
(c)
where rp,+ is the predicted existence probability of a persistent point object with
(c)
(c)
the counter event probability rp,+ = 1 − rp,+ . The predicted PDF of the point
(c)
object’s state is pp,+ (xk+1 ). The upper index (c) denotes values which are different
for individual cells.
(c)
From the relation between the existence probability rp,+ of a Bernoulli RFS and its
PHD (2.36) it follows that


Z


(c)
rp,+ = min 
Dp,+ (xk+1 )dxk+1 , pS  ,
(4.7)
xk+1 ∈c
where xk+1 ∈ c stands short for the set {xk+1 |(xk+1 ∈ c)}. It is the subset of R4
associated to grid cell c. Here, the limitation to a maximum value of the persistence
probability pS is a required approximation since the PHD prediction does not consider
that each grid cell cannot be entered by more than one point object.
Further, the spatial distribution of the predicted potential point object within the
grid cell can also directly be derived from (2.36) and results in
(c)
pp,+ (xk+1 ) =
Dp,+ (xk+1 )
(c)
rp,+
(c)
for all xk+1 ∈ c and rp,+ > 0.
(4.8)
Bernoulli RFS Birth Model
The standard Bernoulli filter defines the following birth model [Ris13]: If no object
exists at time step k, the existence probability of a new-born object at time step
k + 1 is given by the prior birth probability pB . If an object exists at time step k,
the existence probability of a new-born object at time k + 1 is zero, independent of
the survival of the old object. In the case of a new-born object, the single-object
spatial birth PDF is given by pb (xk+1 ). The choice of an appropriate birth model
depends of the environment to be estimated. The velocity variance of the spatial
birth distribution pb (xk+1 ) will typically be smaller for indoor applications than for
applications on a highway.
4.2 Probability Hypothesis Density / Multi-Instance Bernoulli Filter
43
The PHD/MIB filter assumes the same birth process as the standard Bernoulli filter.
(c)
This results in the predicted existence probability rb,+ of a new-born object with 1
(c)
(c)
rb,+ = pB · 1 − rp,+ .
(4.9)
Considering both cases of a persistent or new-born object leads to the predicted
Bernoulli RFS
(
(c)
(c)
1 − rb,+ − rp,+
if Xk+1 = ∅,
(c)
(4.10)
π+ (Xk+1 ) =
(c)
(c)
(c)
rb,+ · pb (xk+1 ) + rp,+ · pp,+ (xk+1 ) if Xk+1 = {xk+1 }.
(c)
Note that the predicted Bernoulli RFS π+ (Xk+1 ) represents both the predicted
dynamic distribution and the predicted occupancy probability of the corresponding
grid cell.
(c)
The predicted occupancy probability po,+ (Ok+1 ) equals the combined predicted
(c)
existence probability r+ of a persistent or a new-born point object in the grid cell,
so by definition of the Bernoulli RFS it follows:
(c)
(c)
(c)
(c)
po,+ (Ok+1 ) = r+ = rb,+ + rp,+ .
(4.11)
As a result of the prediction step, each grid cell contains the predicted Bernoulli
(c)
RFS π+ (Xk+1 ), describing both the existence probability of a potential point object
in the cell and the predicted distribution of its dynamic state.
4.2.2 Update Step of the PHD/MIB Filter
The goal of the update step is to combine the information contained in the predicted
Bernoulli RFS at time step k + 1 with the information contained in a sensor
measurement at time step k + 1. As a basic principle, static occupancy grid maps
estimate individual grid cell states independently of each other. The PHD/MIB
filter follows this principle as well. The following section recalls the processing chain
of sensor data in static occupancy grid maps.
1 Formally,
the probability of a birth event in time step k + 1 depends on the existence probability
of an object at time step k for the Bernoulli filter [Ris13]. For simplicity, equation (4.9) neglects
that fact and directly relates to the predicted existence probability for time step k + 1. This is
appropriate for process models with a high persistence probability (pS ≈ 1).
44
Dynamic Grid Mapping as a Random Finite Set Problem
Measurement Data Preprocessing in Static Occupancy Grid Maps
Figure 4.4 visualizes a common sensor data processing scheme of static occupancy
grid maps. Assume a certain group of measurements provides information about
the environment at time step k + 1. In this example the group consists of three
laser reflections, shown as red beams in Figure 4.4. Each of the three laser beams is
assumed to be an independent measurement zi,k+1 , i ∈ {1, 2, 3}. The inverse laser
measurement model processes one laser beam at a time as introduced in Section 3.1.2.
(c)
It assigns to each grid cell c the individual occupancy probability pzi,k+1 (Ok+1 |zi,k+1 ),
reasoning on the laser beam i. This step produces three different measurement grids,
one for each laser beam as shown in the left column in Figure 4.4. The numbers in
the grid cells state the individual occupancy probabilities.
As a next step, the processing scheme combines the three measurement grids to one
joint measurement grid. Based on the independence assumption of the laser beams, it
(c)
combines the three occupancy probabilities pzi,k+1 (Ok+1 |zi,k+1 ), i ∈ {1, 2, 3} applying
the binary Bayes filter (BBF) for inverse sensor models (3.6) individually in every
(c)
grid cell c. The result is the joint occupancy probability pzk+1 (Ok+1 |zk+1 ), as shown
in the middle row in Figure 4.4. The interpretation of the joint measurement grid
is the following: It holds for every grid cell c the estimated occupancy probability,
reasoning on all measurements at time step k + 1.
Finally, the processing scheme combines the joint measurement grid and the predicted
occupancy grid. It applies the BBF filter again in every grid cell c to combine the
(c)
predicted occupancy probability po,+ (Ok+1 ) with the measurement-based occupancy
(c)
probability pzk+1 (Ok+1 |zk+1 ). The result is the posterior occupancy probability
(c)
po,k+1 (Ok+1 ) as shown in the right column in Figure 4.4.
Measurement Data Preprocessing in the PHD/MIB Filter
In the same manner as the static occupancy grid map, the PHD/MIB filter processes
input in form of a joint measurement grid. Figure 4.5 shows the data processing
scheme of the PHD/MIB filter for the example of three measured laser reflections.
In this example, the measurement grid contains the same information as for the
static occupancy grid map. Again, the goal is to combine individually for each grid
cell c its predicted state at time k + 1 with the information provided for the specific
cell by the measurement grid.
4.2 Probability Hypothesis Density / Multi-Instance Bernoulli Filter
0.5
0.9
0.7
0.5
0.3
0.5
0.5
0.5
0.3
0.5
0.5
0.5
n
ai
m
n
ai
Do
n
om
io
tD
ict
en
ed
m
0.5
Pr
0.5
re
su
ea
0.7
M
0.5
Measurement Grid 1 at k+1
BBF
BBF
0.7
0.6
0.5
0.5
0.5
0.7
0.6
0.5
0.3
0.5
0.7
0.6
0.3
0.3
0.5
0.7
Predicted Grid at k+1
0.5
0.7
0.5
0.5
0.5
0.84
0.5
0.5
0.7
0.9
0.5
0.5
0.5
0.9
0.7
0.5
0.5
0.99
0.93
0.5
0.5
0.99
0.95
0.5
0.3
0.5
0.5
0.5
0.16
0.5
0.9
0.7
0.07
0.5
0.95
0.78
0.3
0.5
0.5
0.5
0.07
0.3
0.5
0.5
0.03
0.16
0.5
0.7
Measurement Grid 2 at k+1
0.5
0.5
0.5
0.5
0.5
0.5
0.7
0.5
0.5
0.5
0.9
0.7
0.3
0.3
0.5
0.5
BBF
Combined Meas. Grid at k+1
45
Updated Grid at k+1
Posterior
Measurement Grid 3 at k+1
Figure 4.4: Data preprocessing in static occupancy grid maps. Left column: Measurement grids for individual laser reflections. Middle
column: Joint measurement grid. Right column: Predicted
occupancy grid and updated occupancy grid.
In case of the PHD/MIB filter, the predicted state in each grid cell c is not defined
(c)
by its occupancy probability alone, but by the Bernoulli distribution π+ (Xk+1 ) as
shown in Figure 4.5. Accordingly, the innovation for each grid cell is carried out in
form of a Bernoulli filter update step.
Intuitively, the Bernoulli filter is an extension of the BBF because the object
existence probability (or occupancy probability) is part of the estimation problem in
the Bernoulli filter. However, in the RFS domain, the observation model is defined
by a multi-object likelihood function γk+1 (Zk+1 |Xk+1 ). This is a prerequisite for
applying the general multi-object Bayes update (2.52).
46
Dynamic Grid Mapping as a Random Finite Set Problem
0.5
0.9
0.7
0.5
0.3
0.5
0.5
0.5
0.3
0.5
0.5
0.5
n
ai
n
m
ai
Do
om
n
io
tD
ict
en
ed
m
0.5
re
su
ea
0.5
Pr
0.7
M
0.5
Measurement Grid 1 at k+1
MIB
π+
π+
π+
π+
π+
π+
π+
π+
π+
π+
π+
π+
π+
π+
π+
π+
Predicted Grid at k+1
0.5
0.7
0.5
0.5
0.5
0.84
0.5
0.5
πk+1 πk+1 πk+1 πk+1
0.5
0.9
0.7
0.5
0.5
0.99
0.93
0.5
πk+1 πk+1 πk+1 πk+1
0.16
0.5
0.9
0.7
πk+1 πk+1 πk+1 πk+1
0.07
0.3
0.5
0.5
πk+1 πk+1 πk+1 πk+1
0.3
0.5
0.5
0.5
0.3
0.5
0.5
0.5
Measurement Grid 2 at k+1
0.5
0.5
0.5
0.5
0.5
0.5
0.7
0.5
0.5
0.5
0.9
0.7
0.3
0.3
0.5
0.5
BBF
Combined Meas. Grid at k+1
Updated Grid at k+1
Posterior
Measurement Grid 3 at k+1
Figure 4.5: Data preprocessing in the PHD/MIB filter. Left column: Measurement grids for individual laser reflections. Middle column:
Joint measurement grid. Right column: Predicted Bernoulli
RFS in every grid cell and updated Bernoulli RFS in every grid
cell.
It is at this stage unclear, how the individual, measurement-based occupancy proba(c)
bility pzk+1 (Ok+1 |zk+1 ) for the grid cell c can be expressed in form a measurement
(c)
(c)
(c)
event Zk+1 and a multi-object likelihood function γk+1 (Zk+1 |Xk+1 ), while analytically providing the same information.
This question will be answered in the remaining part of this chapter. First, the next
section will define the Bernoulli observation model of the PHD/MIB filter. Later,
Section 4.3 will show the formal relation between the Bernoulli observation model
(c)
and the measurement-based occupancy probability pzk+1 (Ok+1 |zk+1 ).
4.2 Probability Hypothesis Density / Multi-Instance Bernoulli Filter
47
Bernoulli Observation Model
The standard Bernoulli filter expects Poisson distributed clutter detections [Ris13].
The PHD/MIB defines the following Bernoulli RFS observation process instead: A
measurement grid map provides an observation for each grid cell based on sensor data
at time step k + 1. The observation process of one grid cell is assumed independent
of other grid cells.
(c)
In each grid cell either one measurement zk+1 or no measurement occurs. The
probability that a measurement occurs in the occupied grid cell c is the cell-specific
(c)
and time-dependent true positive probability pTP,k+1 ∈ (0, 1). The probability
that a measurement occurs in the empty cell c is the false positive probability
(c)
pFP,k+1 ∈ (0, 1).
(c)
With the ratio between the true positive probability pTP,k+1 and the false positive
probability
(c)
pFP,k+1 ,
it is possible to express the same information as with the
(c)
measurement-based occupancy probability pzk+1 (Ok+1 |zk+1 ), as will be shown in
Section 4.3.
The PDF of a false positive measurement is given by the clutter density pcl (z). A
true positive measurement is associated to the point object in the grid cell with the
(c)
association probability pA,k+1 . In this case its distribution is defined by the single(c)
object likelihood function gk+1 (zk+1 |xk+1 ). If the measurement is not associated to
the point object, its PDF is also defined by the clutter density pcl (z).
(c)
The likelihood function gk+1 (zk+1 |xk+1 ) models a measurement process which is
dependent on the state xk+1 of the point object in the grid cell c. With the laser
measurements described in Figure 4.5, this is not the case. The laser measurements
are assumed to only depend on the object existence, not on the object state. An
example for an observation which is dependent on the dynamic object state is the
(c)
Doppler measurement of a radar reflection. With the association probability pA,k+1 ,
it is possible to express that a radar measurement in the vicinity of grid cell c is not
necessarily related to the point object in the grid cell. The association probability
(c)
pA,k+1 decreases with the distance between the measurement and the grid cell. Details
(c)
including the calculation of an exemplary likelihood function gk+1 (zk+1 |xk+1 ) and
its association probability
in Section 5.2.5.
(c)
pA,k+1
for radar Doppler measurements will be provided
48
Dynamic Grid Mapping as a Random Finite Set Problem
Conclusively, each measurement grid cell c contains the individual, time-dependent
(c)
(c)
true positive and false positive probabilities pTP,k+1 and pFP,k+1 , and in case a
measurement occurred in the cell, it additionally provides the single-object likelihood
(c)
(c)
function gk+1 (zk+1 |xk+1 ) and the corresponding association probability pA,k+1 .
(c)
The resulting multi-object likelihood γk+1 (Zk+1 |Xk+1 ) for the measurement grid cell
c considers all four possible cases of measurement and object existence.
Case 1: Zk+1 = ∅, Xk+1 = ∅,
i.e. no measurement occurs in grid cell c and no object exists in grid cell c.
Case 2: Zk+1 = ∅, Xk+1 = {xk+1 },
i.e. no measurement occurs in grid cell c and an object exists in grid cell c.
Case 3: Zk+1 = {zk+1 }, Xk+1 = ∅,
i.e. a measurement occurs in grid cell c and no object exists in grid cell c.
Case 4: Zk+1 = {zk+1 }, Xk+1 = {xk+1 },
i.e. a measurement occurs in grid cell c and an object exists in grid cell c.
The observation process as described above leads to the following multi-object
(c)
likelihood γk+1 (Zk+1 |Xk+1 ):
(c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 = ∅) = pFP,k+1 ,
(c)
(c)
(c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 = {xk+1 }) = pTP,k+1 ,
γk+1 (Zk+1 = {zk+1 }|Xk+1 = ∅) = pFP,k+1 · pcl (zk+1 ),
(c)
γk+1 (Zk+1 = {zk+1 }|Xk+1 = {xk+1 }) =
i
h
(c)
(c)
(c)
(c)
pTP,k+1 pA,k+1 · gk+1 (zk+1 |xk+1 ) + pA,k+1 · pcl (zk+1 ) .
|
{z
}
(c)
:=gA,k+1 (zk+1 |xk+1 )
(4.12)
4.2 Probability Hypothesis Density / Multi-Instance Bernoulli Filter
49
Multi-Object Bayes Update Step
Together, the predicted Bernoulli state (4.10) and the Bernoulli observation process
(4.12) define the multi-object state estimation problem for the individual grid cell
c. The exact solution is obtained by inserting (4.10) and (4.12) into the general
multi-object Bayes update (2.52). This leads to the posterior multi-object PDF
(c)
πk+1 (Xk+1 |Zk+1 ), which again considers all four possible existence cases.
Case 1: Zk+1 = ∅, Xk+1 = ∅,
i.e. no measurement occurs in grid cell c and no object exists in grid cell c results in
(2.52)
(c)
πk+1 (Xk+1 = ∅|Zk+1 = ∅) =
(c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 = ∅) · π+ (Xk+1 = ∅)
R (c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 ) · π+ (Xk+1 )δXk+1
(c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 )
(c)
(c)
pFP,k+1 · r+
(c)
(c)
(c)
pFP,k+1 · r+ + pTP,k+1
=
(c)
pFP,k+1 · r+
R
(4.10),(4.12)
·
(2.31)
(c)
π+ (Xk+1 )δXk+1
(c)
· r+
=
.
(4.13)
Case 2: Zk+1 = ∅, Xk+1 = {xk+1 },
i.e. no measurement occurs in grid cell c and an object exists in grid cell c results in
(2.52)
(c)
πk+1 (Xk+1 = {xk+1 }|Zk+1 = ∅) =
(c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 = {xk+1 }) · π+ (Xk+1 = {xk+1 })
R (c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 ) · π+ (Xk+1 )δXk+1
i
h
(c)
(c)
(c)
(c)
pTP,k+1 · rb,+ · pb (xk+1 ) + rp,+ · pp,+ (xk+1 ) (2.31)
=
R (c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 ) · π+ (Xk+1 )δXk+1
i
h
(c)
(c)
(c)
(c)
pTP,k+1 · rb,+ · pb (xk+1 ) + rp,+ · pp,+ (xk+1 )
.
(c)
(c)
(c)
(c)
pTP,k+1 · r+ + pFP,k+1 · r+
(4.10),(4.12)
=
(4.14)
50
Dynamic Grid Mapping as a Random Finite Set Problem
Case 3: Zk+1 = {zk+1 }, Xk+1 = ∅,
i.e. a measurement occurs in grid cell c and no object exists in grid cell c results in
(2.52)
(c)
πk+1 (Xk+1 = ∅|Zk+1 = {zk+1 }) =
(c)
(c)
γk+1 (Zk+1 = {zk+1 }|Xk+1 = ∅) · π+ (Xk+1 = ∅)
R (c)
(c)
γk+1 (Zk+1 = {zk+1 }|Xk+1 ) · π+ (Xk+1 )δXk+1
(c)
γk+1 (Zk+1
=
(c)
(c)
pFP,k+1 · pcl (zk+1 ) · r+
R
(4.10),(4.12)
= ∅|Xk+1 ) ·
(2.31)
(c)
π+ (Xk+1 )δXk+1
(c)
=
(c)
pFP,k+1 · pcl (zk+1 ) · r+
.
R (c)
(c)
(c)
(c)
(c)
pFP,k+1 · pcl (zk+1 ) · r+ + pTP,k+1 gA,k+1 (zk+1 |xk+1 ) · π+ ({xk+1 })dxk+1
(4.15)
Case 4: Zk+1 = {zk+1 }, Xk+1 = {xk+1 },
i.e. a measurement occurs in grid cell c and an object exists in grid cell c results in
(2.52)
(c)
πk+1 (Xk+1 = {xk+1 }|Zk+1 = {zk+1 }) =
(c)
(c)
γk+1 (Zk+1 = {zk+1 }|Xk+1 = {xk+1 }) · π+ (Xk+1 = {xk+1 }) (4.10),(4.12)
=
R (c)
(c)
γk+1 (Zk+1 = {zk+1 }|Xk+1 ) · π+ (Xk+1 )δXk+1
h
i
(c)
(c)
(c)
(c)
(c)
pTP,k+1 · gA,k+1 (zk+1 |xk+1 ) · rb,+ · pb (xk+1 ) + rp,+ · pp,+ (xk+1 ) (2.31)
=
R (c)
(c)
γk+1 (Zk+1 = ∅|Xk+1 ) · π+ (Xk+1 )δXk+1
h
i
(c)
(c)
(c)
(c)
(c)
pTP,k+1 · gA,k+1 (zk+1 |xk+1 ) · rb,+ · pb (xk+1 ) + rp,+ · pp,+ (xk+1 )
.
R (c)
(c)
(c)
(c)
(c)
pTP,k+1 gA,k+1 (zk+1 |xk+1 ) · π+ ({xk+1 })dxk+1 + pFP,k+1 · pcl (zk+1 ) · r+
(4.16)
4.2 Probability Hypothesis Density / Multi-Instance Bernoulli Filter
51
The update equations (4.13) to (4.16) can be expressed more compact by defining normalization constants. Then the shorter form of the multi-object PDF
(c)
πk+1 (Xk+1 |Zk+1 ) can be written as:
1
(c)
πk+1 (Xk+1 = ∅|Zk+1 = ∅) =
(c)
(c)
µ{∅}
(c)
· pFP,k+1 · r+ ,
1
(c)
πk+1 (Xk+1 = {xk+1 }|Zk+1 = ∅) =
(c)
(c)
· pTP,k+1 · π+ ({xk+1 }),
(c)
µ{∅}
(c)
(c)
πk+1 (Xk+1
= ∅|Zk+1 = {zk+1 }) =
pFP,k+1
(c)
µ{z}
(c)
· pcl (zk+1 ) · r+ ,
(c)
(c)
πk+1 (Xk+1 = {xk+1 }|Zk+1 = {zk+1 }) =
pTP,k+1
(c)
µ{z}
(c)
(c)
· gA,k+1 (zk+1 |xk+1 ) · π+ ({xk+1 }),
(4.17)
with normalization constants
(c)
(c)
(c)
(c)
(c)
µ{∅} = pFP,k+1 · r+ + pTP,k+1 · r+
(4.18)
and
(c)
(c)
(c)
µ{z} = pFP,k+1 · pcl (zk+1 ) · r+
Z
(c)
(c)
(c)
+ pTP,k+1 gA,k+1 (zk+1 |xk+1 ) · π+ ({xk+1 })dxk+1 .
(4.19)
(c)
The posterior πk+1 (Xk+1 ) is a Bernoulli RFS and represents both the dynamic state
and the occupancy probability of the corresponding grid cell. According to the
definition of a Bernoulli RFS, the posterior occupancy probability of grid cell c is
Z
(c)
(c)
(c)
po,k+1 (Ok+1 ) = rk+1 =
πk+1 ({xk+1 }) dxk+1 .
(4.20)
xk+1 ∈c
After the update step, the PHD/MIB filter transforms all Bernoulli RFS instances
into a joint PHD again. The joint PHD is the sum of all Bernoulli RFS instances:
Dk+1 (xk+1 ) =
C
X
c=1
(c)
πk+1 (Xk+1 = {xk+1 }),
(4.21)
52
Dynamic Grid Mapping as a Random Finite Set Problem
where c denotes the index of the corresponding grid cell of each Bernoulli RFS
instance and C is the total number of grid cells. This closes the recursion as
introduced in Figure 4.2.
4.3 Relation between PHD/MIB Filter and Binary
Bayes Filter
The proposed PHD/MIB filter is a generalization of the binary Bayes filter which
does not rely on the assumption of a static environment. Consequently, the filter
equations should simplify to the well-known equations of the binary Bayes filter for
a static process model. Recall, that the generalized BBF as defined by (3.7) states
(c)
(c)
αzk+1 (Ok+1 ) · po,k (Ok )
(c)
po,k+1 (Ok+1 ) =
(c)
(c)
(c)
αzk+1 (Ok+1 ) · po,k (Ok ) + po,k (Fk+1 )
.
(4.22)
(c)
Here, O is the occupancy event and F is the free event. Further, po,k (Ok ) is the
posterior occupancy probability of the former time step k, which equals the prediction
(c)
in a static environment, and po,k+1 (Ok+1 ) is the updated occupancy probability
(c)
at time step k + 1. Most importantly, αzk+1 is the measurement-based occupancy
probability ratio
(c)
αz(c)
=
k+1
pzk+1 (Ok+1 |zk+1 )
(c)
pzk+1 (Fk+1 |zk+1 )
, p(c)
zk+1 (Fk+1 |zk+1 ) > 0,
(4.23)
or the likelihood ratio
(c)
αz(c)
=
k+1
gk+1 (zk+1 |Ok+1 )
(c)
gk+1 (zk+1 |Fk+1 )
(c)
, gk+1 (zk+1 |Fk+1 ) > 0,
(4.24)
depending if the measurement grid cell expresses the information in form of an
occupancy probability or in form of a likelihood density. The motivation to define
the generalized BBF was to show that the information in an occupancy measurement
(c)
grid cell can uniquely be expressed by the ratio αzk+1 , independently of the absolute
(c)
(c)
likelihood values gk+1 (zk+1 |Ok+1 ) and gk+1 (zk+1 |Fk+1 ). A closer look at the update
equations of the PHD/MIB filter (4.16) and (4.16) reveals, that there exists an
(c)
analogy for the true positive probability pTP,k+1 and the false positive probability
(c)
pFP,k+1 .
4.3 Relation between PHD/MIB Filter and Binary Bayes Filter
(c)
53
(c)
Again, the absolute values of pTP,k+1 and pFP,k+1 are irrelevant and only the true
positive to false positive ratio
(c)
αz(c)
=
k+1
pTP,k+1
(c)
pFP,k+1
(c)
, pFP,k+1 > 0
(4.25)
is informative for the update step.
(c)
Of course, additional information is contained in the association probability pA,k+1 ,
(c)
the likelihood function gk+1 (zk+1 |xk+1 ), and the clutter density pcl (z), but this kind
of information relates to the extended part of the PHD/MIB filter, which does not
exist in the original BBF.
In order to reduce the PHD/MIB to the special case of a BBF filter, the following
(c)
proof will define a non-informative likelihood function gk+1 (zk+1 |xk+1 ) and a noninformative clutter density pcl (z). In this case, all information contained in a
(c)
measurement grid cell is given by its true positive to false positive ratio αzk+1 .
Proposition: Assume a deterministic, static process model in the PHD/MIB filter,
so that the predicted intensity distribution for time step k + 1 is equivalent to the
posterior distribution at time k, i.e.
D+ (xk+1 ) = Dk (xk ) ∀ xk+1 = xk ∈ X, k ∈ N.
(4.26)
(c)
Further, assume the measurement likelihood gk+1 (zk+1 |xk+1 ) and the clutter density
pcl (zk+1 ) are equal uniform distributions in a limited subset Zs of the measurement
space, i.e.
(
θ if zk+1 ∈ Zs ,
(c)
gk+1 (zk+1 |xk+1 ) = pcl (zk+1 ) =
(4.27)
0 otherwise,
where θ > 0 is constant. Then the propagation of the posterior occupancy probability
(c)
(c)
(c)
rk = po,k (ok = Ok ) at time k to the posterior occupancy probability rk+1 =
(c)
po,k+1 (ok+1 = Ok+1 ) at time k + 1 of the PHD/MIB filter reduces to the generalized
binary Bayes update (3.7)
(c)
(c)
αzk+1 (Ok+1 ) · po,k (Ok )
(c)
po,k+1 (Ok+1 ) =
(c)
(c)
(c)
αzk+1 (Ok+1 ) · po,k (Ok ) + po,k (Fk+1 )
,
(4.28)
54
Dynamic Grid Mapping as a Random Finite Set Problem
with
αz(c)
(Ok+1 ) =
k+1
 (c)
p

 TP,k+1
if Zk+1 = ∅,
(c)

pFP,k+1
(4.29)
(c)

p

 TP,k+1
if Zk+1 = {zk+1 }.
(c)
pFP,k+1
Proof: Due to the static process model (4.26), the Bernoulli distribution for each
grid cell does not change during the prediction step:
(c)
(c)
π+ = πk .
(4.30)
In case of no measurement in grid cell c, i.e. Zk+1 = ∅, the posterior existence
probability of an object in grid cell c is given by
Z
(c) (4.20)
(c)
rk+1 =
πk+1 ({xk+1 }|∅) dxk+1
xk+1 ∈c
Z
(4.17),(4.30)
=
1
(c)
(c)
µ{∅}
xk+1 ∈c
(c)
· pTP,k+1 · πk ({xk+1 }) dxk+1
(c)
=
(c)
pTP,k+1 · rk
(4.18),(4.30)
(c)
(c)
(c)
(4.31)
(c)
pFP,k+1 · rk + pTP,k+1 · rk
and for Zk+1 = {zk+1 } it follows
Z
(c) (4.20)
(c)
rk+1 =
πk+1 ({xk+1 }|{zk+1 }) dxk+1
xk+1 ∈c
(4.17),(4.30)
Z
=
(c)
pTP,k+1
(c)
xk+1 ∈c
µ{z}
(c)
(c)
· gA,k+1 · πk ({xk+1 }) dxk+1
(c)
=
(c)
(c)
(c)
(c)
(c)
(c)
pFP,k+1 · pcl · rk + pTP,k+1 · gA,k+1 · rk
(c)
(c)
pTP,k+1 · rk
(4.27)
=
(c)
pTP,k+1 · gA,k+1 · rk
(4.19),(4.27),(4.30)
(c)
(c)
(c)
(c)
pFP,k+1 · rk + pTP,k+1 · rk
.
(4.32)
4.4 Discussion
55
4.4 Discussion
This chapter introduced point objects as a representation for the vehicle environment
and described how the existence probability and the dynamic state of a point object
defines the occupancy probability and the dynamic state of a grid cell. Further,
the chapter outlined the connection between the research field of dynamic grid
maps and the random finite set theory. In contrast to former work on dynamic
grid maps [DON11; NRL14; NYK+ 15; TTWB14], random finite set theory allows
a stochastically rigorous definition of the multi-object state estimation problem.
Furthermore, modeling the problem in the random finite set domain offers the
possibility to apply well-established random finite set filters. The chapter suggested
a combination of a PHD filter and multiple instances of Bernoulli filters to estimate
the dynamic multi-object state of point objects.
The motivation for choosing the PHD representation in the prediction step is the
following: All mentioned papers [DON11; NRL14; NYK+ 15; TTWB14] represent
and propagate the dynamic state of grid cells with particles. The prediction step
does not distinguish between particles of different grid cells in all four publications.
In other words: particles are not associated to specific objects. A disadvantage is
that it is not possible to model object interactions, an advantage is that it leads to a
simple prediction step, since every particle can be processed independently of other
particles. The PHD filter shows the same characteristics: By reducing a random
finite set to its PHD, any information on object interdependency gets lost, but the
practical realization of the prediction step is easy to implement, since it considers
only single-object state transition densities (see (2.61)). This analogy between the
PHD filter and the prediction step of dynamic grid maps presented in literature was
the motivation in this thesis to represent the joint posterior of point objects by its
PHD and to apply the prediction step of the PHD filter. Of course, the RFS domain
is not limited to the PHD filter and offers many more options as will be discussed
below in this section.
Further, all mentioned papers [DON11; NRL14; NYK+ 15; TTWB14] carry out the
update step individually for each grid cell. The goal of this step is to estimate the
occupancy probability of the grid cell as well as its dynamic state. The direct analogy
of this estimation problem in the random finite set domain can be modeled with a
Bernoulli random finite set and can be solved with the Bernoulli filter. As outlined
in Chapter 3, classical occupancy grid maps assume a static world and estimate
only the occupancy state of a grid cell, applying the binary Bayes filter. Both the
binary Bayes filter and the Bernoulli filter solve an estimation problem in a discrete,
binary state space. The Bernoulli filter additionally estimates the dynamic state of
a potentially existing object. Intuitively, the Bernoulli filter is an extension of the
binary Bayes filter. The connection between these filters is formally confirmed by
56
Dynamic Grid Mapping as a Random Finite Set Problem
the proof presented at the end of this chapter. The proof shows that the existence
probability of a point object in a grid cell estimated by the Bernoulli filter is identical
to the occupancy probability of the grid cell estimated by the binary Bayes filter.
Conclusively, the combination of a PHD filter and multiple instances of Bernoulli
filters was chosen in this thesis because it is able to explain common approaches
in the research field of dynamic grid maps from the perspective of random finite
sets. However, the random finite set domain offers further well-studied approaches
which might be suitable for propagating the multi-object state of point objects or
dynamic grid maps in general. An example is the labeled multi-Bernoulli (LMB)
filter [RVVD14], which received much attention recently. Among other things, the
LMB filter is able to consider the interdependency of the existence of objects. This
could be a potential improvement in comparison to the PHD/MIB filter and the
application of the LMB filter for dynamic grid maps should be investigated in future
research.
As mentioned above, the advantage of neglecting interdependencies between point
objects is a simplified implementation in comparison to approaches which do consider
these object interdependencies. More specifically, if the PHD/MIB filter is realized
with particles, the filter can process particles and grid cells in parallel to a great
part. This is interesting for practical applications, since parallel processing chip
technology is on the upgrade today.
The remaining part of this thesis focuses on the challenge of realizing and implementing the PHD/MIB filter as a real-time application for self-driving cars. The following
chapter describes the particle realization of the PHD/MIB filter and Chapter 6
outlines in detail a highly efficient parallelized implementation.
Chapter 5
Particle Realization and
Dempster-Shafer Approximation
The PHD/MIB filter can be realized in different ways. A main character of the
realization is the representation form of the state probability hypothesis density
(PHD). Widely used realizations of the original PHD filter are the Gaussian mixture
probability hypothesis density (GM-PHD) filter [VM06] and the SMC-PHD filter
as introduced in Section 2.2.5, also referred to as particle realization [Ris13; Sid03;
VSD05]. Particle realizations are particularly well-suited for parallel implementation
as will be outlined in Chapter 6. This chapter presents in the first part the particle
realization of the PHD/MIB filter.
A remaining challenge for practical applications are huge unobserved areas in the
robot or vehicle environment. As these areas could potentially contain objects,
the original particle implementation of the PHD/MIB filter populates these areas
with particles. The second part of this chapter presents an approximation of the
PHD/MIB filter using the Dempster-Shafer theory of evidence (DST) [Dem68; Sha76;
Sme90], which significantly reduces the number of required particles. In the third
part, the chapter summarizes the assumptions and approximations in the PHD/MIB
filter and the additional approximations introduced by the Dempster-Shafer-based
approach.
5.1 Particle Realization of the PHD/MIB Filter
The section presents the particle realization of the PHD/MIB filter, separated into
prediction and update step.
58
Particle Realization and Dempster-Shafer Approximation
5.1.1 Particle Prediction Step
The prediction step is similar to the prediction step of the original SMC-PHD filter
as presented in Section 2.2.5. Particles are random samples of the posterior PHD at
(i)
(i)
time step k. A particle set consists of ν particles and their weights {xk , wk }νi=1 .
Together they represent the posterior as
Dk (xk ) =
ν
X
(i)
(i)
wk δ(xk − xk ).
(5.1)
i=1
Here, δ is the Dirac delta function as defined in (2.17). Recall that the posterior
PHD Dk (xk ) represents the set of all point objects in the vehicle environment.
Prediction of Persistent Objects
To represent predicted persistent objects, the prediction draws particles by sampling
the proposal density qk+1 :
(i)
(i)
xp,+ ∼ qk+1 (·|xk , Zk+1 ).
(5.2)
The index p depicts that these particles represent a persistent point object. The
remaining part of the section assumes it is possible to sample from the transition
density f+ , which is used as proposal density:
(i)
(i)
qk+1 (xk+1 |xk , Zk+1 ) = f+ (xk+1 |xk ).
(i)
(5.3)
(i)
The sampling provides the predicted particle set {xp,+ , wp,+ }νi=1 for time step k + 1,
where the particle weights are multiplied with the persistence probability:
(i)
(i)
wp,+ = pS · wk .
(5.4)
The set represents the predicted PHD of persistent point objects:
Dp,+ (xk+1 ) =
ν
X
i=1
(i)
(i)
wp,+ δ(xk+1 − xp,+ ).
(5.5)
5.1 Particle Realization of the PHD/MIB Filter
59
Transition from a PHD to Multiple Instances of Bernoulli RFSs
As depicted in Figure 4.2, the PHD/MIB filter now transforms the representation
form from the joint PHD to individual, independent Bernoulli RFSs for each grid
cell. Accordingly, the following steps are carried out for each grid cell c individually.
Let
(i,c)
(i,c) ν
(c)
p,+
{xp,+ , wp,+ }i=1
(5.6)
be the set of particles predicted into grid cell c at time step k + 1 according to (5.2)
(c)
and (5.4). The symbol νp,+ represents the number of particles predicted into grid
cell c at time step k + 1.
To keep the notation simple, consider the set (5.6) already truncated, i.e., the sum
of weights does not exceed the persistence probability pS . Recall that the PHD
filter prediction step neglects any object interaction. It doesn’t consider the fact
that a grid cell cannot be entered by more than one point object. Consequently, it
is possible that the sum of predicted particle weights in one grid cell exceeds the
persistence probability pS . As stated by (4.7) and (4.8), if and only if the the sum
of predicted particle weights in cell c exceeds pS , the particle weights in cell c have
to normalized to sum up to pS .
According to (4.7), the sum of predicted particle weights in grid cell c then equals
(c)
the predicted existence probability rp,+ of a persistent object in cell c:
(c)
νp,+
(c)
rp,+ =
X
(i,c)
wp,+ ∈ (0, pS ].
(5.7)
i=1
Prediction of New-Born Objects
(c)
According to (4.10), the predicted existence probability rb,+ of a new-born object in
grid cell c is given by
(c)
(c)
rb,+ = pB · 1 − rp,+ ,
(5.8)
60
Particle Realization and Dempster-Shafer Approximation
where the birth probability pB is the probability for a birth event in an empty cell.
Predicted new-born objects in grid cell c are represented by the particle set
(i,c) ν
(i,c)
(c)
b,+
.
{xb,+ , wb,+ }i=1
(5.9)
The particles of this set are sampled from the spatial birth PDF pb :
(i,c)
xb,+ ∼ pb (·).
(5.10)
(c)
The number of new-born particles νb,+ for each grid cell c is a design parameter of
the system.
Since the new-born particle weights sum up to the predicted existence probability
(c)
rb,+ (4.9) of a new-born object in grid cell c, the weight of each new-born particle is
given by
(c)
(i,c)
wb,+ =
rb,+
(c)
.
(5.11)
νb,+
Predicted Bernoulli RFS
Together, the persistent and the new-born particle sets represent the predicted
(c)
Bernoulli RFS π+ (Xk+1 ) in grid cell c:
(c)
(c)
νb,+
νp,+
(c)
π+ ({xk+1 })
=
X
(i,c)
wp,+ δ(xk+1
−
(i,c)
xp,+ )
+
i=1
X
(i,c)
(i,c)
wb,+ δ(xk+1 − xb,+ )
(5.12)
i=1
and
(c)
(c)
(c)
π+ (∅) = 1 − rp,+ − rb,+ .
(5.13)
5.1.2 Particle Update Step
Assume a measurement grid map taken at time step k + 1 provides for each grid cell
an observation as stated above in Section 4.2.2. The update step adapts the weights
of the particle set (5.12).
5.1 Particle Realization of the PHD/MIB Filter
61
The update rules for persistent and new-born particles are identical. The notation
system uses the weight symbol w∗ with ∗ ∈ {p,b} in equations that are identical for
both persistent particle weights wp and new-born particle weights wb .
In case a measurement occurred in measurement grid cell c, unnormalized adapted
(i,c)
particle weights w
e∗,k+1 are calculated according to (4.17):
(i,c)
(c)
(c)
(i,c)
(i,c)
w
e∗,k+1 = pTP,k+1 · gA,k+1 (zk+1 |x∗,+ ) · w∗,+ .
(5.14)
The normalized weights are given by
(i,c)
(i,c)
w∗,k+1 =
w
e∗,k+1
(5.15)
(c)
µ{z}
with (4.19)
(c)
(c)
νb,+
νp,+
(c)
µ{z}
=
(c)
pFP,k+1
· pcl (zk+1 ) ·
(c)
r+
+
X
(i,c)
w
ep,k+1
+
X
i=1
(i,c)
w
eb,k+1 .
(5.16)
i=1
In case no measurement occurred in measurement grid cell c, the update rule for
(i,c)
both the persistent and new-born particles to calculate adapted weights w∗,k+1 is
according to (4.17):
(c)
pTP,k+1
(i,c)
w∗,k+1 =
(c)
(c)
(i,c)
(c)
(c)
pTP,k+1 · r+ + pFP,k+1 · r+
w∗,+ .
(5.17)
Notice that for multi-object distributions, normalization does not mean all particle
weights sum up to 1. Instead, the sum of updated particle weights equals the
(c)
posterior existence probability rk+1 of a point object in grid cell c at time step k + 1,
(c)
which is also the posterior occupancy probability po,k+1 (Ok+1 ) of the grid cell:
(c)
(c)
νp,k+1
(c)
rk+1
=
(c)
po,k+1 (Ok+1 )
=
X
i=1
νb,k+1
(i,c)
wp,k+1
+
X
i=1
(i,c)
wb,k+1 .
(5.18)
62
Particle Realization and Dempster-Shafer Approximation
(c)
The posterior Bernoulli RFS πk+1 (Xk+1 ) of grid cell c is given by:
(c)
(c)
νp,k+1
(c)
πk+1 ({xk+1 }) =
X
νb,k+1
(i,c)
(i,c)
wp,k+1 δ(xk+1 − xp,k+1 ) +
i=1
X
(i,c)
(i,c)
wb,k+1 δ(xk+1 − xb,k+1 )
i=1
(5.19)
and
(c)
(c)
(c)
πk+1 (∅) = rk+1 = 1 − rk+1 .
(5.20)
Joint PHD Representation
The PHD/MIB represents the posterior multi-object state of all point objects in the
environment by its PHD. The transformation from multiple instances of Bernoulli
RFSs to a joint PHD is given by (4.21)
X
(i,c)
(i,c)
Dk+1 (xk+1 ) =
wp,k+1 δ(xk+1 − xp,k+1 )
(c)
i∈[1,νp,k+1 ],c∈[1,C]
+
X
(i,c)
(i,c)
wb,k+1 δ(xk+1 − xb,k+1 ),
(5.21)
(c)
i∈[1,νb,k+1 ],c∈[1,C]
where C ist the overall number of grid cells. Usually, particle filter realizations
of a PHD filter provide only the persistent part of the posterior PHD as output
[Ris13]. Depending on the application, new-born particles considerably increase the
uncertainty of the estimated state of objects. So it is often beneficial to consider
their influence on the state estimation only after another recursion.
5.1.3 Resampling
For many applications it is important to keep the overall number of used particles
constant. Therefore, the PHD/MIB filter resamples the constant number of ν
particles from the joint posterior particle set. For each particle the probability to be
drawn is proportional to its weight.
(i)
(i)
Let {xk+1 , wk+1 }νi=1 be the set of resampled particles and their weights. The new
weights of the particles are all equal and normalized to sum up to the same value as
5.2 Real-time Approximation using Dempster-Shafer Theory of Evidence
63
the posterior weights of the persistent and the new-born particles together:
X (i)
X
(i,c)
wk+1 =
wp,k+1
i∈[1,ν]
(c)
i∈[1,νp,k+1 ],c∈[1,C]
+
X
(i,c)
wb,k+1 .
(5.22)
(c)
i∈[1,νb,k+1 ],c∈[1,C]
5.2 Real-time Approximation using
Dempster-Shafer Theory of Evidence
For some application scenarios, the presented particle realization of the PHD/MIB
filter might not be real-time capable. A possible reason are huge unobserved areas in
grid maps. Since the presented particle realization of the PHD/MIB filter represents
potential point objects in unobserved areas with particles, it requires a large number
of them to cover the multi-object state space.
All mentioned publications of particle-based dynamic grid maps [DON11; NRL14;
NYK+ 15; TTWB14] use particles only for occupied grid cells, not for unobserved
grid cells. One possibility to distinguish between unobserved and occupied cells is to
use Dempster-Shafer masses [Dem68; Sha76; Sme90; YFE94] instead of occupancy
probabilities. The approach is commonly applied to classical grid maps [NTDD14;
NWW+ 13; YHCZ00] and to particle based dynamic grid maps [NYK+ 15; TTWB14].
The approach presented in this section is a coarse approximation of the particlebased PHD/MIB filter applying the Dempster-Shafer theory of evidence (DST).
The resulting approximation will be referred to as Dempster-Shafer PHD/MIB (DSPHD/MIB) filter. The DS-PHD/MIB filter represents the multi-object state with a
substantially reduced number of particles compared to the original PHD/MIB filter
and is also easier to implement. An efficient, massively parallelized implementation
of the DS-PHD/MIB filter will be presented in Chapter 6.
5.2.1 Dempster-Shafer Theory of Evidence
In the previous part of the thesis, Bayesian inference has served as the basic mathematical framework for information fusion. This section introduces the DST as an
alternative approach.
64
Particle Realization and Dempster-Shafer Approximation
Formal Definition
The frame of discernment in DST is the set of elementary hypothesis Ω. Elementary
hypotheses are disjoint events. The power set 2Ω is the set of all subsets of Ω
including the empty set. An element of the power set 2Ω is called a proposition.
The function m : 2Ω → [0, 1] is called a basic belief assignment (BBA) or mass if
X
m(∅) = 0,
(5.23)
m(A) = 1.
(5.24)
A∈2Ω
A mass value m(A) > 0 expresses a certain belief in proposition A without further
distinction between the elements of A.
Analogy to Kolmogorov’s Probability Theory
To understand the Dempster-Shafer theory of evidence (DST), it is important to
recall Kolmogorov’s probablity theory [Kol50]. Let ΩK be a non-empty set called
universal set and let F be a sigma-algebra1 on ΩK . The function p : F → [0, 1] is
called a probability function and assigns a probability to the event E ∈ F if it fulfills
the following three axioms:
p(E) ≥ 0 for all E ∈ F,
(5.25)
p(ΩK ) = 1,
(5.26)
p(D ∪ E) = p(D) + p(E) for all D, E ∈ F and D ∩ E = ∅.
(5.27)
The axioms are often referred to as probability axioms or Kolmogorov’s three
axioms. The triple consisting of the universal set ΩK , a sigma-algebra on ΩK and
the probability function p is called a probability space. Analogously, the DST
defines a triple consisting the frame of discernment Ω, its power set 2Ω and the
BBA m. However, a probability function p and a BBA m describe uncertainty in
fundamentally different ways. Table 5.1 summarizes the analogy between the DST
and Kolmogorov’s probability theory.
1F
⊆ 2ΩK is a sigma-algebra on ΩK if the following three conditions hold:
ΩK ∈ F ,
ΩK \ A ∈ F for all A ∈ F ,
A1 ∪ A2 ∪ A3 , ∪... ∈ F for all A1 , A2 , A3 , ... ∈ F .
5.2 Real-time Approximation using Dempster-Shafer Theory of Evidence
65
Table 5.1: Analogies of DST to Kolmogorov’s probablity theory.
Kolmogorov
DST
Set of elementary
hypothesis
Universal set ΩK
Frame of discernment Ω
Function Domain
Event space F ,
is a sigma-algebra on ΩK
Power set 2Ω
Function
Probability function
p : F → [0, 1]
Basic belief assignment
m : 2Ω → [0, 1]
Axioms
p(E) ≥ 0 for all E ∈ F ,
p(ΩK ) = 1,
p(D∪E) = p(D)+p(E) ∀ D, E ∈ F |D∩E = ∅.
Pm(∅) = 0,
m(A) = 1.
A∈2Ω
Example: Wheel of Fortune
The following scenario gives an example for the difference between the two domains
DST and Kolmogorov’s probability theory. Regard a wheel of fortune which is known
to be either green or red but is partly covered as shown in Figure 5.1. Assume the
uncovered area is half green and half red. The result of a wheel turn can either
be green (G) or red (R), therefore both the universal set ΩK and the frame of
discernment Ω are given by
ΩK = Ω = {G, R}.
(5.28)
This example considers the power set of the universal set ΩK as event space F :
F = 2ΩK = {∅, {G}, {R}, ΩK } .
(5.29)
The probability for the events green or red as an outcome of a wheel spin is unknown
since a part of the wheel is covered. A commonly applied approach in such cases is
to assume a uniform distribution for the covered area. This results in the following
probability function p:
p(∅) = 0,
p(G) = 0.5,
p(R) = 0.5,
p(ΩK ) = 1.
(5.30)
Note that with this approach, the probability function p is independent of how much
of the wheel is covered, as long as the uncovered area is half green and half red. In
contrast, the BBA m distinguishes between uncertainty in a statistical sense and
lack of knowledge. The latter is also called epistemic uncertainty. A covered part of
the wheel is an example for epistemic uncertainty. The BBA m : 2{G,R} → [0, 1] of a
wheel spin result depends on how much of the wheel is covered. Assume again that
the visible area of the wheel is always half green and half red, but it is unknown how
66
Particle Realization and Dempster-Shafer Approximation
Figure 5.1: Wheel of fortune as an example for a BBA m : 2{G,R} → [0, 1]
with elementary hypothesis green (G) and red (R). It is known
that the wheel is either green or red, but it is unknown how the
covered area (grey) is divided into green and red.
the colors are distributed behind the cover. If the wheel is completely uncovered, m
is given by
m(∅) = 0,
m(G) = 0.5,
m(R) = 0.5,
m(ΩK ) = 0.
(5.31)
m(ΩK ) = 0.5.
(5.32)
If the wheel is half covered, m is given by
m(∅) = 0,
m(G) = 0.25,
m(R) = 0.25,
If the wheel is completely covered, m is given by
m(∅) = 0,
m(G) = 0,
m(R) = 0,
m(ΩK ) = 1.
(5.33)
Note that mass assigned to the frame of discernment Ω is not necessarily 1. Instead,
the mass assigned to Ω expresses epistemic uncertainty.
Pignistic Transformation
Important properties of a BBA with regard to a specific proposition are belief,
plausibility and uncertainty.
5.2 Real-time Approximation using Dempster-Shafer Theory of Evidence
67
Figure 5.2: Belief, uncertainty and plausibility of the BBA m for the event
green (G).
The belief Belm (A) of the BBA m for the proposition A is defined
X
Belm (A) =
m(B).
(5.34)
B⊆A,B6=∅
The plausibility Plm (A) of the BBA m for a proposition A is given by
X
Plm (A) =
m(B),
(5.35)
B∩A6=∅
The uncertainty Um (A) of the BBA m for the proposition A calculates to
Um (A) = Plm (A) − Belm (A).
(5.36)
The belief represents a pessimistic guess, whereas the plausibility represents an
optimistic guess. Figure 5.2 visualizes belief, plausibility and uncertainty for the
example of the wheel of fortune. One possibility to transform the BBA m into a
probability function Pigm is the pignistic transformation [Sme90]:
Pigm (A) =
X |A ∩ B|
m(B),
|B|
(5.37)
B⊆Ω
where the number of elementary hypotheses in the proposition · are denoted |·|.
68
Particle Realization and Dempster-Shafer Approximation
For the binary example of the wheel fortune, the pignistic transformation distributes
the mass for the covered area m(Ω) equally to the probability for green and red.
As a result, the pignistic transformations of the BBAs (5.31), (5.32) and (5.33) all
result in the probability function (5.30).
Generally, the pignistic transformation Pigm fulfills
Belm (A) ≤ Pigm (A) ≤ Plm (A).
(5.38)
A BBA which is only valid with a certain probability α is considered a discounted
BBA mα (A). It is defined as
(
αm(A)
for A =
6 ∅,
α
m (A) =
(5.39)
1 − α + αm(Ω) for A = Ω.
The symbol α is also referred to as discount factor.
Dempster-Shafer Rule of Combination and Relation to Binary Bayes
Filter
The Dempster-Shafer rule of combination combines two independent BBAs m1 and
m2 and is defined
m1⊕2 (A) = m1 (A) ⊕ m2 (A)
P
m1 (X)m2 (Y )
=A
= X∩Y P
∀A ∈ 2Ω .
1−
m1 (X)m2 (Y )
(5.40)
X∩Y =∅
The BBA given by m(Ω) = 1 does not provide any information. Accordingly, it
has no effect when combined with another BBA according to (5.40). Therefore,
m(Ω) = 1 is also called neutral BBA.
In the remaining part of this theses, BBAs describe the state of a grid cell with the
frame of discernment containing the elements occupied and free: Ω = {O, F }. The
combination rule (5.40) then combines two independent BBAs m1 and m2 when
they reason about the state of the same grid cell c.
In contrast, grid maps in the probability domain combine two independent binary
occupancy probability functions p1 and p2 reasoning about the state of grid cell c
with the binary Bayes filter (3.6).
5.2 Real-time Approximation using Dempster-Shafer Theory of Evidence
69
The Dempster-Shafer rule of combination (5.40) relates to the binary Bayes filter
(3.6) as follows: Proposition: Let Ω = {O, F } be the frame of discernment for two
independent BBAs m1 and m2 with
m1 (Ω) = m2 (Ω) = 0
(5.41)
mi (A) > 0 for all i ∈ {1, 2}, A ∈ {O, F }.
(5.42)
and
Further, let p1 and p2 be the probability functions resulting from the pignistic
transformation (5.37) of m1 and m2 , respectively and let pBBF be the probability
function resulting from combining p1 and p2 with the binary Bayes filter (3.6).
Describing the other processing path, let m1⊕2 be the BBA resulting from combining
m1 and m2 with the Dempster-Shafer rule of combination (5.40) and let p1⊕2 be the
probability function resulting from the pignistic transformation (5.37) of m1⊕2 .
Then pBBF equals p1⊕2 . This means: applying the pignistic transformation (5.37)
on m1 and m2 and combining the resulting probability functions p1 and p2 with the
binary Bayes filter (3.6) is equivalent to combining m1 and m2 with the DempsterShafer rule of combination (5.40) to m1⊕2 and applying the pignistic transformation
(5.37) on m1⊕2 if conditions (5.41) and (5.42) hold.
Proof: From (5.41) it follows with axiom (5.24) that
mi (A)
(5.24)(5.41)
=
1 − mi (B) for all i ∈ {1, 2}, A, B ∈ {O, F }, A 6= B.
(5.43)
Therefor the pignistic transformation (5.37) of m1 and m2 leads to
pi (A)
(5.37),(5.43)
=
mi (A) for all i ∈ {1, 2}, A ∈ {O, F }.
(5.44)
Inserting (5.44) into the binary Bayes filter (3.6) leads to
pBBF (A)
for all A ∈ {O, F }.
m1 (A) · m2 (A)
m1 (A) · m2 (A) + (1 − m1 (A)) (1 − m2 (A))
m1 (A) · m2 (A)
=
.
2 · m1 (A) · m2 (A) − m1 (A) − m2 (A) + 1
(3.6),(5.44)
=
(5.45)
70
Particle Realization and Dempster-Shafer Approximation
Combining m1 and m2 with the Dempster-Shafer rule of combination (5.40) considering (5.41) provides
m1⊕2 (A) = m1 (A) ⊕ m2 (A)
m1 (A) · m2 (A)
1 − m1 (A) · m2 (B) − m1 (B) · m2 (A)
m1 (A) · m2 (A)
(5.43)
=
1 − m1 (A) · (1 − m2 (A)) − m2 (A) · (1 − m1 (A))
m1 (A) · m2 (A)
=
2 · m1 (A) · m2 (A) − m1 (A) − m2 (A) + 1
(5.40),(5.41)
=
(5.46)
for all A, B ∈ {O, F }, A 6= B and
m1⊕2 (A)
(5.41),(5.42)
=
0
(5.47)
for all A ∈ {∅, Ω}.
Finally, applying the pignistic transformation (5.37) on m1⊕2 considering (5.47)
leads to
p1⊕2 (A)
(5.37),(5.47)
=
for all A ∈ {O, F }.
m1⊕2 (A)
(5.46)
=
m1 (A) · m2 (A)
2 · m1 (A) · m2 (A) − m1 (A) − m2 (A) + 1
(5.48)
5.2 Real-time Approximation using Dempster-Shafer Theory of Evidence
71
5.2.2 State Representation with Dempster-Shafer
Approximation
The DS-PHD/MIB filter represents the occupancy state of a grid cell with a BBA
m : 2Ω → [0, 1]. The frame of discernment Ω contains the events occupied and free:
Ω = {O, F }. Thus, each grid cell stores a mass for occupied m(O) and a mass for
free m(F ). Further information about the application of DST to grid maps can be
found in [KD11; MCB11; NWW+ 13].
The DS-PHD/MIB filter represents the posterior state of an individual grid cell c at
(i,c) ν
(i,c)
(c)
(c)
k
time k with the particle set {xk , wk }i=1
and the mass for free mk (Fk ). Here,
the sum of particle weights represents the mass for occupied:
(c)
(c)
mk (Ok ) =
νk
X
(i,c)
wk
.
(5.49)
i=1
(c)
The occupancy probability po,k (O) in a grid cell is given by the pignistic transformation (5.37), which in this case results in
(c)
(c)
(c)
(c)
po,k (Ok ) = mk (Ok ) + 0.5 · (1 − mk (Ok ) − mk (Fk )).
(5.50)
(c)
The distribution of the particles represents the spatial PDF pk (xk ) of a point object
in grid cell c:
(c)
(c)
pk (xk )
=
1
νk
X
(c)
mk (Ok ) i=1
(i,c)
wk
(i,c)
δ(xk − xk
).
(c)
(5.51)
(c)
Note that the prior BBA of unobserved grid cells is given by mk (Ok ) = mk (Fk ) = 0.
Considering definition (5.49), no particles are present in unobserved areas. Particles
are only present in grid cell with an occupancy mass greater zero. The DS-PHD/MIB
filter concentrates particles in areas where obstacles have been detected and thus
increases the particle density in occupied areas. This is an advantage in comparison
to the original PHD/MIB filter because in practical applications, the number of
particles is limited.
This comes with the disadvantage that the absence of particles does not represent
free space. In contrast, free space has to be modeled independently with the explicit
mass for free space. The problem of explicit free space representation is revealed in
the prediction step as will be outlined in the next section.
72
Particle Realization and Dempster-Shafer Approximation
5.2.3 Prediction Step with Dempster-Shafer Approximation
The DS-PHD/MIB filter applies (4.4) and (5.4) to predict particles to the next time
(i,c)
(i,c) ν
(c)
p,+
step. In analogy to the PHD/MIB filter (5.6), let {xp,+ , wp,+ }i=1
be the set of
particles predicted into cell c at time step k + 1. Again, the predicted weights are
truncated, so the sum of predicted weights in one grid cell is limited to 1.
The DS-PHD/MIB filter estimates the predicted occupancy mass of grid cell c by
(c)
νp,+
(c)
mp,+ (Ok+1 )
=
X
(i,c)
wp,+ .
(5.52)
i=1
The predicted mass for free is modeled as in a static grid map and given by
h
i
(c)
(c)
(c)
mp,+ (Fk+1 ) = min α(T )mk (Fk ) , 1 − mp,+ (Ok+1 ) ,
(5.53)
α(T ) ∈ [0, 1].
Here, the discount factor α(T ) models the decreasing prediction reliability, depending
on the time interval T between two update steps. Since the sum of masses cannot
exceed 1, the predicted free space evidence is limited accordingly.
5.2.4 Update Step with Dempster-Shafer Approximation
The PHD/MIB filter considers both the existence probability and the spatial distribution of point objects in a joint Bayesian innovation step, formally derived as
a Bernoulli filter. In contrast, the DS-PHD/MIB filter does not formally derive
the update step, but applies a simplified update step with the goal of modeling the
probabilistic update equations of the PHD/MIB filter as close as possible in the
Dempster-Shafer domain.
The DS-PHD/MIB approximation updates the existence probability of a point object
in grid cell c independently of its spatial distribution. Accordingly, the DS-PHD/MIB
filter expects the following information in each measurement grid cell:
(c)
• The observed occupancy BBA mzk+1 : 2{O,F } → [0, 1],
(c)
• the spatial likelihood function gk+1 (zk+1 |xk+1 ), and
(c)
(c)
• the association probability pA,k+1 between the likelihood function gk+1 (zk+1 |xk+1 )
and the point object.
5.2 Real-time Approximation using Dempster-Shafer Theory of Evidence
73
Figure 5.3: A simple occupancy measurement grid originated by a laser
measurement with multiple beams. The red lines represent laser
beams that hit obstacles at their ends. The grid on the left-hand
side provides an occupancy probability pzk+1 (ok+1 |zk+1 ) for each
grid cell. The middle figure and the figure on the right-hand
side show the same occupancy measurement grid represented
by evidences for occupied mzk+1 (Ok+1 |zk+1 ) (middle) and free
mzk+1 (Fk+1 |zk+1 ) (right), respectively. The pignistic transformation of the Dempster-Shafer grid results in the classical grid
on the left-hand side. The DS-PHD/MIB filter initializes new
c 2015 IEEE.
particles only in green grid cells [NYK+ 15], Figure 5.3 shows an example for a measurement grid with occupancy BBAs. An
(c)
example for a likelihood function gk+1 (zk+1 |xk+1 ) and the association probability
(c)
pA,k+1 in measurement grid cells will be given in Section 5.2.5.
Existence Update
The DS-PHD/MIB filter approximates the existence update by simply combining the
(c)
(c)
predicted BBA mp,+ and the observed BBA mzk+1 of the corresponding measurement
grid cell with the Dempster-Shafer rule of combination (5.40):
(c)
(c)
mk+1 = mp,+ ⊕ m(c)
zk+1 .
(5.54)
74
Particle Realization and Dempster-Shafer Approximation
Birth Model
The DS-PHD/MIB filter splits the mass for occupied into two parts: occupied by a
persistent object and occupied by a new-born object:
(c)
(c)
(c)
mk+1 (Ok+1 ) = %p,k+1 + %b,k+1 .
(5.55)
Assume the PHD/MIB filter updates the state of a point object with a uniformly
(c)
distributed likelihood gk+1 (zk+1 |xk+1 ) and a uniformly distributed clutter density
pcl (zk+1 ) as defined in (4.27). Then the relation between the updated existence
(c)
(c)
probability rb,k+1 of a new-born object and the updated existence probability rp,k+1
of a persistent object results in
h
i
(c)
(c)
(c)
rb,k+1 (4.16) rb,+ (4.9) pB 1 − rp,+
=
=
.
(5.56)
(c)
(c)
(c)
rp,k+1
rp,+
rp,+
Analogously, the DS-PHD/MIB models the relation between masses for a new-born
and a persistent object as
h
i
(c)
(c)
p
1
−
m
B
%b,k+1
p,+
=
.
(5.57)
(c)
(c)
%p,k+1
mp,+
Equation (5.57) is an approximation of the original PHD/MIB relation (5.56). It
neglects the fact that point objects of neighboring, occluded grid cells might enter grid
cell c. The approximation is a consequence of representing only the dynamic state of
occupied grid cells (with an occupancy mass greater zero) with particles and ignoring
the dynamic state of unobserved grid cells. The impact of this approximation on
the estimation result will be discussed further in Section 5.3 and in Section 7.2.3.
Combining (5.55) and (5.57) delivers the resulting posterior masses for a new-born
and a persistent object at time step k + 1:
h
i
(c)
(c)
mk+1 (Ok+1 ) · pB 1 − mp,+ (Ok+1 )
(c)
h
i,
(5.58)
%b,k+1 =
(c)
(c)
mp,+ (Ok+1 ) + pB 1 − mp,+ (Ok+1 )
(c)
(c)
(c)
%p,k+1 = mk+1 (Ok+1 ) − %b,k+1 .
(5.59)
The existence update as outlined so far takes only the observed occupancy BBA
(c)
mzk+1 in a measurement grid cell into account.
5.2 Real-time Approximation using Dempster-Shafer Theory of Evidence
75
The next section outlines how the DS-PHD/MIB considers the spatial likelihood
(c)
(c)
function gk+1 (zk+1 |xk+1 ) and the association probability pA,k+1 in a measurement
grid cell.
Spatial Update
The DS-PHD/MIB filter provides three particle sets to approximate the posterior
(c)
spatial distribution pk+1 (xk+1 ).
The first particle set represents a persistent object and results from the set predicted
(i,c) ν
(i,c)
(c)
p,+
into grid cell c, denoted as {xp,+ , wp,+ }i=1
. It is updated by multiplying the
(c)
weights with the spatial measurement likelihood gk+1 (zk+1 |xk+1 ). This leads to the
unnormalized updated weights
(i,c)
(c)
(i,c)
(i,c)
w
ep,k+1 = gk+1 (zk+1 |xp,+ ) · wp,+ .
(5.60)
The particle states remain unchanged:
(i,c)
(i,c)
xp,k+1 = xp,+ .
(5.61)
The normalized particle weights are given by
(i,c)
(c)
(c)
(i,c)
(c)
(c)
(i,c)
wp,k+1 = pA,k+1 · µA · w
ep,k+1 + 1 − pA,k+1 · µA · wp,+ ,
(5.62)
with
−1
(c)
X

(i,c) 
(c)
=
w
ep,k+1  · %p,k+1

νp,+
(c)
µA
(5.63)
i=1
and
−1
(c)
X

(c)
(i,c) 
=
wp,+  · %p,k+1 =

νp,+
(c)
µA
i=1
(c)
%p,k+1
.
(c)
(5.64)
mp,+ (Ok+1 )
(c)
Equation (5.62) considers that with a probability of (1 − pA,k+1 ), the likelihood
(c)
(i,c)
function gk+1 (zk+1 |xp,+ ) is not associated with the point object in the grid cell. In
76
Particle Realization and Dempster-Shafer Approximation
this case, the weight update and normalization step serves solely to normalize the
predicted particle weights in such a way that they sum up to the posterior persistent
(c)
occupancy mass %p,k+1 .
The second and third particle sets represent new-born objects. For computational
efficiency reasons, they are only created in grid cells where the corresponding
(c)
measurement grid cell reports a mass for occupied: mzk+1 (Ok+1 ) > 0.
This is analogous to the adaptive birth intensity of the standard PHD filter, presented
in Section 2.2.5.
(i,c)
ν
(i,c)
(c)
A,k+1
The second particle set {xA,k+1 , wA,k+1 }i=1
represents a new-born object under
(c)
the assumption that the spatial measurement zk+1 in grid cell c is associated to the
point object in grid cell c. The particles are sampled from the probability density
(c)
(c)
(c)
function pb,zk+1 (xk+1 |zk+1 ) of the state xk+1 given the measurement zk+1 in grid
cell c:
(i,c)
(c)
(c)
xA,k+1 ∼ pb,zk+1 (·|zk+1 ).
(5.65)
The weight of each particle in the second set can directly be calculated to
(c)
(i,c)
wA,k+1 =
(c)
pA,k+1 · %b,k+1
(c)
.
(5.66)
νA,k+1
(c)
(c)
Details about creating a probability density function pb,zk+1 (xk+1 |zk+1 ) of the state
(c)
xk+1 given the measurement zk+1 can be found in [Ris13], p. 38.
(i,c)
(i,c)
ν
(c)
A,k+1
The third particle set {xA,k+1 , wA,k+1 }i=1
represents a new-born object under
(c)
the assumption that the spatial measurement zk+1 in grid cell c is not associated to
the point object in grid cell c. The particles of this set are sampled from the prior
birth distribution pb (xk+1 ):
(i,c)
xA,k+1 ∼ pb (·)
The weight of each particle in the third set can directly be calculated to
(c)
(c)
1 − pA,k+1 · %b,k+1
(i,c)
wA,k+1 =
.
(c)
νA,k+1
(5.67)
(5.68)
5.2 Real-time Approximation using Dempster-Shafer Theory of Evidence
(c)
77
(c)
An example how to choose the individual particle numbers νA,k+1 and νA,k+1 in
each grid cell will be presented in Section 6.2.
Finally, the posterior spatial state distribution of the point object in grid cell c at
time k + 1 is given by
(c)
(c)
pk+1 (xk+1 )
(i,c)
=
(i,c) ν
1
νk+1
X
(c)
mk+1 (Ok+1 ) i=1
(i,c)
(i,c)
wk+1 δ(xk+1 − xk+1 ),
(5.69)
(c)
k+1
where the set {xk+1 , wk+1 }i=1
is the union of all three particle sets created in the
spatial update step. The grid cell additionally stores the posterior mass for free
(c)
mk+1 (Fk+1 ) as calculated in (5.54), which completes the posterior state together
with the particle set.
The resampling step is identical to the resampling step of the original PHD/MIB
filter.
5.2.5 Practical Example: Radar Doppler Measurements
This section gives an example for calculating measurement grids from radar Doppler
(c)
measurements including likelihood functions gk+1 (zk+1 |xk+1 ) in individual grid cells
+
as suggested by the author in [NYK 15].
Radar sensors generally provide a lower angular resolution than laser scanners
[WHLS15]. However, radar occupancy measurement grids containing masses for
(c)
(c)
occupied mzk+1 (Ok+1 |zk+1 ) and masses for free mzk+1 (Ok+1 |zk+1 ) in individual grid
cells can be generated in a similar way as suggested in Figure 5.3. Radar sensors
are able to provide additional information with the Doppler measurement, resulting
from the radial velocity component of objects with respect to the radar sensor.
Assume the ego movement of a radar sensor over ground is compensated and the
(c)
remaining two-dimensional Doppler measurement vz,k+1 occurs in grid cell c at time
(c)
step k + 1 as shown by the red vector in Figure 5.4. The orientation of vz,k+1 results
from the azimuth value of the radar reflection and the magnitude is provided as
Doppler information by the radar sensor. An example for real-world radar reflections
of a Delphi single beam monopulse (SBMP) radar is shown as part of the evaluation
in Chapter 7, Figure 7.2.
78
Particle Realization and Dempster-Shafer Approximation
(c)
Figure 5.4: A Doppler measurement vector vz,k+1 (red) and possible orig(i,c)
c 2015
inating velocity hypotheses vk+1 (blue) [NYK+ 15], IEEE.
(i,c)
Figure 5.5: The measurement residual γk+1 (green) between a Doppler mea(c)
surement vector vz,k+1 (red) and a particle’s predicted velocity
(i,c)
c 2015 IEEE.
vector vk+1 (blue) [NYK+ 15], (i,c)
(c)
Possible velocity hypotheses vk+1 leading to the Doppler measurement vz,k+1 are
depicted in blue in Figure 5.4.
(i,c)
(c)
(i,c)
(i,c)
The goal is to calculate the likelihood Λ(xp,+ ) = gk+1 (zk+1 |xp,+ ) of the state xp,+
of the particle i with velocity
(i,c)
vk+1 .
The projection of the two-dimensional particle
(i,c)
(c)
(i,c)
velocity vk+1 on the two-dimensional Doppler measurement vz,k+1 is denoted vk+1||z
(i,c)
(see Figure 5.5). The measurement residual γk+1 is the absolute distance between
(i,c)
the projected particle velocity vk+1||z and the Doppler measurement:
(i,c)
(i,c)
(c)
γk+1 = vk+1||z − vz,k+1 .
(5.70)
5.3 Summary of Approximations
79
Assume the radial velocity measurement uncertainty is given by an unbiased, Gaussian
(i,c)
PDF with standard deviation σr . Then the likelihood Λ(xp,+ ) for particle i is given
by
!
(i,c)
γ 2 k+1
1
(i,c)
(c)
(i,c)
Λ(xp,+ ) = gk+1 (zk+1 |xp,+ ) = p
,
(5.71)
exp −
2σr2
2πσr2
if the orientation uncertainty of the two-dimensional Doppler measurement is neglected.
If Doppler measurements are sparse in comparison to the grid cell size, it is beneficial
to consider the closest Doppler measurement for grid cell c, even if it did not occur
exactly inside the cell. In this case, it is unclear if the measurement is associated with
the point object in grid cell c. This can be modeled by decreasing the association
(c)
probability pA,k+1 with increasing distance of the closest Doppler measurement to
the grid cell. In case of dense radar reflections and if several radar measurements
might overlap with one grid cell, the Doppler likelihood functions can be combined
to a function of weighted Gaussian mixtures.
5.3 Summary of Approximations
The chapter presented the particle realization of the PHD/MIB filter and an approximation called DS-PHD/MIB filter using the Dempster-Shafer theory of evidence.
Throughout Chapters 4 and 5, several assumptions and approximations have been
introduced for both the PHD/MIB filter and the DS-PHD/MIB filter. An advantage of deriving the PHD/MIB filter in the RFS domain is that the assumptions
and approximations can be clearly stated. Furthermore, even the potential effects
of those approximations can be estimated. As an example, the PHD filter has
been vastly investigated in literature and its behaviour in practical applications
is well-understood [MRSD14; Ris13; VSD05]. Table 5.2 summarizes the essential
assumptions and approximations in the PHD/MIB filter.
Additional assumptions in the PHD/MIB filter to those listed in Table 5.2 are that
the process noise is uncorrelated over time, the measurement noise is uncorrelated
over time and that process noise and measurement noise are independent. These
noise properties are commonly assumed by Bayesian filters.
The Bernoulli filter update step for an individual grid cell as given by (4.17) is the
exact solution of the multi-object state estimation problem defined by the predicted
Bernoulli state (4.10) and the Bernoulli observation process (4.12).
80
Particle Realization and Dempster-Shafer Approximation
The particle realization of the PHD/MIB filter as presented in Chapter 2.1.3 is
again an approximation, as all particle realizations are. With an infinite number of
particles, the particle realization converges to the general PHD/MIB filter.
A problem in the online application of the PHD/MIB filter is the huge number of
particles required to model potential objects in unobserved areas. To mitigate this
effect, the Dempster-Shafer PHD/MIB (DS-PHD/MIB) filter splits the occupancy
information in one grid cell into an evidence mass for occupied and an evidence
mass for free. Unobserved grid cells contain zero evidence for occupied or free. The
DS-PHD/MIB filter then only initializes particles in grid cells with an occupancy
mass greater zero and not in unobserved areas. Because the dynamic behavior of
potential objects in unobserved areas is no longer considered, the DS-PHD/MIB
is a simplification of the original PHD/MIB filter. The additional approximations
introduced by the DS-PHD/MIB filter are summarized in Table 5.3.
5.3 Summary of Approximations
81
Table 5.2: Approximations and assumptions in the PHD/MIB filter.
Estimation step
Assumption/ Approximation
Impact on estimation result
Environment Definition
The world consists of independent point objects in the size
of grid cells as defined in Section 4.1.1.
Point objects are correlated,
because groups of them build
rigid entities like traffic participants. The environment
model does not consider this
fact.
As an effect, the
PHD/MIB filter might estimate inconsistent velocities
for a group of point objects
representing one vehicle. The
effect will be further investigated in the evaluation in Section 7.2.3.
Posterior multi-object state
representation and prediction
The PHD/MIB filter represents the posterior multiobject state distribution of
point objects by its PHD and
applies the PHD filter prediction step as outlined in Section 4.2.1.
The PHD filter prediction
step is unable to consider any
object interdependency. As
an effect, point objects can
cross through each other or
collide into each other during
the prediction step, which is
an unrealistic behavior. Furthermore, a well-studied characteristic of the PHD filter
is that the estimated object
cardinality is heavily fluctuating and objects quickly dissolve after few missed detection. This behavior motivated
an extension called CPHD filter [Ris13].
Transformation from PHD
representation into multiple
instances of Bernoulli RFSs
The PHD/MIB filter transforms the predicted PHD
into multiple instances of
Bernoulli RFSs, one for each
grid cell. This is outlined in
Section 4.2.1.
The assumption behind this
transformation is that not
more than one point object
can enter and occupy one grid
cell during one time step. For
the most cases, this is a realistic assumption. However, depending on the size of the grid
cells, two small traffic participants like pedestrians might
be able to enter the same grid
cell in the real world. The effect of the assumption in this
case is a wrong cardinality estimation of one point object
instead of two in the grid cell.
82
Particle Realization and Dempster-Shafer Approximation
Table 5.3: Additional approximations and assumptions introduced by the
DS-PHD/MIB filter.
Estimation step
Assumption/ Approximation
Impact on estimation result
Prediction step
The DS-PHD/MIB filter
omits the prediction of
potential dynamic objects in
unobserved areas.
The DS-PHD/MIB filter represents the dynamic state of
a grid cell with particles only
if it holds a Dempster-Shafer
mass greater zero for the event
"occupied" as stated in (5.52).
Unobserved grid cells contain
no particles. As a result, the
filter does not model that potential objects in unobserved
areas might enter the free
space during the prediction
step.
Birth model
The DS-PHD/MIB birth
model (5.57) only considers
the predicted mass for "occupied" in a grid cell but does
not distinguish between free
cells and unobserved cells.
The DS-PHD/MIB filter neglects that point objects predicted into unobserved areas
compete with other, potentially existing point objects.
This is problematic if particles
move along stationary, elongated objects, appearing in
the visible field in a consistent pattern. Due to the lack
of competing particles in the
unobserved areas, the particles have a higher chance to
survive, causing false positive
motion estimations. This issue will be discussed in detail
in Section 7.2.3 and is visualized in Figure 7.7.
Observation model
The DS-PHD/MIB neglects
clutter distributions of measurements as stated in (5.60)
and (5.62).
The original PHD/MIB filter
takes the clutter PDF of measurements into account. Since
clutter distributions are often
unknown in practice, the DSPHD/MIB filter only considers the association probability between a grid cell and a
measurement, instead. Consequently, measurement outliers
have a potentially higher impact in the DS-PHD/MIB filter in comparison to the original PHD/MIB filter, which
puts the likelihood function
into relation to the clutter
density, as defined by (4.17).
Chapter 6
Parallel Implementation
This chapter describes an implementation of the particle-based DS-PHD/MIB filter
with a focus on massively parallel processing systems such as graphics processing
units.
6.1 Parallelization Challenges
Particles can naturally be processed in parallel, but a challenge is to assign each
particle to its corresponding grid cell in an efficient way. The assignment is necessary
to predict the grid cell occupancy mass (5.49) and to calculate the normalization
factor during the update step (5.62). Another challenge is to calculate statistical
moments of grid cells like mean and variance of the velocity in a balanced way: the
calculation time should be independent of the number of particles assigned to a grid
cell.
The proposed solution sorts the particles after the prediction step according to
the grid cell index they have been predicted into. The time complexity of sorting
is O(n log n) in regard to the number of particles n. Although the parallelization
potential of sorting is somewhat limited, there are sophisticated sorting algorithms
capable of achieving a high throughput especially on massively parallel architectures
[SHG09]. The availability of a sorted particle array has several advantages: First,
the assignment of sorted particles to grid cells is straightforward as will be detailed
below. Furthermore, particle state values can be efficiently accumulated in separate
arrays, similar to an integral image data structure [VJ01]. This facilitates calculation
of a grid cell’s statistical moments with a computational complexity independent of
the number of particles assigned to the cell.
84
Parallel Implementation
Another advantage of sorting particles is a simple overall implementation since all
remaining advanced problems can then be solved with standard routines. Highly efficient parallel implementations of these routines are available for graphics processing
units, where for the parallel implementation of the particle-based DS-PHD/MIB filter
sampling of random numbers [NVI16], sorting [HB16] and accumulation [HB16]have
been employed.
6.2 Parallel Algorithm
In the following, implementation details of the proposed parallel algorithm are
described. The auxiliary data structures rendering the algorithm particularly efficient
are given as follows. All particles and grid cells are stored in the arrays particle_array
and grid_cell_array, respectively. Assume a measurement grid map with the same
dimensions as the grid map is already available and measurement grid cells are stored
in the array meas_cell_array. An example for efficiently parallelized measurement
grid calculation can be found in [HKOB10]. For modeling noise processes, a sufficient
amount of random numbers is sampled beforehand during idle times and stored in
extra arrays. The parallel DS-PHD/MIB recursion is summarized in Figure 6.1 and
outlined in the following sections.
Particle Prediction
Algorithm 6.1 provides pseudo code for the particle prediction. The algorithm
predicts all particles in parallel applying equations (4.4) and (5.4). This includes
calculating the new grid cell index of each particle after prediction. The appropriate
amount of random numbers should be sampled in a separate step in advance, so the
prediction step just needs to look them up.
Assignment of Particles to Grid Cells
Pseudo code for the particle assignment is given in Algorithm 6.2. First, the algorithm
sorts all particles according to the grid cell index they have been predicted into.
Each grid cell can store two particle indices. They represent the first and last index
of the particle group that has been predicted into the grid cell. For the assignment,
each particle checks if it is the first or the last particle of a group with the same
index. This is true if the right or left neighbor particle holds a grid cell index which
differs from the particle’s own grid cell index. If so, it writes its index into the
6.2 Parallel Algorithm
85
1 Particle Prediction
2 Assignment of Particles to Grid Cells
3 Grid Cell Occupancy Prediction and Update
4 Update of Persistent Particles
5 Initialization of New Particles
6 Statistical Moments of Grid Cells
7 Resampling
Figure 6.1: Overview of the PHD/MIB implementation, broken down into
seven steps.
according grid cell. Since there can only be at most one first or last particle per grid
cell, the assignment can run in parallel without any writing conflicts.
Grid Cell Occupancy Prediction and Update
Algorithm 6.3 details the occupancy update. The goal of this step is to calculate
for each grid cell the predicted and updated occupancy BBA. First, the algorithm accumulates in parallel all particle weights and stores the result in the array
weight_array_accum. The remaining part of the algorithm is carried out in parallel
for all grid cells. Each cell reads two values from weight_array_accum. The first
value is the accumulated particle weight of all preceding grid cells excluding its own
particles, the second value is the accumulated particle weight of all preceding grid
cells including its own particles. The cell simply subtracts the first value from the
(c)
second value to calculate its predicted occupancy mass mp,+ (Ok+1 ) according to
(5.52). Consider that this operation is independent of the number of particles in
one grid cell, so its time complexity is constant. This is optimal with respect to
(c)
load balancing between threads. The predicted free mass mp,+ (Fk+1 ) is calculated
according to (5.53).
particle_array
. This array stores particles including weight and corresponding grid cell index (constant size ν)
grid_cell_array
. This array stores the grid cells (constant size C)
p_S
. Persistence probability of point objects, is a design parameter of the process model
for i ∈ {0, . . . , length(particle_array) − 1} do
. Parallel for loop over all particles
predict(particle_array, i, p_S)
. Applies (4.4) and (5.4), calculates new grid cell index and stores it inside the particle
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
meas_cell_array
. This array stores the measurement grid cells (constant size C)
born_masses_array
. This array will store the mass for a new-born object per grid cell (constant size C)
p_B
. Birth probability of point objects, is a design parameter of the process model
weight_array_accum ← accumulate(weight_array)
. Inclusively accumulates all particle weights to weight_array_accum
for j ∈ {0, . . . , length(grid_cell_array) − 1} do
. Parallel for loop over all grid cells
start_idx ← get_particle_start_idx(grid_cell_array[j])
. Gets start index in particle_array of cell j
end_idx ← get_particle_end_idx(grid_cell_array[j])
. Gets end index in particle_array of cell j
m_occ_pred ← subtract(weight_array_accum, start_idx, end_idx)
. Calculates predicted occupancy mass of cell j (5.52)
m_free_pred ← predict_free_mass(grid_cell_array[j])
. Predicts free mass of cell j (5.53)
m_occ_up ← update_o(m_occ_pred, m_free_pred, meas_cell_array[j])
. Combination to posterior occ. mass (5.54)
m_free_up ← update_f(m_occ_pred, m_free_pred, meas_cell_array[j])
. Combination to posterior free mass (5.54)
rho_b ← separate_newborn_part(m_occ_pred, m_occ_up, p_B)
. Calculate new-born part of posterior occupancy mass (5.58)
rho_p ← m_occ_up − rho_b
. Calculates remaining persistent part of posterior occupancy mass (5.59)
born_masses_array[j] ← rho_b
. Stores new-born part of posterior occupancy mass of cell j in born_masses_array
store_values(rho_b, rho_p, m_free_up, grid_cell_array, j)
. Stores updated BBA in grid cell j
Algorithm 6.3 Grid Cell Occupancy Prediction and Update
1: weight_array
. This array will be an additional storage for the particle weights (constant size ν)
2: sort(particle_array)
. Sorts by the grid cell index the particle has been predicted into
3: for i ∈ {0, . . . , length(particle_array) − 1} do
. Parallel for loop over all particles
4:
j ← get_grid_cell_idx(particle_array[i])
. Reads the grid cell index of the predicted particle
5:
if is_first_particle(particle_array, i) then
. Checks if i is the first particle of a group with same grid cell index
6:
set_particle_start_idx(grid_cell_array, j, i)
. Sets i as particle start index of grid cell j in grid_cell_array
7:
if is_last_particle(particle_array, i) then
. Checks if i is the last particle of a group with same grid cell index
8:
set_particle_end_index(grid_cell_array, j, i)
. Sets i as particle end index of grid cell j in grid_cell_array
9:
weight_array[i] ← get_particle_weight(particle_array[i])
. Copies weight of particle i to weight_array at index i
Algorithm 6.2 Assignment of Particles to Grid Cells
1:
2:
3:
4:
5:
Algorithm 6.1 Particle Prediction
86
Parallel Implementation
for i ∈ {0, . . . , length(particle_array) − 1} do
. Parallel for loop over all persistent particles
weight_array[i] ← update_unnorm(particle_array, i, meas_cell_array)
. Calculates unnormalized weight update (5.60)
weight_array_accum ← accumulate(weight_array)
. Accumulates unnormalized weights of persistent particles
for j ∈ {0, . . . , length(grid_cell_array) − 1} do
. Parallel for loop over all grid cells
start_idx ← get_particle_start_idx(grid_cell_array[j])
. Gets start index in particle_array of cell j
end_idx ← get_particle_end_idx(grid_cell_array[j])
. Gets end index in particle_array of cell j
m_occ_accum ← subtract(weight_array_accum, start_idx, end_idx)
. Accumulated unnormalized updated particle weight of cell j
rho_p ← get_pers_occ_mass(grid_cell_array[j])
. Gets persistent part of posterior occupancy mass of cell j
mu_A ← calc_norm_assoc(m_occ_accum, rho_p)
. Calculates normalization component for an associated measurement (5.63)
mu_UA ← calc_norm_unassoc(grid_cell_array[j])
. Calculates normalization component for an unassociated measurement (5.64)
set_normalization_components(grid_cell_array, j, mu_A, mu_UA)
. Stores components mu_A and mu_UA in grid cell j
for i ∈ {0, . . . , length(particle_array) − 1} do
. Parallel for loop over all persistent particles
weight_array[i] ← normalize(particle_array[i], grid_cell_array)
. Normalizes particle weights (5.62)
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
birth_particle_array
. This array will be the storage for new-born particles for this recursion (constant size νb )
particle_orders_array_accum ← accumulate(born_masses_array)
. Accumulates mass part of new-born object of each cell
normalize_particle_orders(particle_orders_array_accum, νb )
. Normalizes the particle orders to a total number of νb
for j ∈ {0, . . . , length(grid_cell_array) − 1} do
. Parallel for loop over all grid cells
start_idx ← calc_start_idx(particle_orders_array_accum, j)
. Calculates first index in birth_particle_array of cell j
end_idx ← calc_end_idx(particle_orders_array_accum, j)
. Calculates last index in birth_particle_array of cell j
num_new_particles ← end_idx − start_idx + 1
. Stores number of new particles for cell j
p_A ← get_assoc_probability(meas_cell_array[j])
. Reads association probability to spatial measurement of cell j
nu_A ← calc_num_assoc(num_new_particles, p_A)
. Calculates number of new associated particles (6.1) and (6.2)
nu_UA ← num_new_particles − nu_A
. Calculates number of new unassociated particles
w_A ← calc_weight_assoc(nu_A,p_A, born_masses_array[j])
. Calculates weight of an associated new particle (5.66)
w_UA ← calc_weight_unassoc(nu_UA,p_A, born_masses_array[j])
. Calculates weight of an unassociated new particle (5.68)
store_weights(w_A, w_UA, grid_cell_array, j)
. Stores weights for new particles in cell j
for i ∈ {start_idx . . . start_idx + nu_A} do
. For loop over new associated particles of cell j
set_cell_idx_A(birth_particle_array, i, j)
. Sets j as grid cell index of new particle i with flag for associated
for i ∈ {start_idx + nu_A + 1 . . . end_idx} do
. For loop over new unassociated particles of cell j
set_cell_idx_UA(birth_particle_array, i, j)
. Sets j as grid cell index of new particle i with flag for unassociated
for i ∈ {0, . . . , length(birth_particle_array) − 1} do
. Parallel for loop over all new-born particles
initialize_new_particle(birth_particle_array, i, grid_cell_array)
. Initializes new-born particle i according to (5.65) or (5.67)
Algorithm 6.5 Initialization of New Particles
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
Algorithm 6.4 Update of Persistent Particles
6.2 Parallel Algorithm
87
vel_x_array
. Separate storage for the velocity in x-direction of particles (constant size ν)
vel_y_array
. Separate storage for the velocity in y-direction of particles (constant size ν)
vel_x_squared_array
. Separate storage for the squared velocity in x-direction of particles (constant size ν)
vel_y_squared_array
. Separate storage for the squared velocity in y-direction of particles (constant size ν)
vel_xy_array
. Separate storage for the multiplied velocities in both directions of particles (constant size ν)
for i ∈ {0, . . . , length(particle_array) − 1} do
. Parallel for loop over all persistent particles
w ← weight_array[i]
. Stores the updated, normalized weight of the persistent particle i
vel_x ← get_vel_x(particle_array[i])
. Stores x-velocity of particle i
vel_y ← get_vel_y(particle_array[i])
. Stores y-velocity of particle i
vel_x_array[i] ← w ∗ vel_x
. Stores weighted x-velocity of particle i
vel_y_array[i] ← w ∗ vel_y
. Stores weighted y-velocity of particle i
vel_x_squared_array[i] ← w ∗ vel_x ∗ vel_x
. Stores weighted squared x-velocity of particle i
vel_y_squared_array[i] ← w ∗ vel_y ∗ vel_y
. Stores weighted squared y-velocity of particle i
vel_xy_array[i] ← w ∗ vel_x ∗ vel_y
. Stores weighted product of x- and y-velocity of particle i
vel_x_array_accum ← accumulate(vel_x_array)
. Accumulates velocities in x-direction
vel_y_array_accum ← accumulate(vel_y_array)
. Accumulates velocities in y-direction
vel_x_squared_array_accum ← accumulate(vel_x_squared_array)
. Accumulates squared velocities in x-direction
vel_y_squared_array_accum ← accumulate(vel_y_squared_array)
. Accumulates squared velocities in y-direction
vel_xy_array_accum ← accumulate(vel_xy_array)
. Accumulates product of velocities in x- and y-direction
for j ∈ {0, . . . , length(grid_cell_array) − 1} do
. Parallel for loop over all grid cells
rho_p ← get_pers_occ_mass(grid_cell_array, j)
. Gets persistent part of posterior occupancy mass in grid cell j
start_idx ← get_particle_start_idx(grid_cell_array[j])
. Gets start index in particle_array of cell j
end_idx ← get_particle_end_idx(grid_cell_array[j])
. Gets end index in particle_array of cell j
mean_x_vel ← calc_mean(vel_x_array_accum, start_idx, end_idx, rho_p)
. Applies (6.3)
mean_y_vel ← calc_mean(vel_y_array_accum, start_idx, end_idx, rho_p)
. Applies (6.3)
var_x_vel ← calc_variance(vel_x_squared_array_accum, start_idx, end_idx, rho_p, mean_x_vel)
. Applies (6.5)
var_y_vel ← calc_variance(vel_y_squared_array_accum, start_idx, end_idx, rho_p, mean_y_vel)
. Applies (6.5)
covar_xy_vel ← calc_covariance(vel_xy_array_accum, start_idx, end_idx, rho_p, mean_x_vel, mean_y_vel)
. Applies (6.6)
store(grid_cell_array, j, mean_x_vel, mean_y_vel, var_x_vel, var_y_vel, covar_xy_vel)
. Store stochastic moments in grid cell j
1:
2:
3:
4:
5:
6:
7:
rand_array
. Array with sorted, equally distributed random numbers (constant size ν)
idx_array_resampled
. Array with indices of resampled particles (constant size ν)
particle_array_next
. Particle array for the next time step (constant size ν)
joint_weight_array_accum ← accumulate(weight_array, birth_weight_array)
. Accumulates normalized particle weights
idx_array_resampled ← calc_resampled_indeces(joint_weight_array_accum, rand_array)
. Calculates resampled particle indices
for i ∈ {0, . . . , length(particle_array) − 1} do
. Parallel for loop over all persistent particles of the next time step
particle_array_next[i] ← copy_particle(particle_array, birth_particle_array, idx_array_resampled[i])
. Copy resampled particle
Algorithm 6.7 Resampling
1:
2:
3:
4:
5:
6:
7:
8:
9:
10:
11:
12:
13:
14:
15:
16:
17:
18:
19:
20:
21:
22:
23:
24:
25:
26:
27:
28:
29:
Algorithm 6.6 Statistical Moments of Grid Cells
88
Parallel Implementation
6.2 Parallel Algorithm
89
(c)
Each cell reads the observed occupancy BBA mzk+1 from the corresponding measure(c)
ment grid cell and combines it with its predicted occupancy BBA mp,+ according
(c)
to (5.54) to calculate its updated occupancy BBA mk+1 . This enables the grid cell
to separate the posterior occupancy mass
(c)
mk+1 (Ok+1 )
(c)
into the part %p,k+1 for a
(c)
persistent object and the part %b,k+1 for a new-born object according to (5.58) and
(c)
(5.59). Each cell stores the part %b,k+1 for a new-born object in a separate array,
which will be used later to calculate the number of newly drawn particles for this
cell.
Update of Persistent Particles
The update of persistent particles is described in Algorithm 6.4. Each particle has
stored its corresponding grid cell index during the prediction step, which is assumed
the same as the corresponding measurement grid cell index. All persistent particles
calculate in parallel their unnormalized updated weight according to (5.60). These
weights are then accumulated in the array weight_array_accum. Recall that each
grid cell has already stored the index range of its corresponding particles in Algorithm
6.2. Consequently, in a parallel for loop, each grid cell can look up the accumulated
weight of its updated, unnormalized particles in weight_array_accum analogously to
Algorithm 6.3. Each grid cell uses the result to calculate its normalization component
(c)
(c)
µA (5.63) and stores it. The other normalization component µA as given by (5.64)
can directly be calculated and stored in the grid cell. In the next parallel for loop over
all particles, each particle uses the grid map as a lookup table for its normalization
(c)
(c)
components µA and µA and normalizes itself (5.62).
Initialization of New Particles
Algorithm 6.5 depicts pseudo code for the particle initialization. New particles are
stored in the array birth_particle_array. The total number of new particles for
all grid cells νb remains constant over time, which is feasible for many real-time
applications. The goal of this algorithm is to initialize for each grid cell a certain
number of new particles, which is proportional to the new-born part of its updated
(c)
occupancy mass %b,k+1 . Therefore, the algorithm accumulates new-born occupancy
masses of all grid cells in the array particle_orders_array. Then it normalizes each
value of the array to sum up to the discrete number νb .
90
Parallel Implementation
The normalized array then serves as a lookup table to be used by each grid cell to
find out its first and last corresponding index in the array birth_particle_array, as
(c)
well as the individual number of new particles νb,k+1 assigned to grid cell c in this
(c)
time step. Each grid cell splits this number into the number νA,k+1 of new particles
(c)
which are associated to the spatial measurement gk+1 (zk+1 |xk+1 ) and the number
(c)
νA,k+1 of new particles which are not associated. The relation is given by
(c)
νA,k+1
(c)
(c)
=
νA,k+1
pA,k+1
(c)
1 − pA,k+1
,
(6.1)
and
(c)
(c)
(c)
νA,k+1 + νA,k+1 = νb,k+1 .
(6.2)
Each grid cell also calculates and stores the weights for associated (5.66) and
unassociated (5.68) new particles. As a next step, each grid cell iterates over all
its assigned new particles and defines it as an associated or unassociated particle,
respectively, and sets the particle grid cell index. Finally, each particle initializes
itself with a random initial state within its grid cell. Again, enough random numbers
should be sampled in a separate step in advance so the initialization step just needs
to look them up. This time, each particle uses the grid map as a lookup table for its
initial position and weight.
Statistical Moments of Grid Cells
Algorithm 6.6 calculates the first two statistical moments of the two-dimensional
velocity [vx vy ]T in a grid cell, considering all updated, normalized, persistent particles.
Assuming grid cell c contains a certain number of particles, the mean velocity
(c)
component in x-direction v x is given by
(c)
v (c)
x =
1
(c)
%p,k+1
νp,k+1
X
(i,c)
(i,c)
wp,k+1 · vx,p,k+1 ,
(6.3)
i=1
(i,c)
and analogously for the component in y-direction. The symbol vx,p,k+1 denotes the
(i,c)
velocity x-component of a posterior persistent particle xp,k+1 in grid cell c.
6.2 Parallel Algorithm
91
(c)
Recall that %p,k+1 is the persistent part of the posterior occupancy mass, which
(i,c)
equals the sum of updated, normalized weights wp,k+1 of persistent particles in grid
cell c:
(c)
νp,k+1
(c)
%p,k+1 =
X
(i,c)
wp,k+1 .
(6.4)
i=1
(c)
The algorithm estimates the variance of the velocity component in x-direction σ 2 vx
through 1
(c)
(c)
σ 2 vx
=
νp,k+1
1
X
(c)
%p,k+1
2
(i,c)
(i,c)
wp,k+1 · vx,p,k+1 − v (c)
x
i=1
(c)
=
νp,k+1
1
wp,k+1 ·
(c)
%p,k+1
=
(i,c)
X
1
(i,c)
vx,p,k+1
2
(i,c)
− 2 · vx,p,k+1 · v (c)
x
+
v (c)
x
2
i=1
(c)
νp,k+1
X
(c)
%p,k+1
2
2
(i,c)
(i,c)
wp,k+1 · vx,p,k+1
− v (c)
.
x
(6.5)
i=1
(c)
The estimation for the variance of the velocity component in y-direction σ 2 vy follows
analogously.
(c)
The covariance between the velocity x- and y-components σvx vy is estimated by
(c)
σv(c)
x vy
=
1
X
(c)
%p,k+1
=
νp,k+1
1
(c)
%p,k+1
(i,c)
(i,c)
(i,c)
(c)
wp,k+1 · vx,p,k+1 − v (c)
·
v
−
v
x
y
y,p,k+1
i=1
(c)
νp,k+1
X
(i,c)
(i,c)
(i,c)
(i,c)
(i,c)
(i,c)
(c)
(c)
wp,k+1 · vx,p,k+1 · vy,p,k+1 − 2 · v (c)
+ v (c)
x · vy
x · vy
i=1
(c)
=
1 It
1
(c)
%p,k+1
νp,k+1
X
(c)
wp,k+1 · vx,p,k+1 · vy,p,k+1 − v (c)
x · vy .
(6.6)
i=1
should be mentioned that (6.5) provides a biased variance and (6.6) provides a biased covariance
of the weighted particle samples. The bias increases with a decreasing number of particles
in both cases. However, the calculation of the unbiased variance and unbiased covariance of
weighted particle samples is computationally more expensive. In practical applications only the
statistical moments of grid cell with a certain number of particles are accepted as valid results,
which is why Algorithm 6.6 applies the presented approximation.
92
Parallel Implementation
Since particles are sorted by their grid cell index, the calculation of all sums in
equations (6.3), (6.4), (6.5), and (6.6) can be realized by parallel accumulation of
the according values. A parallel for loop over all grid cells then only subtracts the
corresponding accumulated values. Again, the time complexity is constant for each
cell, independent of the individual number of particles in the cell at time step k + 1.
Consequently, threads executing this calculation in parallel for all grid cells are
perfectly balanced.
Resampling
Particles are resampled according to Algorithm 6.7 to avoid degeneration. The
resampling step accumulates the normalized weights of persistent and new-born
particles in the array joint_weight_array_accum. It draws ν sorted random numbers
which are equally distributed between 0 and the sum of all particles and stores the
random numbers in the array rand_array. Each random number falls into a certain
interval of accumulated weights, which corresponds to a certain particle index. For
each random number in rand_array, the corresponding particle is chosen and copied
into the particle array for the next time step particle_array_next.
Chapter 7
Evaluation
The chapter evaluates the Dempster-Shafer approximation of the PHD/MIB filter
with real-world sensor data and investigates if the DS-PHD/MIB filter performs as
expected in different scenarios. The first part of the chapter describes the test vehicle,
which is used for sensor data recording. The second part qualitatively evaluates and
discusses characteristic properties of dynamic grid mapping as a general approach
for state estimation of dynamic vehicle environments in comparison to object-based
tracking methods. The third part quantitatively evaluates the velocity estimation
performance and consistency of the DS-PHD/MIB filter, its ability to separate static
and dynamic obstacles and its computation time in dependence of the number of
particles used.
7.1 Experiment Setup
A test vehicle records sensor data for the qualitative and quantitative evaluation.
All experiments on the DS-PHD/MIB filter use a fixed parameter set as described
below if not explicitly stated otherwise.
7.1.1 Test Vehicle
The test vehicle is equipped with a Valeo four-layer laser scanner with a horizontal
field of view of 145 degrees and a horizontal resolution of 0.25 degrees in the front
bumper as shown in Figure 7.1. Further sensor properties of the laser scanner are
depicted in Table 7.1.
94
Evaluation
Figure 7.1: Sensor setup, not true to scale. The image shows the opening
angles of the Delphi SBMP in the front left bumper (red) and
the front right bumper (blue) and the Valeo laser scanner in the
front center bumper (yellow).
Table 7.1: Ibeo ScaLa sensor features.
Parameter
Horizontal field-of-view
Horizontal resolution
Vertical field-of-view
Number of vertical layers
Distance resolution
Typical range passenger car
Value
145 deg
0.25 deg
3.2 deg
4
< 0.1 ms
150 m
The grid map in the experiments will be limited to a radius of 60 m. Within this
range, vehicles are expected to reflect several laser points. Exemplary laser sensor
data is depicted in Figure 7.2.
Additionally, two short range Delphi SBMP radars are attached to the front left and
front right sides. One radar provides up to 64 targets within a limited range of 40 m
and a horizontal field of view of 75 degrees. The radar provides the two-dimensional
position of a target and its radial velocity relative to the radar sensor. The vehicle
speed and yaw rate are available via CAN messages, so the ego movement of a radar
sensor can be compensated. Further sensor properties of the radar scanners are
depicted in Table 7.2. The target list of the Delphi radar is sparser than the laser
point cloud as shown in Figure 7.2.
7.1 Experiment Setup
95
Figure 7.2: Sensor data from Delphi SBMP in front left bumper (red) and
front right bumper (blue) and Valeo laser scanner in front center
bumper (yellow).
Here, the two-dimensional position of a radar target is shown as a dot and the radial
velocity is drawn as a line. The length of the line is proportional to the absolute
value of the radial velocity.
7.1.2 Filter configuration
The parameter set of the DS-PHD/MIB filter is designed for a typical urban vehicle
environment scenario consisting of static obstacles, pedestrians, bikes, cars, trucks
and other traffic participants. The grid map covers an area of 120 m by 120 m with
the test vehicle in the center. Each grid cell measures 10 cm by 10 cm.
Table 7.3 shows the parameter set of the DS-PHD/MIB filter. The parallel implementation is tested on an Nvidia GTX980 GPU, supported by a single core of an
Intel i7 processor.
96
Evaluation
Table 7.2: Delphi SBMP sensor features.
Parameter
Frequency band
Horizontal field-of-view
Range
Range rate
Accuracy range
Accuracy range rate
Accuracy angle
Distance resolution
Value
76 to 77 GHz
75 deg
0.15 to 40 m
−55 to 50 m/s
±0.1 m
±0.07 m/s
±1 deg
< 0.1 ms
Table 7.3: Overview of the experimental parameters of the DS-PHD/MIB
filter.
Parameter
Grid map edge size
Grid cell edge size
Number of consistent particles
Number of new-born particles
Persistence probability
Standard deviation velocity new-born particles
Standard deviation process noise position
Symbol
ν
νb
pS
σB,v
σp
Standard deviation process noise velocity
σv
Birth probability
pB
Value
120 m
0.1 m
2 · 106
2 · 105
0.99
4 m/s
m
0.02 T/
s
0.8
m/s
T/s
0.02
7.2 Qualitative Evaluation
Representing the environment as a grid map offers some advantages and some
disadvantages in comparison to object-based representations [DRN15]. This section
presents and discusses characteristics of dynamic grid maps in general and of the
DS-PHD/MIB filter in particular in a qualitative manner.
7.2.1 Free-form Obstacle Representation in Dynamic
Environments
The remaining part of the thesis visualizes the estimation result of the DS-PHD/MIB
filter for a specific time step k as presented in Figure 7.3. In this traffic scenario, the
test vehicle waits at an intersection and observes a cyclist (1) and three cars (2-4).
7.2 Qualitative Evaluation
97
The visualization determines the brightness of a grid cell based on its occupancy
probability. Occupied grid cells are black, free grid cells are white and unknown
grid cells are grey. Figure 7.3 reveals the assumed opening angle of the laser scanner
with the bright free space area. The radar sensors don’t estimate free space but
only occupancy in this application. The reason is that realistic free space models for
radars are much more complicated than free space models for lasers. Furthermore,
the main advantage of radars lies in the Doppler measurement as will be outlined
later.
The estimated, two-dimensional mean velocity v in each grid cell as calculated by
(6.3) is represented by a thin, black vector if the occupancy probability of the grid
cell exceeds a certain threshold. The cyclist in Figure 7.3 is slower than the cars,
identifiable by its shorter velocity vectors. The static obstacles all show small velocity
vectors without dominant orientation. The quantitative evaluation will examine
the velocity estimation of the DS-PHD/MIB filter in detail in Section 7.3.1. The
velocity vectors don’t reveal their corresponding variance. This is to a certain degree
visualized with the color saturation of a grid cell. The visualization calculates the
Mahalanobis distance m0 between the estimated two-dimensional probability density
p(v) : R2 → R of the velocity distribution in a cell and the velocity v0 = 0. The
density is approximated from the particle representation as a Gaussian distribution
with mean v and covariance matrix P as calculated in (6.3), (6.5), (6.6). Then the
Mahalanobis distance m0 is given by
m0 = vP −1 v T .
(7.1)
The color saturation increases with the Mahalanobis distance m0 . Any grid cell with
a Mahalonobis distance greater than the limit mS draws itself with full saturation.
This thesis visualizes grid maps with mS = 4 because experiments showed this value
helps the human eye distinguish dynamic and stationary obstacles.
The grid representation shows some advantages and some disadvantages in comparison to object-based representations [DRN15]. One advantage is that the grid
map unifies the representation of occupied, free and occluded areas. For many
applications it is important to distinguish between free space and unobserved areas.
Many multi-object tracking approaches cluster sensor data like laser reflections in
a preprocessing step, especially approaches based on multiple instances of Kalman
filters [MDM10; WTT+ 07]. This involves the risk that reflections from one object
are separated, or reflections from different objects are combined to one cluster, which
can affect the tracking performance. In contrast, the dynamic grid does not cluster
sensor data in a preprocessing step but simply reasons on the state of grid cells
based on measurements.
98
Evaluation
Figure 7.3: Exemplary dynamic vehicle environment with bike (1) and cars
(2-4).
Some multi-object tracking approaches also don’t cluster data, e.g. particle based
approaches [Pet11] or optimization methods like data driven Markov chain Monte
Carlo [VA09]. However, these approaches still rely on predefined models for object
shapes. Common types of predefined object shapes are cuboids [Pet11; VA09] or
ellipsoids [Koc08]. If the appearance of traffic participants differs from the shape
model, it might impair the tracking result. Especially static obstacles in the vehicle
environment show all kind of shapes, which is why many object tracking approaches
only focus on dynamic objects and rely on an additional grid map for representing
static obstacles [NSD14], [WTT+ 07].
However, also dynamic objects can appear in various shapes. An example for a
complex outline of a dynamic obstacle is shown in Figure (7.4). In this scenario, the
test vehicle is waiting at a red traffic light. A truck (1) is turning left, showing a
complex outline of its front part as seen from the laser scanner perspective. The
grid map does not rely on any predefined object shape models. Consequently, the
grid map is able to represent the front part of the truck, unaffected by the complex
shape.
7.2 Qualitative Evaluation
99
Figure 7.4: Exemplary dynamic vehicle environment with truck (1) and cars
(2-3).
In this case, the medium part of the truck is too high above ground to be detected
by the laser scanner, which might be problematic for some applications, but this is a
general problem of the used sensor setup.
Both Figure 7.3 and Figure 7.4 give examples for complex traffic scenarios with
dynamic and static obstacles. A dynamic grid map represents the environment and
its dynamic state in a unified form representing dynamic and static objects, free
space and occluded areas. An advantage of object-based representations is that many
approaches assign a unique ID to every object [MD11; RVVD14], which remains
constant over time. Also, many applications rely on an object-based representation
for situation interpretation [KNW+ 15; Urm+ 08]. In order to provide an objectbased representation, post-processing steps like clustering would be necessary as an
extension to the presented dynamic grid mapping approach.
100
Evaluation
It is worth mentioning that emerging object tracking methods estimate free-form
shapes with polygons to avoid the discussed problems with predefined object shapes
[HSRD16; VDN15]. Up to this day it is extremely challenging to use these methods in
practical applications due to their high computational load. However, the approaches
are potentially powerful and might become applicable in the future when availability
of computational power increases.
7.2.2 Heterogeneous Sensor Data Fusion
A further, specific advantage of grid-based environment representations is its simple
solution to association problems. For many multi-object tracking approaches, data
association is a core challenge [Mah07b; ML08]. The grid map indirectly addresses
the problem by simply associating measurements to grid cells based on spatial
relations.
The dynamic grid map in particular benefits from heterogeneous sensor data. The
test vehicle is equipped with laser and radar sensors. The spatial precision of the grid
maps depicted in Figure 7.3 and Figure 7.4 is a result of the low spatial measurement
uncertainty of the laser scanner. Figure 7.5 gives an example on how the Doppler
measurements of the radar sensors contribute to the overall performance of the
dynamic grid map.
In this situation, the test vehicle waits at a crossing behind a leading vehicle, which
changes its dynamic state from standing to moving. The time line increases from left
to right in Figure 7.5 as can be seen in the video image. The upper grid map time line
row shows the result of the dynamic grid map based on laser-only measurements. The
lower grid map time line row shows the result of fusing laser and radar measurements
in the grid map.
In the second time step, the leading vehicle just started moving. The laser-only
grid map indicates almost no movement, since there is a great overlap of occupied
grid cells between the first time step and the second time step. In contrast, the
radar Doppler measurement instantaneously detects the motion as clearly visible
in the second time step in the lower grid map row. The quantitative evaluation in
Section 7.3.1 examines in detail the influence of the radar Doppler measurement on
the speed estimation convergence rate.
7.2 Qualitative Evaluation
101
Figure 7.5: The time line shows four time steps with a leading vehicle starting
to move with increasing time from left to right. The upper grid
row shows the laser-only grid map, the lower grid row shows
the fusion result of laser and radar sensor data in the grid map.
Fusion with radar improves the convergence rate.
7.2.3 Systemic Limitations of Dynamic Grid Maps
The previous sections outlined advantages of grid-based representations in comparison
to object-based representations for dynamic state estimation of a vehicle environment.
This sections describes disadvantages and systemic limitations of dynamic grid maps.
As outlined above, clustering data and assuming object shapes which don’t represent
the actual object appearance is problematic. However, if the clustering is successful
and the assumed object shape is suitable, object-based approaches potentially
outperform grid-based approaches due to additional information contained in the
process model. Additional information is, e.g., that an object usually does not
divide into several objects and all parts of an object perform a consistent movement.
Since a grid map does not comprehend the concept of objects, it can not consider
these object-related elementary rules in its process model. An example for resulting
impairments is shown in Figure 7.6. The test vehicle follows two leading vehicles in
a turn. Figure 7.6 depicts a time line with increasing time from left to right. The
two middle grid maps show that the average orientation estimation is delayed and
does not face exactly into the direction of the corresponding vehicle.
102
Evaluation
Figure 7.6: The time line shows four time steps with two leading vehicles
driving through a turn with increasing time from left to right.
The two middle grid maps show that the orientation estimation
of the DS-PHD/MIB filter is delayed.
This is partly caused by the simple state model of constant velocity and constant
orientation (4.4), of course. But even more complex process models can not entirely
solve the problem since the dynamic grid map internally represents the leading cars
as a congregation of point objects, which all move independently of each other and
not in a consistent pattern. In contrast, object-based filters have been presented,
which are able to successfully estimate the turn rate of vehicles without any delay,
especially with help of radar doppler measurements [KBK+ 14].
A related problem is depicted in Figure 7.7. In this scenario, the test vehicle follows
a Segway on a parking lot. A parked car on the right hand side of the driveway is
partly occluded. The car is marked with a black arrow in Figure 7.7. Due to the
forward motion of the test vehicle, the occlusion situation changes and a growing
part of the parked car becomes visible.
An object-based perception system, which is focusing on the visible stationary corner
of the parked car has the potential to correctly estimate a static object in this case.
In contrast, the DS-PHD/MIB filter can not distinguish increasing object visibility
from motion. As a result, a false positive motion estimation of the dynamic grid map
appears in all four time steps. The reason is again the independence assumption of
dynamic point objects in grid cells.
7.2 Qualitative Evaluation
103
Figure 7.7: The time line shows four time steps with the test vehicle driving
in a parking area with increasing time from left to right. The
dynamic grid map estimates a false positive movement where
a parked car becomes increasingly visible. The false positive
movement estimation is marked with a black arrow in all time
steps.
This effect is even intensified by the Dempster-Shafer approximation. In contrast to
the original PHD/MIB filter, the Dempster-Shafer approximation does not consider
if a grid cell has been visible or occluded in the previous time step when calculating
the relation between a persistent and a new-born object existence probability (5.58).
The original PHD/MIB filter considers the fact that occluded areas might potentially
be already occupied by point objects and therefore calculates a lower existence
probability for point objects predicted from occupied into occluded grid cells. Since
none of the discussed publications on particle-based dynamic grid maps [DON11;
NRL14; NYK+ 15; TTWB14] populates occluded areas with particles, this effect can
be expected to occur in all corresponding realizations.
7.2.4 Discussion
The previous sections qualitatively outlined potential advantages and disadvantages
of dynamic grid maps in comparison to object-based state estimation of dynamic
vehicle environments. In summary, grid maps rely neither on clustering methods
nor on predefined object shape models, which is advantageous when expected object
104
Evaluation
Figure 7.8: Velocity estimation test scenario: A Segway approaches the
test vehicle. The estimated mean velocity of every grid cell is
visualized as a blue vector.
shapes are various and complex and when a robust, unified representation for dynamic
and static obstacles is desired. However, if it is possible to represent expected objects
with predefined shape models and if the precision of the state estimation of dynamic
objects has a priority, object-based tracking approaches potentially outperform
dynamic grid maps, which are not able to consider elementary, object-related rules.
7.3 Quantitative Evaluation
The quantitative evaluation examines the performance of the DS-PHD/MIB filter. A
focus lies on the effect of the birth probability, which will be varied in all experiments.
In a first experiment, an object approaches the ego vehicle with varying speed.
The evaluation examines the speed estimation performance and consistency of the
DS-PHD/MIB filter. In a second experiment, the ego vehicle follows a dynamic
7.3 Quantitative Evaluation
105
object and the evaluation investigates the ability of the DS-PHD/MIB filter to
separate dynamic and static obstacles in the vehicle’s environment. The evaluation
also determines the effect of fusing radar and laser data in comparison to using laser
data only. Finally, the computation time of the parallel implementation with varying
numbers of particles is analyzed.
7.3.1 Velocity Estimation of a Moving Object
In this test scenario, a Segway approaches the test vehicle starting from a distance of
ca. 50 m, see Figure 7.8. The Segway accelerates slowly, drives with almost constant
speed and then decelerates strongly. The set of grid cells representing the Segway is
(S)
denoted S. The experiment evaluates the mean velocity v x of these grid cells:
v (S)
=
x
X 1
v (c) .
|S| x
(7.2)
c∈S
The x-component of the estimated velocity is aligned to the longitudinal axis of the
test vehicle.
Figure 7.9 shows the results of the experiment. In the beginning, the Segway
accelerates slowly. Since the process model assumes constant velocity, the estimated
velocity is delayed during the acceleration phase. During the constant velocity phase,
the estimation converges closely to the real velocity for birth probabilities pB = 0.005
and pB = 0.02.
Choosing a process model with a higher birth probability of pB = 0.1 results in a
persistent bias of the estimated velocity. The reason is that the mean of the birth
distribution is zero, so new-born particles generally distort the velocity estimation
towards zero. The results show that choosing an appropriate birth probability is
important for the velocity estimation performance of the filter.
Fusion of radar data which contains Doppler measurements further improves the
velocity estimation. In this realization, the Segway reaches the radar field of view
at a time of ca. 4 s. Before that point in time, the Segway is outside the radar
range and only rarely detected by radar. Radar Doppler measurements reduce even
small remaining bias effects and lead to a much faster convergence during the strong
deceleration phase in the end. The small peak after 12 s is assumed to be caused by a
movement of the person riding the Segway, commonly referred to as micro-Doppler.
106
Evaluation
Laser and radar, pB = 0.02
Laser only, pB = 0.02
Laser only, pB = 0.005
Laser only, pB = 0.1
Ground truth
−1
Velocity x-component v x
(S)
in m/s
0
−2
−3
−4
−5
0
2
4
6
8
10
12
14
Time in s
Figure 7.9: Velocity estimation results under variation of the birth probability pB . The Segway reaches the short range radar field of view
after a time of ca. 5 s. Fusion with radar further improves the
velocity estimation.
7.3.2 Consistency of the DS-PHD/MIB filter
This section evaluates the consistency of the DS-PHD/MIB filter in dependency
of the birth probability pB , focusing on the x-component of the estimated velocity.
(c)
The DS-PHD/MIB filter provides for each grid cell the estimation variance σ 2 vx .
The experiment considers the combined distribution of the set S of all grid cells
(S)
representing the Segway as a Gaussian mixture. Hence the combined variance σ 2 vx
of the Segway is given by
X 1 (c)
(S)
(c)
(S)
σ 2 vx =
σ 2 vx + v 2 x − v 2 x .
(7.3)
|S|
c∈S
This corresponds to the variance of all particles representing the Segway.
(S)
Figure 7.10 shows the standard deviation σvx of the velocity x-component of the
Segway provided by the DS-PHD/MIB filter. The test scenario is the same as used
for the velocity estimation. The uncertainty increases with the birth probability pB
due to the high number of new-born particles in the dynamic object.
107
Laser
Laser
Laser
Laser
6
(S)
Standard deviation σvx in m/s
7.3 Quantitative Evaluation
and radar, pB = 0.02
only, pB = 0.02
only, pB = 0.005
only, pB = 0.1
4
2
0
0
2
4
6
8
10
12
14
Time in s
(S)
Figure 7.10: Estimation standard deviation σvx of the Segway’s velocity xcomponent provided by the DS-PHD/MIB filter under variation
of the birth probability pB .
To evaluate the consistency of the DS-PHD/MIB filter, the experiment calculates
the normalized estimation error squared (NEES) , given by [BLK01]:
=
(S)
where vx
(S)
vx
(S)
− vx
σ 2 (S)
vx
2
,
(7.4)
is the true velocity x-component of the Segway.
Figure 7.11 shows the result of the consistency test and compares the NEES to
the 95% level [BLK01]. The applied process model (4.4) assumes constant velocity
and does not model acceleration maneuvers. To compensate this, the filter designer
can choose a higher velocity process noise than expected during constant velocity
maneuvers as a trade-off between both modes.
The consistency check shows why the birth probability pB should not be chosen too
small. Especially during acceleration maneuvers, the result can become inconsistent,
because the filter underestimates the uncertainty of the estimation result, which
happens during the deceleration phase with a birth probability of pB = 0.005.
108
Evaluation
12
Laser and radar, pB = 0.02
Laser only, pB = 0.02
Laser only, pB = 0.005
Laser only, pB = 0.1
NEES 95%
10
NEES 8
6
4
2
0
0
2
4
6
8
10
12
14
Time in s
Figure 7.11: Normalized estimation error squared (NEES) of the estimated
velocity x-component under variation of the birth probability
pB .
7.3.3 Separation of Moving and Stationary Obstacles
In the second test scenario, the Segway drives along between parked vehicles with
the ego vehicle following behind. Except for the Segway, the environment is static.
In a manual post-processing step, all grid cells were labeled as dynamic or static.
An example situation of the test is depicted in Figure 7.7.
The evaluation uses the DS-PHD/MIB filter as a classifier to separate grid cells into
dynamic or static. The criterion for the assignment is the Mahalanobis distance m0
between the estimated two-dimensional probability density p(v) : R2 → R of the
velocity distribution in a cell and the velocity v = 0 as defined in equation (7.1).
Applying different threshold values τm , the evaluation classifies grid cells as a static
detection if m0 < τm or as a dynamic detection if m0 ≥ τm .
Figure 7.12 shows the receiver operating characteristics (ROC) curve of the assignment. It shows that the DS-PHD/MIB filter is able to achieve a true positive rate
of 99% (ratio of correctly detected dynamic cells to total number of dynamic cells)
at a false positive rate of 1% (ratio of falsely detected dynamic cells to total number
of static cells) in the test scenario.
7.3 Quantitative Evaluation
109
1
True positive rate
0.98
0.96
0.94
Laser
Laser
Laser
Laser
0.92
0.9
0
0.02
and radar, pB = 0.02
only, pB = 0.02
only, pB = 0.005
only, pB = 0.1
0.04
0.06
0.08
0.1
False positive rate
Figure 7.12: Detection of dynamic grid cells with the DS-PHD/MIB filter
under variation of the birth probability pB .
The birth probability pB of the process model has an important influence on the
estimation result. Best classification performance is achieved with a birth probability
value of pB = 0.02, which also delivered consistent estimation results in the previous
experiment. Again, fusion with radar data further improves the overall result. As
described in details in [NYK+ 15], Doppler measurements help reduce false positive
movement estimation in grid cells. An exemplary visualization of the dynamic grid
map with and without radar is given in Figure 7.13.
110
Evaluation
Figure 7.13: Left: Laser only, false positive movements occur on both sides.
Right: Fusion of laser and radar, false positive movements are
reduced. The color code represents the direction of movement.
7.3 Quantitative Evaluation
111
150
Time in ms
Complete recursion
Sort and assign particles
100
50
0
0.2
0.4
0.6
Number of particles
0.8
1
·10
7
Figure 7.14: Parallel implementation of the DS-PHD/MIB filter: Computation time with varying number of particles and 1.44 · 106 grid
cells.
7.3.4 Computation Time
The parallel implementation presented in Chapter 6 allows to run the DS-PHD/MIB
filter in real-time applications. Figure 7.14 shows the computation time of one
recursion of the DS-PHD/MIB filter in dependence on the number of persistent
particles ν, running on an Nvidia GTX980 GPU. The number of new-born particles
νb is chosen to be 10% of the number of persistent particles in this experiment.
As discussed in Chapter 6, the time complexity of the sorting step is above linear.
However, in the range between 1 · 106 and 10 · 106 particles, the absolute computation
time relates approximately linear to the number of particles. A typical environment
perception application with a refresh time of ca. 50 ms can process more then 2 · 106
particles and 1.44 · 106 grid cells in each update step, which are also the numbers
that have been used during the experiments.
Chapter 8
Conclusion and Outlook
This thesis presented a mathematically rigorous approach for the dynamic state
estimation of occupancy grid cells in the domain of the random finite set (RFS)
theory.
The new state estimator is called the PHD/MIB filter. It estimates the occupancy
state and the dynamic state of grid cells based on heterogeneous sensor data measurements. It models the dynamic environment with point objects and defines a
stochastic multi-object process model and a stochastic multi-object observation
model. A grid cell is occupied if and only if it contains a point object and the
dynamic state of an occupied grid cell equals the dynamic state of its contained
point object.
The filter represents the posterior multi-object state of point objects by its PHD
and applies the prediction step of the common PHD filter. For the update step, it
approximates the multi-object state with multiple instances of Bernoulli random
finite sets, one for each grid cell. Finally, the filter updates each grid cell individually
applying the update step of the Bernoulli filter.
In contrast to former approaches, the top-down derivation of the proposed PHD/MIB
filter allows to clearly state the introduced approximations and assumptions. A
formal proof showed that the assumption of a deterministic, static process model
reduces the PHD/MIB filter to the well-established binary Bayes filter (BBF) for
static occupancy grid maps.
The thesis outlined a sequential Monte Carlo realization of the PHD/MIB filter
propagating particles to represent the multi-object state of grid cells. An approximation in the Dempster-Shafer domain called DS-PHD/MIB filter was proposed, which
requires a significantly smaller number of particles. A massively parallelized algorithm suitable for a GPU implementation of the DS-PHD/MIB filter was presented
114
Conclusion and Outlook
as pseudo code and outlined in detail. One of the main challenges for the parallel
implementation was the assignment of particles to grid cells, which was solved by a
sorting step.
The qualitative evaluation discussed and explained the behavior and the performance
of the DS-PHD/MIB filter in comparison to tracking approaches on object level.
An advantage of the grid-based representation is that neither a clustering step nor
any predefined object shape model is required for the dynamic state estimation.
Therefore, DS-PHD/MIB filter is a robust choice for estimating environments containing dynamic and static objects with complex shapes. However, if it is possible to
correctly model the outline of obstacles with predefined shapes, object-based tracking
approaches have the potential to estimate the object state more precisely than the
DS-PHD/MIB filter, relying on the additional information they gain through the
shape model.
The quantitative evaluation with real-world sensor data showed that appropriate
stochastic models for the system process and for the observation process lead
to consistent estimation results of the DS-PHD/MIB filter. In particular, the
birth probability of the Bernoulli filter must be chosen adequately for the velocity
estimation to converge. The experiments confirmed that the DS-PHD/MIB filter
provides useful results in regard to velocity estimation of dynamic obstacles and
separation of dynamic and static obstacles. They further showed that the presented
parallel implementation can be calculated in real-time on a standard GPU, suitable
for practical applications in vehicle environment perception like autonomous driving.
Further research should investigate possibilities of explicitly modeling the dynamic
behavior of free space, which is bound to a naive model in the DS-PHD/MIB
approximation. Theoretically, the original PHD/MIB filter is able to model potential
dynamic objects in occluded areas which could lead to useful applications. A
main enabler of the massively parallel, real-time capable implementation of the
DS-PHD/MIB filter is based on the fact that the PHD prediction step does not
consider object interaction. As a result, particles perform chaotic movement inside an
object, which is not possible in reality, of course. A challenging, however promising
approach is to consider interaction between point objects.
Some common sensors for vehicle or robotic environment perception take measurements continuously over time, e.g. rotating laser or radar scanners. In this thesis,
measurements have been represented in form of a measurement grid taken at a
specific time. In case of continuous sensing, a measurement grid represents a time
interval instead of a single point in time. Considering the exact measurement time in
the DS-PHD/MIB filter potentially improves estimation results especially in highly
dynamic environments. Finally, an extension to a three-dimensional model promises
better handling of conflicts occurring in the two-dimensional representation.
Acronyms
BBA
BBF
BOF
basic belief assignment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
binary Bayes filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
Bayesian occupancy filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
CVCO
constant velocity, constant orientation . . . . . . . . . . . . . . . . . . . . . . . . . 41
DDMCMC
data driven Markov chain Monte Carlo . . . . . . . . . . . . . . . . . . . . . . . . . 2
DS-PHD/MIB Dempster-Shafer PHD/MIB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
DST
Dempster-Shafer theory of evidence . . . . . . . . . . . . . . . . . . . . . . . . . . . 63
FISST
finite set statistics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
GM-PHD
Gaussian mixture probability hypothesis density . . . . . . . . . . . . . . 57
i.i.d.
IPDA
independent identically distributed . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
integrated probabilistic data association . . . . . . . . . . . . . . . . . . . . . . . 12
JIPDA
JPDA
joint integrated probabilistic data association . . . . . . . . . . . . . . . . . 12
joint probabilistic data association . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
LMB
labeled multi-Bernoulli . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
MHT
MPDF
multi-hypothesis tracking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
multi-object probability density function . . . . . . . . . . . . . . . . . . . . . . 13
PDA
PDF
PHD
PHD/MIB
probabilistic data association . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
probability density function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
probability hypothesis density . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .13
probability hypothesis density / multi-instance Bernoulli . . . . . . 37
RFS
ROC
random finite set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
receiver operating characteristics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108
116
SBMP
SIR
SLAMMOT
SMC
SMC-PHD
Acronyms
single beam monopulse . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .77
sequential importance resampling . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
simultaneous localization, mapping and moving object tracking . 1
sequential Monte Carlo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
sequential Monte Carlo probability hypothesis density . . . . . . . . . 22
List of Symbols
Subscripts
k
+
p
b
B
∗
cl
A
A
time step k
predicted value of a variable or density, short for k + 1|k
indicator for persistent object for a probability density function
indicator for new-born object for a probability density function
indicator for new-born object for a discrete probability function
indicator for either new-born or persistent object
indicator for a clutter event
indicates an object is associated to a measurement
indicates an object is not associated to a measurement
Superscripts
c
i
grid cell index c
particle index i
Vectors, scalars and states
x
x̂
x
z
ξ
ζ
E{·}
v
va||b
(S)
vx
σv2x
σ vx vy
state vector
expected value of random state vector x
mean value of random state vector x
measurement vector
process noise vector
measurement noise vector
expected value of a random variable
velocity vector
projection of vector va on vector vb
mean velocity x-component of cluster S
variance of velocity x-component
covariance between velocity x- and y-component
118
w
w
e
ν
N
N̂
M
λ
λB
λcl
o ∈ {O, F }
o
List of Symbols
weight of a particle
unnormalized weight of a particle
number of particles
number of objects
expected number of objects
maximum possible number of objects in a random finite set
expected cardinality
expected number of new-born objects
expected number of clutter detections
occupancy state with possible realizations occupied O and free F
counter event of o
Matrices
F
H
I
K
P
Q
R
S
state transition matrix
measurement matrix
identity matrix
Kalman gain
estimation error covariance matrix
process noise covariance matrix
measurement noise covariance matrix
innovation covariance matrix
Sets
Ω
X
Z
universal set
random finite set of objects
random finite set of measurements
Spaces
R
X
Z
space of real numbers
state space
measurement space
Distributions
N (x; µ, P )
p
f+
pb
normal distribution over x with mean µ and covariance matrix P
probability density function
state transition probability density function
birth probability density function
List of Symbols
pcl
q
D
γb
κ
π
πb
πcl
ρ
m
119
clutter probability density function
proposal probability density function
probability hypothesis density
birth probability hypothesis density
clutter probability hypothesis density
multi-object probability density function
multi-object probability density function of birth process
multi-object probability density function of clutter process
cardinality distribution
Dempster-Shafer binary belief assignment
Specific probabilities and masses
r
pS (x)
pB
pD (x)
pTP (z)
pFP (z)
po (o)
pz (o)
mz
%p
%b
existence probability
persistence probability of an object with state x
discrete birth probability
detection probability of an object with state x
true positive probability of measurement z
false positive probability of measurement z
probability of occupancy state o
probability of occupancy state o reasoning on measurement z
Dempster-Shafer binary belief assignment reasoning on measurement z
Dempster-Shafer occupancy mass, persistent part
Dempster-Shafer occupancy mass, new-born part
Measurement likelihoods
g(z|x)
γ(Z|X)
likelihood function of measurement z given state x
multi-object likelihood of measurement set Z given multi-object state X
Notation
A
AT
A−1
δ(·)
x
y
matrix
transpose of a matrix
inverse of a matrix
Dirac delta function
abscissa of a Cartesian coordinate system
ordinate of a Cartesian coordinate system
Bibliography
[BF88]
Bar-Shalom, Y. and Fortmann, T.: Tracking and Data Association.
Academic Press, Inc., 1988.
[BH10]
Bouzouraa, M. and Hofmann, U.: Fusion of occupancy grid mapping and model based object tracking for driver assistance systems
using laser and radar sensors. In: Proceedings of the IEEE Intelligent
Vehicles Symposium, pages 294–300, 2010.
[BLK01]
Bar-Shalom, Y.; Li, X.-R.; and Kirubarajan, T.: Estimation with
Applications To Tracking and Naviation. John Wiley & Sons, Inc.,
New York, 2001.
[BT75]
Bar-Shalom, Y. and Tse, E.: Tracking in a cluttered environment
with probabilistic data association. In: Automatica, volume 11, no. 5,
pages 451–460, 1975.
[BWT11]
Bar-Shalom, Y.; Willett, P. K.; and Tian, X.: Tracking and Data
Fusion. YBS Publishing, Storrs, CT, USA, 2011.
[CPL+ 06]
Coué, C.; Pradalier, C.; Laugier, C.; Fraichard, T.; and Bessiere, P.:
Bayesian occupancy filtering for multitarget tracking: an automotive
application. In: International Journal of Robotics Research, volume 25,
no. 1, pages 19–30, 2006.
[Dem68]
Dempster, A. P.: A generalization of Bayesian inference. In: Journal
of the Royal Statistical Society. Series B (Methodological), volume 30,
no. 2, pages 205–247, 1968.
[DN13]
Danescu, R. and Nedevschi, S.: Flexible solution for modeling and
tracking generic dynamic 3d environments. In: Proceedings of the IEEE
Conference on Intelligent Transportation Systems, pages 1686–1692.
IEEE, 2013.
[DON11]
Danescu, R.; Oniga, F.; and Nedevschi, S.: Modeling and tracking the
driving environment with a particle-based occupancy grid. In: IEEE
Transactions on Intelligent Transportation Systems, volume 12, no. 4,
pages 1331–1342, 2011.
122
Bibliography
[DRN15]
Dietmayer, K.; Reuter, S.; and Nuss, D.: Representation of fused
environment data. In: Winner, H.; Hakuli, S.; Lotz, F.; and Singer, C.
(editors): Handbook of Driver Assistance Systems, Springer International Publishing, Cham, 2015.
[Elf89]
Elfes, A.: Using occupancy grids for mobile robot perception and navigation. In: Computer, volume 22, no. 6, pages 46–57, 1989.
[FBS83]
Fortmann, T.; Bar-Shalom, Y.; and Scheffe, M.: Sonar tracking of
multiple targets using joint probabilistic data association. In: IEEE
Journal of Oceanic Engineering, volume 8, no. 3, pages 173–184, 1983.
[GBSD09]
Gindele, T.; Brechtel, S.; Schroder, J.; and Dillmann, R.: Bayesian
occupancy grid filter for dynamic environments using prior map knowledge. In: Proceedings of the IEEE Intelligent Vehicles Symposium,
pages 669–676, 2009.
[HB16]
Hoberock, J. and Bell, N.: Thrust: A Parallel Template Library. Version 1.8.1. 2016.
[HKOB10]
Homm, F.; Kaempchen, N.; Ota, J.; and Burschka, D.: Efficient
occupancy grid computation on the GPU with lidar and radar for road
boundary detection. In: Proceedings of the IEEE Intelligent Vehicles
Symposium, pages 1006–1013, 2010.
[HL64]
Ho, Y.-C. and Lee, R.: A Bayesian approach to problems in stochastic
estimation and control. In: IEEE Transactions on Automatic Control,
volume 9, no. 4, pages 333–339, 1964.
[HSRD16]
Hirscher, T.; Scheel, A.; Reuter, S.; and Dietmayer, K.: Multiple
extended object tracking using Gaussian processes. In: Proceedings of
the International Conference on Information Fusion, pages 868–875.
IEEE, 2016.
[JUD95]
Julier, S.; Uhlmann, J.; and Durrant-Whyte, H.: A new approach
for filtering nonlinear systems. In: Proceedings of the 1995 American
Control Conference. Volume 3, pages 1628–1632, 1995.
[Kal60]
Kalman, R.: A new approach to linear filtering and prediction problems.
In: Journal of Basic Engineering, volume 82, no. 1, pages 35–45, 1960.
[KBK+ 14]
Kellner, D.; Barjenbruch, M.; Klappstein, J.; Dickmann, J.; and Dietmayer, K.: Instantaneous full-motion estimation of arbitrary objects
using dual doppler radar. In: Proceedings of the IEEE Intelligent
Vehicles Symposium, pages 324–329, 2014.
[KD11]
Konrad, M. and Dietmayer, K.: Occupancy grid mapping using the
Dempster-Shafer-theory. In: Proceedings of the International Workshop
on Intelligent Transportation (WIT), pages 167–172, 2011.
Bibliography
123
[KNW+ 15]
Kunz, F.; Nuss, D.; Wiest, J.; et al.: Autonomous driving at ulm
university: a modular, robust, and sensor-independent fusion approach.
In: Proceedings of the IEEE Intelligent Vehicles Symposium, pages 666–
673, 2015.
[Koc08]
Koch, J. W.: Bayesian approach to extended object and cluster tracking
using random matrices. In: IEEE Transactions on Aerospace and
Electronic Systems, volume 44, no. 3, pages 1042–1059, 2008.
[Kol50]
Kolmogorov, A.: Foundations of the Theory of Probability. Chelsea
Pub. Co., 1950.
[LPP+ 11]
Laugier, C.; Paromtchik, I.; Perrollaz, M.; et al.: Probabilistic analysis
of dynamic scenes and collision risks assessment to improve driving safety. In: IEEE Intelligent Transportation Systems Magazine,
volume 3, no. 4, pages 4–19, 2011.
[Mah03]
Mahler, R.: Multitarget Bayes filtering via first-order multitarget moments. In: IEEE Transactions on Aerospace and Electronic Systems,
volume 39, no. 4, pages 1152–1178, 2003.
[Mah07a]
Mahler, R.: Statistical Multisource-Multitarget Information Fusion.
Artech House, Norwood, 2007.
[Mah07b]
Mahler, R.: PHD filters of higher order in target number. In: IEEE
Transactions on Aerospace and Electronic Systems, volume 43, no. 4,
pages 1523–1543, 2007.
[Mah13]
Mahler, R.: ’’Statistics 102” for multisource-multitarget detection and
tracking. In: IEEE Journal of Selected Topics in Signal Processing,
volume 7, no. 3, pages 376–389, 2013.
[MCB11]
Moras, J.; Cherfaoui, V.; and Bonnifait, P.: Moving objects detection
by conflict analysis in evidential grids. In: Proceedings of the IEEE
Intelligent Vehicles Symposium, pages 1122–1127, 2011.
[MD11]
Munz, M. and Dietmayer, K.: Using Dempster-Shafer-based modeling
of object existence evidence in sensor fusion systems for advanced
driver assistance systems. In: Proceedings of the IEEE Intelligent
Vehicles Symposium, pages 776–781, 2011.
[MDM10]
Munz, M.; Dietmayer, K.; and Mählisch, M.: Generalized fusion of
heterogeneous sensor measurements for multi target tracking. In: Proceedings of the International Conference on Information Fusion, 2010.
[ME04]
Musicki, D. and Evans, R.: Joint integrated probabilistic data association: JIPDA. In: IEEE Transactions on Aerospace and Electronic
Systems, volume 40, no. 3, pages 1093–1099, 2004.
[MES94]
Musicki, D.; Evans, R.; and Stankovic, S.: Integrated probabilistic data
association. In: IEEE Transactions on Automatic Control, volume 39,
no. 6, pages 1237–1241, 1994.
124
Bibliography
[ML08]
Musicki, D. and La Scala, B.: Multi-target tracking in clutter without
measurement assignment. In: IEEE Transactions on Aerospace and
Electronic Systems, volume 44, no. 3, pages 877–896, 2008.
[MRSD14]
Meissner, D.; Reuter, S.; Strigel, E.; and Dietmayer, K.: Intersectionbased road user tracking using a classifying multiple-model PHD filter.
In: IEEE Intelligent Transportation Systems Magazine, volume 6,
no. 2, pages 21–33, 2014.
[NRL14]
Nègre, A.; Rummelhard, L.; and Laugier, C.: Hybrid sampling Bayesian
occupancy filter. In: Proceedings of the IEEE Intelligent Vehicles Symposium, pages 1307–1312, 2014.
[NSD14]
Nuss, D.; Stübler, M.; and Dietmayer, K.: Consistent environmental modeling by use of occupancy grid maps, digital road maps, and
multi-object tracking. In: Proceedings of the IEEE Intelligent Vehicles
Symposium, pages 1371–1377, 2014.
[NTDD14]
Nuss, D.; Thom, M.; Danzer, A.; and Dietmayer, K.: Fusion of laser
and monocular camera data in object grid maps for vehicle environment perception. In: Proceedings of the International Conference on
Information Fusion, 2014.
[NVI16]
NVIDIA: CUDA CURAND Library. NVIDIA Corporation, Santa
Clara, CA, USA, 2016. url: http : / / docs . nvidia . com / cuda /
curand.
[NWW+ 13]
Nuss, D.; Wilking, B.; Wiest, J.; et al.: Decision-free true positive
estimation with grid maps for multi-object tracking. In: Proceedings of
the IEEE Conference on Intelligent Transportation Systems, pages 28–
34, 2013.
[NYK+ 15]
Nuss, D.; Yuan, T.; Krehl, G.; et al.: Fusion of laser and radar sensor
data with a sequential Monte Carlo Bayesian occupancy filter. In:
Proceedings of the IEEE Intelligent Vehicles Symposium, pages 1074–
1081, 2015.
[Pet11]
Petrovskaya, A.: Towards Dependable Robotic Perception. PhD thesis,
Computer Science Department, Stanford University, 2011.
[RAG04]
Ristic, B.; Arulampalam, S.; and Gordon, N.: Beyond the Kalman
Filter: Particle Filters for Tracking Applications. Artech House Inc.,
2004.
[RCVV12]
Ristic, B.; Clark, D.; Vo, B.-N.; and Vo, B.-T.: Adaptive target birth
intensity for PHD and CPHD filters. In: IEEE Transactions on
Aerospace and Electronic Systems, volume 48, no. 2, pages 1656–1668,
2012.
[Rei79]
Reid, D.: An algorithm for tracking multiple targets. In: IEEE Transactions on Automatic Control, volume 24, no. 6, pages 843–854, 1979.
Bibliography
125
[Ris13]
Ristic, B.: Particle Filters for Random Set Models. Springer, New
York, 2013.
[RVVD14]
Reuter, S.; Vo, B.-T.; Vo, B.-N.; and Dietmayer, K.: The labeled
multi-Bernoulli filter. In: IEEE Transactions on Signal Processing,
volume 62, no. 12, pages 3246–3260, 2014.
[RWW+ 13]
Reuter, S.; Wilking, B.; Wiest, J.; Munz, M.; and Dietmayer, K.:
Real-time multi-object tracking using random finite sets. In: IEEE
Transactions on Aerospace and Electronic Systems, volume 49, no. 4,
pages 2666–2678, 2013.
[Sha76]
Shafer, G.: A Mathematical Theory of Evidence. Princeton University
Press, Princeton, 1976.
[SHG09]
Satish, N.; Harris, M.; and Garland, M.: Designing efficient sorting
algorithms for manycore GPUs. In: Proceedings of the IEEE International Symposium on Parallel & Distributed Processing, 2009.
[Sid03]
Sidenbladh, H.: Multi-target particle filtering for the probability hypothesis density. In: Proceedings of the International Conference on
Information Fusion. Volume 2, pages 800–806, 2003.
[Sme90]
Smets, P.: The combination of evidence in the transferable belief model.
In: IEEE Transactions on Pattern Analysis and Machine Intelligence,
volume 12, no. 5, pages 447–458, 1990.
[SSM62]
Smith, G. L.; Schmidt, S. F.; and Mc Gee, L. A.: Application of
statistical filter theory to the optimal estimation of position and velocity
on board a circumlunar vehicle. Tech. rep. NASA Ames Research
Center, Tech. Rep. TR R-135, 1962.
[TBF05]
Thrun, S.; Burgard, W.; and Fox, D.: Probabilistic Robotics (Intelligent
Robotics and Autonomous Agents). The MIT Press, Cambridge, 2005.
[Thr03]
Thrun, S.: Learning occupancy grid maps with forward sensor models.
In: Auton. Robots, volume 15, no. 2, pages 111–127, 2003.
[TTWB14]
Tanzmeister, G.; Thomas, J.; Wollherr, D.; and Buss, M.: Grid-based
mapping and tracking in dynamic environments using a uniform evidential environment representation. In: Proceedings of the IEEE International Conference on Robotics and Automation, pages 6090–6095,
2014.
[Urm+ 08]
Urmson, C. et al.: Autonomous driving in urban environments: boss
and the urban challenge. In: Journal of Field Robotics, Special Issue on
the 2007 DARPA Urban Challenge, Part I, volume 25, no. 8, Martin
Buehler Karl Lagnemma, S. S. (editor), pages 425–466, 2008.
126
Bibliography
[VA09]
Vu, T.-D. and Aycard, O.: Laser-based detection and tracking moving
objects using data-driven markov chain Monte Carlo. In: Proceedings
of the IEEE International Conference on Robotics and Automation,
pages 3800–3806, 2009.
[VDN15]
Vatavu, A.; Danescu, R.; and Nedevschi, S.: Stereovision-based multiple
object tracking in traffic scenarios using free-form obstacle delimiters
and particle filters. In: IEEE Transactions on Intelligent Transportation Systems, volume 16, no. 1, pages 498–511, 2015.
[VJ01]
Viola, P. and Jones, M.: Rapid object detection using a boosted cascade of simple features. In: Proceedings of the IEEE Conference on
Computer Vision and Pattern Recognition, pages 511–518, 2001.
[VM06]
Vo, B.-N. and Ma, W.-K.: The Gaussian mixture probability hypothesis
density filter. In: IEEE Transactions on Signal Processing, volume 54,
no. 11, pages 4091–4104, 2006.
[VSD05]
Vo, B.-N.; Singh, S.; and Doucet, A.: Sequential Monte Carlo methods
for multitarget filtering wit random finite sets. In: IEEE Transactions
on Aerospace and Electronic Systems, volume 41, no. 4, pages 1224–
1245, 2005.
[VV13]
Vo, B.-T. and Vo, B.-N.: Labeled random finite sets and multi-object
conjugate priors. In: IEEE Transactions on Signal Processing, volume 61, no. 13, pages 3460–3475, 2013.
[VVP14]
Vo, B.-N.; Vo, B.-T.; and Phung, D.: Labeled random finite sets and
the Bayes multi-target tracking filter. In: IEEE Transactions on Signal
Processing, volume 62, no. 24, pages 6554–6567, 2014.
[WHLS15]
Handbook of driver assistance systems. Basic information, components
and systems for active safety and comfort. In: Winner, H.; Hakuli, S.;
Lotz, F.; and Singer, C. (editors), Springer International Publishing,
Cham, 2015.
[WTT+ 07]
Wang, C.-C.; Thorpe, C.; Thrun, S.; Hebert, M.; and Durrant-Whyte,
H.: Simultaneous localization, mapping and moving object tracking.
In: International Journal of Robotics Research, volume 26, no. 9,
pages 889–916, 2007.
[YFE94]
Yager, R.; Fedrizzi, M.; and Eds, J. K.: Advances in the DempsterShafer Theory of Evidence. New York: Wiley, 1994.
[YHCZ00]
Yi, Z.; Ho, Y. K.; Chua, C. S.; and Zhou, X. W.: Multi-ultrasonic
sensor fusion for autonomous mobile robots. In: AeroSense, pages 314–
321. International Society for Optics and Photonics, 2000.
Publications
Parts of this thesis were pre-released in author’s publications.
Nuss, D.; Reuter, S.; Konrad, M.; Munz, M.; and Dietmayer, K.: Using grid maps
to reduce the number of false positive measurements in advanced driver assistance
systems. In: Proceedings of the 15th International IEEE Conference on Intelligent
Transportation Systems, pages 1509–1514, 2012.
Nuss, D.; Reuter, S.; Thom, M.; et al.: A random finite set approach for dynamic
occupancy grid maps with real-time application. In: ArXiv e-prints, 2016.
Nuss, D.; Stübler, M.; and Dietmayer, K.: Consistent environmental modeling by use
of occupancy grid maps, digital road maps, and multi-object tracking. In: Proceedings
of the IEEE Intelligent Vehicles Symposium, pages 1371–1377, 2014.
Nuss, D.; Thom, M.; Danzer, A.; and Dietmayer, K.: Fusion of laser and monocular
camera data in object grid maps for vehicle environment perception. In: Proceedings
of the International Conference on Information Fusion, 2014.
Nuss, D.; Wilking, B.; Wiest, J.; et al.: Decision-free true positive estimation with
grid maps for multi-object tracking. In: Proceedings of the IEEE Conference on
Intelligent Transportation Systems, pages 28–34, 2013.
Nuss, D.; Yuan, T.; Krehl, G.; et al.: Fusion of laser and radar sensor data with
a sequential Monte Carlo Bayesian occupancy filter. In: Proceedings of the IEEE
Intelligent Vehicles Symposium, pages 1074–1081, 2015.
128
Publications
Deusch, H.; Nuss, D.; Konrad, P.; et al.: Improving localization in digital maps with
grid maps. In: 16th International IEEE Conference on Intelligent Transportation
Systems (ITSC), pages 1522–1527, 2013.
Dietmayer, K.; Nuss, D.; and Reuter, S.: Handbuch fahrerassistenzsysteme: grundlagen, komponenten und systeme für aktive sicherheit und komfort. In: ed. by Winner,
H.; Hakuli, S.; Lotz, F.; and Singer, C. Springer Fachmedien Wiesbaden, Wiesbaden,
2015. Chap. Repräsentation fusionierter Umfelddaten, pages 453–480.
Dietmayer, K.; Reuter, S.; and Nuss, D.: Representation of fused environment data.
In: Winner, H.; Hakuli, S.; Lotz, F.; and Singer, C. (editors): Handbook of Driver
Assistance Systems, Springer International Publishing, Cham, 2015.
Deusch, H.; Wiest, J.; Reuter, S.; et al.: Multi-sensor self-localization based on maximally stable extremal regions. In: IEEE Intelligent Vehicles Symposium, pages 555–
560, 2014.
Konrad, M.; Nuss, D.; and Dietmayer, K.: Localization in digital maps for road
course estimation using grid maps. In: Proceedings of the IEEE Intelligent Vehicles
Symposium, pages 87–92, 2012.
Kunz, F.; Nuss, D.; Wiest, J.; et al.: Autonomous driving at ulm university: a
modular, robust, and sensor-independent fusion approach. In: Proceedings of the
IEEE Intelligent Vehicles Symposium, pages 666–673, 2015.
Wiest, J.; Deusch, H.; Nuss, D.; et al.: Localization based on region descriptors in
grid maps. In: Proceedings of the IEEE Intelligent Vehicles Symposium, 2014.
Yuan, T.; Nuss, D.; Krehl, G.; Maile, M.; and Gern, A.: Fundamental properties of
dynamic occupancy grid systems for vehicle environment perception. In: IEEE Workshop on Computational Advances in Multi-Sensor Adaptive Processing (CAMSAP),
pages 153–156, 2015.
Supervised Theses
Danzer, A.: Einbringung von Videodaten in Belegtheitskarten für Fahrerassistenzsysteme. Bachelor Thesis, Universität Ulm, 2014.
Hatzelmann, P.: Online-Erstellung digitaler Straßenkarten auf Basis von Kamerabildern und Odometriemessungen mit Loop-Closing. Bachelor Thesis, Universität
Ulm, 2012.
Hörmann, S.: Head-pose-estimation für Videobrillen-Anwendungen. Diploma Thesis,
Universität Ulm, 2012.
Konrad, P.: Spline-basierte Segmentierung von Fahrspurmarkierungen zur Erstellung
digitaler Karten. Bachelor Thesis, Universität Ulm, 2012.
Lukaseder, T.: A strategy to resolve inconsistencies between grid mapping and object
tracking in vehicular environmental perception. Master Thesis, Universität Ulm, 2014.
Peukert, J.: Object generation from a dynamic occupancy gridmap considering autocorrelation. Master Thesis, Universität Karlsruhe, 2015.
Stübler, M.: Konsistente Modellierung des Fahrzeugumfelds basierend auf digitalen
Straßenkarten, Occupancy-Grids und Multi-Objekt-Tracking. Master Thesis, Universität Ulm, 2014.
Weiss, F.: Fusion von Lagesensorik und Kamera zur Kopfposenschätzung. Diploma
Thesis, Universität Ulm, 2012.
ISBN 978-3-941543-28-7
e-ISBN 978-3-941543-29-4