Linköping studies in science and technology. Dissertations.

No. 1409

Sensor Fusion for Automotive

Applications

Christian Lundquist

Department of Electrical Engineering

Linköping University, SE–581 83 Linköping, Sweden

Linköping 2011
Cover illustration: The intensity map describes the density of stationary targets
along the road edges. A photo of the the driver’s view in the green car is shown
in Figure 5a on Page 216.

Linköping studies in science and technology. Dissertations.

No. 1409

Sensor Fusion for Automotive Applications

Christian Lundquist

[email protected]
www.control.isy.liu.se
Division of Automatic Control
Department of Electrical Engineering
Linköping University
SE–581 83 Linköping
Sweden

ISBN 978-91-7393-023-9 ISSN 0345-7524

Copyright © 2011 Christian Lundquist

Printed by LiU-Tryck, Linköping, Sweden 2011

To Nadia
Abstract
Mapping stationary objects and tracking moving targets are essential for many
autonomous functions in vehicles. In order to compute the map and track esti-
mates, sensor measurements from radar, laser and camera are used together with
the standard proprioceptive sensors present in a car. By fusing information from
different types of sensors, the accuracy and robustness of the estimates can be
increased.
Different types of maps are discussed and compared in the thesis. In particular,
road maps make use of the fact that roads are highly structured, which allows rel-
atively simple and powerful models to be employed. It is shown how the informa-
tion of the lane markings, obtained by a front looking camera, can be fused with
inertial measurement of the vehicle motion and radar measurements of vehicles
ahead to compute a more accurate and robust road geometry estimate. Further,
it is shown how radar measurements of stationary targets can be used to estimate
the road edges, modeled as polynomials and tracked as extended targets.
Recent advances in the field of multiple target tracking lead to the use of finite set
statistics (fisst) in a set theoretic approach, where the targets and the measure-
ments are treated as random finite sets (rfs). The first order moment of a rfs
is called probability hypothesis density (phd), and it is propagated in time with
a phd filter. In this thesis, the phd filter is applied to radar data for construct-
ing a parsimonious representation of the map of the stationary objects around
the vehicle. Two original contributions, which exploit the inherent structure in
the map, are proposed. A data clustering algorithm is suggested to structure the
description of the prior and considerably improving the update in the phd filter.
Improvements in the merging step further simplify the map representation.
When it comes to tracking moving targets, the focus of this thesis is on extended
targets, i.e., targets which potentially may give rise to more than one measure-
ment per time step. An implementation of the phd filter, which was proposed
to handle data obtained from extended targets, is presented. An approximation
is proposed in order to limit the number of hypotheses. Further, a framework to
track the size and shape of a target is introduced. The method is based on mea-
surement generating points on the surface of the target, which are modeled by an
rfs.
Finally, an efficient and novel Bayesian method is proposed for approximating
the tire radii of a vehicle based on particle filters and the marginalization concept.
This is done under the assumption that a change in the tire radius is caused by
a change in tire pressure, thus obtaining an indirect tire pressure monitoring
system.
The approaches presented in this thesis have all been evaluated on real data from
both freeways and rural roads in Sweden.

v
Populärvetenskaplig sammanfattning
Dagens bilar blir säkrare för varje år. Tidigare var det den passiva säkerheten med
bilbälten, krockkuddar och krockzoner som förbättrades. Nu sker de snabbaste
förändringarna inom aktiv säkerhet, med förarstödsystem såsom antisladd och
nödbromssystem och förarvarningssystem för farliga filbyten och framförvaran-
de hinder.
Förarstödsystem kräver omvärldsuppfattning, och de sensorer för detta som finns
i bruk idag är kamera och radar. Kameran ser t.ex. filmarkeringar och kan använ-
das för att detektera fordon och fotgängare. Radarn är mycket bra på att detekte-
ra rörliga objekt och på avståndsbedömning, och används idag för att följa bilen
framför. Radarn ger en mängd information som idag är outnyttjad, och ett syfte
med avhandlingen är att undersöka hur radarinformationen kan användas fullt
ut i framtida säkerhetssystem.
Ett bidrag i avhandlingen är att använda radarns mätningar av stillastående ob-
jekt på och jämte vägen för att bygga upp en lokal karta. Kartan visar områden
som är körbara och hinder som ska undvikas. Ett annat bidrag är att sortera alla
de träffar från rörliga objekt som radarn registrerar och ge en noggrann karta över
hur många andra fordon det finns, samt deras positioner, hastigheter och storle-
kar. Tillsammans bildar dessa två kartor en lägesbild som kan användas i nästa
generations kollisionsundvikande system som kan kombinera broms och styring-
repp för att på ett intelligent, säkert och kontrollerat sätt väja undan i kritiska
nödsituationer.
En annan typ av säkerhetssystem är varningssystem till föraren på förändring-
ar i fordonsdynamiska parametrar som kan försämra köregenskaperna. Exempel
på sådana parametrar är däcktryck, friktion och fellastning. Ett bidrag i avhand-
lingen är ett nytt sätt att skatta en sänkning i däcktryck, genom att kombinera
sensorer i fordonet med satellitnavigering.
De metoder som presenteras i avhandlingen har utvärderats på verkliga data från
bland annat motorvägar och landsvägar i Sverige.

vii
Acknowledgments
It all started in the end of the 80s, when my teacher asked me what I wanted to
become when I grow up. I had only one wish, to study at Chalmers; followed
by my answer my teacher laughed and told me “you will never make it, you are
too stupid". This incentivized me, and encourage by other teachers I managed to
finish school and be admitted at Chalmers. Several years later I’m now here and
I have finished my thesis, something I would never have managed alone, without
the support of many wonderful persons.
I would like to thank my supervisor professor Fredrik Gustafsson for his positive,
inspiring, encouraging and relaxed attitude and my co-supervisor Dr. Thomas B.
Schön for giving me a speedy start into academic research. Whenever I run into
problems Dr. Umut Orguner, who possesses an enormous amount of knowledge
about everything beyond least squares, pushed me in the right direction. Thank
you for your help and for always having time. Further, I would like to acknowl-
edge professor Svante Gunnarsson, who is a skillful head of the group, and his
predecessor professor Lennart Ljung. They have created a wonderful research
atmosphere which makes enjoyable going to office.
Part of the work has been performed with colleagues. I would like to mention Lic.
Karl Granstöm, since one can’t find a better partner to collaborate with. Further
I like to mention Dr. Lars Hammarstrand, at Chalmers, with whom I had very
useful and interesting discussions. Finally, I would like to thank Dr. Emre Özkan,
who opened the Gaussian window and let me see other distributions.
Andreas Andersson at Nira Dynamics AB and Dr. Andreas Eidehall at Volvo Per-
sonvagnar AB have rescued me from the simulation swamp, by supporting me
with measurement data from prototype vehicles. Dr. Gustaf Hendeby and Dr.
Henrik Tidefelt helped me with latex issues, without their support this thesis
would not be in this professional shape. Ulla Salaneck, Åsa Karmelind and Ninna
Stensgård have helped with many practical things during the years in the group.
An important part of the PhD studies is the time spent outside the office bunker,
i.e., in the fika room and in pubs. I would like to thank Lic. Jonas Callmer, Lic.
Karl Granström and Lic. Martin Skoglund for sharing vagnen with me. Further
I would like to thank Lic. Christian Lyzell, Dr. Ragnar Wallin, Dr. Henrik Ohls-
son Lic. Zoran Sjanic and Sina Khoshfetrat Pakazad for being generous and good
friends.
Dr. Wolfgang Reinelt became a mentor for me, he supported and encouraged me
during the time at ZF Lenksysteme GmbH. With him I wrote my first publica-
tions. My former boss Gerd Reimann taught me about vehicle dynamics and the
importance of good experiments; I will always remember Sinuslenken.
One of the most inspiring persons I met is Dr. Peter Bunus. Together with pro-
fessor Fredrik Gustafsson, Dr. David Törnqvist, Lic. Per Skoglar and Lic. Jonas
Callmer we started SenionLab AB. I’m looking forward to keeping on working
with all of them in the near future.

ix
x Acknowledgments

I would like to acknowledge the supported from the SEnsor fusion for Safety
(sefs) project within the Intelligent Vehicle Safety Systems (ivss) program and
the support from the Swedish Research Council under the frame project grant
Extended Target Tracking.
I would never have been able to fulfill my wish to study without the support from
my family, for this I am endlessly thankful. Finally, I would like to thank amore
della mia vita Nadia, who has brought so much love and joy into my life.

Linköping, October 2011

Christian Lundquist
 Contents

Notation xvii

I Background
1 Introduction 3
1.1 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Automotive Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . 4
1.3 Sensor Fusion for Safety . . . . . . . . . . . . . . . . . . . . . . . . 6
1.4 Extended Target Tracking . . . . . . . . . . . . . . . . . . . . . . . . 7
1.5 Components of the Sensor Fusion Framework . . . . . . . . . . . . 7
1.6 Publications . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
1.7 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
1.8 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
1.8.1 Outline of Part I . . . . . . . . . . . . . . . . . . . . . . . . . 17
1.8.2 Outline of Part II . . . . . . . . . . . . . . . . . . . . . . . . 17

2 Models of Dynamic Systems 21

2.1 Overview of the Models Used in the Thesis . . . . . . . . . . . . . . 22
2.2 Discretizing Continuous-Time Models . . . . . . . . . . . . . . . . 26
2.3 Linear State Space Model . . . . . . . . . . . . . . . . . . . . . . . . 28
2.4 Nonlinear State Space Model with Additive Noise . . . . . . . . . . 28

3 Estimation Theory 31
3.1 Static Estimation Theory . . . . . . . . . . . . . . . . . . . . . . . . 32
3.1.1 Least Squares Estimator . . . . . . . . . . . . . . . . . . . . 33
3.1.2 Probabilistic Point Estimates . . . . . . . . . . . . . . . . . . 35
3.2 Filter Theory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
3.2.1 The Kalman Filter . . . . . . . . . . . . . . . . . . . . . . . . 36
3.2.2 The Extended Kalman Filter . . . . . . . . . . . . . . . . . . 38
3.2.3 The Unscented Kalman Filter . . . . . . . . . . . . . . . . . 39
3.2.4 The Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . 41

xi
xii CONTENTS

4 Target Tracking 45
4.1 Single Target Tracking . . . . . . . . . . . . . . . . . . . . . . . . . 46
4.2 Extension to Multitarget Tracking . . . . . . . . . . . . . . . . . . . 47
4.2.1 Data Association . . . . . . . . . . . . . . . . . . . . . . . . 47
4.2.2 Track Management . . . . . . . . . . . . . . . . . . . . . . . 50
4.3 Extended Target Tracking . . . . . . . . . . . . . . . . . . . . . . . . 51
4.3.1 Point Features . . . . . . . . . . . . . . . . . . . . . . . . . . 52
4.3.2 Spatial Distribution . . . . . . . . . . . . . . . . . . . . . . . 53
4.3.3 Elliptical Shaped Target . . . . . . . . . . . . . . . . . . . . 54
4.3.4 Curved Target . . . . . . . . . . . . . . . . . . . . . . . . . . 55
4.3.5 Extended Target Tracking and PHD filter . . . . . . . . . . 56

5 PHD Filter and Its Implementation 57

5.1 Introduction to Finite Set Statistics . . . . . . . . . . . . . . . . . . 58
5.1.1 Random Finite Set . . . . . . . . . . . . . . . . . . . . . . . . 59
5.1.2 Belief-Mass and Multitarget Density Function . . . . . . . . 59
5.1.3 The Multitarget Bayes Filter . . . . . . . . . . . . . . . . . . 61
5.2 Introduction to the PHD filter . . . . . . . . . . . . . . . . . . . . . 62
5.2.1 Approximations in Single-Target Tracking . . . . . . . . . . 63
5.2.2 Approximations in Multitarget Tracking . . . . . . . . . . . 63
5.2.3 The PHD Filter . . . . . . . . . . . . . . . . . . . . . . . . . 64
5.2.4 Generalizations of the PHD filter . . . . . . . . . . . . . . . 68
5.3 Gaussian Mixture Implementation . . . . . . . . . . . . . . . . . . 70
5.3.1 Gaussian Mixture PHD Approximation . . . . . . . . . . . 71
5.3.2 GM-PHD Filter Algorithm . . . . . . . . . . . . . . . . . . . 72
5.3.3 Merge, Prune and Extract Targets . . . . . . . . . . . . . . . 74

6 Concluding Remarks 77
6.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
6.2 Future Research . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79

Bibliography 81

II Publications
A Situational Awareness and Road Prediction for Trajectory Control Ap-
plications 97
1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99
2 Modeling the Environment with a Map . . . . . . . . . . . . . . . . 100
3 Feature Based Map . . . . . . . . . . . . . . . . . . . . . . . . . . . 103
3.1 Radar and Laser . . . . . . . . . . . . . . . . . . . . . . . . . 103
3.2 Cameras and Computer Vision . . . . . . . . . . . . . . . . 104
4 Road Map . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
4.1 Road Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 107
4.2 Mapping of the Road Lanes . . . . . . . . . . . . . . . . . . 110
4.3 Mapping of the Road Edges . . . . . . . . . . . . . . . . . . 114
CONTENTS xiii

5 Occupancy Grid Map . . . . . . . . . . . . . . . . . . . . . . . . . . 118

5.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . 120
5.2 OGM with Radar Measurements . . . . . . . . . . . . . . . 121
5.3 Experiments and Results . . . . . . . . . . . . . . . . . . . . 121
6 Intensity Based Map . . . . . . . . . . . . . . . . . . . . . . . . . . . 123
7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 125
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128

B Joint Ego-Motion and Road Geometry Estimation 133

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135
2 Sensor Fusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137
3 Dynamic Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 139
3.1 Geometry and Notation . . . . . . . . . . . . . . . . . . . . 140
3.2 Ego Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . 141
3.3 Road Geometry . . . . . . . . . . . . . . . . . . . . . . . . . 142
3.4 Leading Vehicles . . . . . . . . . . . . . . . . . . . . . . . . 146
3.5 Summarizing the Dynamic Model . . . . . . . . . . . . . . . 147
4 Measurement Model . . . . . . . . . . . . . . . . . . . . . . . . . . 148
5 Experiments and Results . . . . . . . . . . . . . . . . . . . . . . . . 150
5.1 Parameter Estimation and Filter Tuning . . . . . . . . . . . 151
5.2 Validation Using Ego Vehicle Signals . . . . . . . . . . . . . 152
5.3 Road Curvature Estimation . . . . . . . . . . . . . . . . . . 152
6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 157
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 158

C Extended Target Tracking Using Polynomials With Applications to

Road-Map Estimation 163
1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 165
2 Related Literature . . . . . . . . . . . . . . . . . . . . . . . . . . . . 167
2.1 Extended Target Tracking . . . . . . . . . . . . . . . . . . . 167
2.2 Contours and Curves . . . . . . . . . . . . . . . . . . . . . . 168
2.3 Errors In Variables . . . . . . . . . . . . . . . . . . . . . . . 168
2.4 Application . . . . . . . . . . . . . . . . . . . . . . . . . . . 169
3 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 169
4 State Space Representation for an Extended Object . . . . . . . . . 171
4.1 Measurement Model . . . . . . . . . . . . . . . . . . . . . . 171
4.2 Process Model . . . . . . . . . . . . . . . . . . . . . . . . . . 176
5 Multi-Target Tracking Algorithm . . . . . . . . . . . . . . . . . . . 176
5.1 Gating and Data Association . . . . . . . . . . . . . . . . . . 177
5.2 Track Handling . . . . . . . . . . . . . . . . . . . . . . . . . 179
6 Application Example and Use of Prior Information . . . . . . . . . 180
6.1 State Space Model . . . . . . . . . . . . . . . . . . . . . . . . 181
6.2 Using Prior Information in Extended Track Generation . . 181
6.3 Experiments and Results . . . . . . . . . . . . . . . . . . . . 183
7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 186
A Appendix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 186
xiv CONTENTS

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 188

D Road Intensity Based Mapping using Radar Measurements with a

PHD Filter 193
1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 195
2 Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 197
3 GM-PHD Filter . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201
3.1 Time Evolution . . . . . . . . . . . . . . . . . . . . . . . . . 201
3.2 Measurement Update . . . . . . . . . . . . . . . . . . . . . . 202
4 Joint Clustering and Estimation . . . . . . . . . . . . . . . . . . . . 203
4.1 K-Means Clustering . . . . . . . . . . . . . . . . . . . . . . 203
4.2 Regression Clustering . . . . . . . . . . . . . . . . . . . . . 203
4.3 Road Edge Estimation . . . . . . . . . . . . . . . . . . . . . 205
5 Merging . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 206
5.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . 206
5.2 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 208
5.3 Road Mapping . . . . . . . . . . . . . . . . . . . . . . . . . . 208
6 Road Mapping Example . . . . . . . . . . . . . . . . . . . . . . . . 212
6.1 System Model . . . . . . . . . . . . . . . . . . . . . . . . . . 212
6.2 Spawn Process . . . . . . . . . . . . . . . . . . . . . . . . . . 213
6.3 GM-PHD Filter Recursion . . . . . . . . . . . . . . . . . . . 213
6.4 Experiments and Results . . . . . . . . . . . . . . . . . . . . 215
7 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 217
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 219

E Extended Target Tracking Using a Gaussian-Mixture PHD Filter 223

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 225
2 Target Tracking Problem Formulation . . . . . . . . . . . . . . . . . 227
3 Gaussian-Mixture Implementation . . . . . . . . . . . . . . . . . . 229
4 Partitioning the Measurement Set . . . . . . . . . . . . . . . . . . . 233
4.1 Distance Partitioning . . . . . . . . . . . . . . . . . . . . . . 233
4.2 Alternative Partitioning Methods . . . . . . . . . . . . . . . 236
4.3 Sub-Partitioning . . . . . . . . . . . . . . . . . . . . . . . . . 237
5 Target Tracking Setup . . . . . . . . . . . . . . . . . . . . . . . . . . 239
6 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 241
6.1 Partitioning Methods . . . . . . . . . . . . . . . . . . . . . . 241
6.2 Comparison with GM-PHD . . . . . . . . . . . . . . . . . . 241
6.3 Standard Single Measurement Targets . . . . . . . . . . . . 244
6.4 Unknown Expected Number of Measurements γ . . . . . . 245
7 Experiment Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 249
7.1 Experiment with Close Targets . . . . . . . . . . . . . . . . 250
7.2 Experiment with Occlusion . . . . . . . . . . . . . . . . . . 250
8 Conclusions and Future Work . . . . . . . . . . . . . . . . . . . . . 252
A Appendix . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 254
A.1 Proof of Theorem 10 . . . . . . . . . . . . . . . . . . . . . . 254
A.2 Variable Probability of Detection for the Laser Sensor . . . 254
CONTENTS xv

Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 256

F Estimating the Shape of Targets with a PHD Filter 259

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 261
2 The RFS Extended Target Model . . . . . . . . . . . . . . . . . . . . 263
3 Filtering Framework . . . . . . . . . . . . . . . . . . . . . . . . . . . 266
3.1 Prediction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 267
3.2 Measurement Update . . . . . . . . . . . . . . . . . . . . . . 267
4 RBPF-PHD Implementation . . . . . . . . . . . . . . . . . . . . . . 270
4.1 PHD Prediction and Update . . . . . . . . . . . . . . . . . . 270
4.2 Particle Update . . . . . . . . . . . . . . . . . . . . . . . . . 271
4.3 Algorithm . . . . . . . . . . . . . . . . . . . . . . . . . . . . 272
5 Simulation Example . . . . . . . . . . . . . . . . . . . . . . . . . . . 274
6 Conclusions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 276
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 279

G Tire Radii and Vehicle Trajectory Estimation Using a Marginalized PF 281

1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 283
2 Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 285
3 Parameter and State Estimation . . . . . . . . . . . . . . . . . . . . 288
3.1 State Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . 288
3.2 Parameter Estimation . . . . . . . . . . . . . . . . . . . . . . 289
3.3 Noise Marginalization . . . . . . . . . . . . . . . . . . . . . 290
4 Models for Comparison . . . . . . . . . . . . . . . . . . . . . . . . . 292
4.1 Augmented State Vector . . . . . . . . . . . . . . . . . . . . 292
4.2 Measurement Noise Estimation . . . . . . . . . . . . . . . . 294
4.3 Summarizing the Four Methods . . . . . . . . . . . . . . . . 295
4.4 Wheel Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . 295
5 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 296
6 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 301
Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 304

Index 307
 Notation

Operators

Notation Meaning
min minimize
max maximize
arg min A the value x that minimizes A
x
arg max A the value x that maximizes A
x
E expectation
Cov covariance
Var variance
diag(a, b) diagonal matrix with elements a and b
blkdiag(A, B) Block diagonal matrix with matrices A and B
Pr(A) probability of event A
AT transpose of matrix A
Std standard deviation
Tr(A) trace of matrix A
|x| absolute value of x (or cardinality if x is a set)
x̂ estimate of the stochastic variable x
, equal by definition
∼ is distributed according to
∝ proportional to
∈ belongs to
ẋ time derivative of x

xvii
xviii Notation

Probability theory and Distributions

Notation Meaning
iW inverse Wishart distribution
D intesity or phd function
N normal or Gaussian distribution
NiW normal inverse Wishart distribution
Pois Poisson ditribution
St student-t distribution
U uniform distribution
p(x) density function of x
p(x, y) joint density function of x and y
p(x|y) conditional density function of x given y

Sets

Notation Meaning
N set of natural numbers
M set of map variables or features
R set of real numbers
S set of measurement generating points
X set of state variables
Z set of measurements
Nx number of elements in the set X
∅ empty set

Random Variables

Notation Meaning
x state
y measurement (no data association needed)
z measurement (data association needed)
u input
P state covariance
w process noise
Q process noise covariance
e measurement noise
R measurement noise covariance
m map state
p point feature
s state of a measurement generating point
θ parameter
nx dimension of the vector x
x̂k|κ estimate of x at time given measurements y1:κ
Notation xix

Geometry and dynamics

Notation Meaning
c Cartesian representation
p polar representation
x Cartesian position x-coordinate (longitudinal)
y Cartesian position y-coordinate (lateral)
z Cartesian position z-coordinate (vertical)
w width
l length
a acceleration
v velocity
u, v pixel coordinates
d displacement vector
r range (between sensor and target)
ψ heading angle, yaw angle or bearing (angle around z-
axis)

Miscellaneous

Notation Meaning
k discrete time variable
T sample time
lb wheel base (distance between front and rear axle)
lt wheel track (distance between left and right wheel)
co road curvature
c1 road curvature derivative
xx Notation

Abbreviations

Abbreviation Meaning
abs antilock braking system
acc adaptive cruise control
c2c car to car communication
can controller area network
eiv errors in variables
eio errors in output
ekf extended Kalman filter
ela emergency lane assist
et-gm-phd extended target Gaussian mixture probability hypoth-
esis density
fisst finite set statistics
fmcw frequency modulated continuous-wave (a radar sys-
tem)
gm-phd gaussian mixture probability hypothesis density
gnn global nearest neighbor (data association)
gps global positioning system
imu inertial measurement unit
kf Kalman filter
lka lane keeping assistance
ls least Squares
mae mean absolute error
map maximum a posteriori
mc Monte Carlo (experiments based on repeated random
sampling)
mgp measurement generating point (reflection point on tar-
get)
ml maximum likelihood
mpf marginalized particle filter
nn nearest neighbor (data association)
ogm occupancy grid map
olr optical lane recognition
ospa optimal subpattern assignment (multitarget evalua-
tion measure)
pdf probability density function
phd probability hypothesis density
pf particle Filter
rfs random finite set
rmse root mean square error
slam simultaneous localization and mapping
s.t. subject to
ukf unscented Kalman filter
virt virtual sensor measurement
wls weighted least squares
 Part I

Background
 Introduction
1
This thesis is concerned with the problem of estimating the motion of a vehicle
and the characteristics of its surroundings. More specifically, the description of
the ego vehicle’s surroundings consists of other vehicles and stationary objects
as well as the geometry of the road. The signals from several different sensors,
including camera, radar and inertial sensor, must be combined and analyzed to
compute estimates of various quantities and to detect and classify many objects
simultaneously. Sensor fusion allows the system to obtain information that is
better than if it was obtained by individual sensors.
Situation awareness is the perception of environmental features, the comprehen-
sion of their meaning and the prediction of their status in the near future. It
involves being aware of what is happening in and around the vehicle to under-
stand how the subsystems impact on each other.
Sensor fusion is introduced in Section 1.1 and its application within the automo-
tive community is briefly discussed in Section 1.2. The study presented in this
thesis was conducted within two Swedish research project, briefly described in
Section 1.3 and 1.4. The sensor fusion framework and its components, such as
infrastructure, estimation algorithms and various mathematical models, are all
introduced in Section 1.5. Finally, the chapter is concluded with an overview
of the author’s publications in Section 1.6, a statement of the contributions in
Section 1.7 and the outline of this thesis given in Section 1.8.

1.1 Sensor Fusion

Sensor fusion is the process of using information from several different sensors to
compute an estimate of the state of a dynamic system. The resulting estimate is

3
4 1 Introduction

Sensors Sensor Fusion Applications

State Estimation State

.. Estimate ..
. Process Model .

Measurement Model

Figure 1.1: The main components of the sensor fusion framework are shown
in the middle box. The framework receives measurements from several sen-
sors, fuses them and produces one state estimate, which can be used by sev-
eral applications.

in some sense better than it would be if the sensors were used individually. The
term better can in this case mean more accurate, more reliable, more available
and of higher safety integrity. Furthermore, the resulting estimate may in some
cases only be possible to obtain by using data from different types of sensors.
Figure 1.1 illustrates the basic concept of the sensor fusion framework. Many
systems have traditionally been stand alone systems with one or several sensors
transmitting information to only one single application. Using a sensor fusion
approach it might be possible to remove one sensor and still perform the same
tasks, or add new applications without the need to add new sensors.
Sensor fusion is required to reduce cost, system complexity and the number of
components involved and to increase accuracy and confidence of sensing.

1.2 Automotive Sensor Fusion

Within the automotive industry there is currently a huge interest in active safety
systems. External sensors are increasingly important and typical examples used
in this work are radar sensors and camera systems. Today, a sensor is usually con-
nected to a single function. However, all active safety functions need information
about the state of the ego vehicle and its surroundings, such as the lane geometry
and the position of other vehicles. The use of signal processing and sensor fusion
to replace redundant and costly sensors with software attracted recent attention
in the ieee Signal Processing Magazine (Gustafsson, 2009).
The sensors in a modern passenger car can be divided into a number of sub-
groups; there are internal sensors measuring the motion of the vehicle, external
sensors measuring the objects surrounding the vehicle and there are sensors com-
municating with other vehicles and with the infrastructure. The communication
between sensors, fusion framework, actuators and controllers is made possible by
1.2 Automotive Sensor Fusion 5

the controller area network (can). It is a serial bus communication protocol de-
veloped by Bosch in the early 1980s and presented by Kiencke et al. (1986) at the
sae international congress in Detroit. An overview of the can bus, which has be-
come the de facto standard for automotive communication, is given in Johansson
et al. (2005).

Internal sensors are often referred to as proprioceptive sensors in the literature.

Typical examples are gyroscopes, primarily measuring the yaw rate about the ve-
hicle’s vertical axis, and accelerometers, measuring the longitudinal and lateral
acceleration of the vehicle. The velocity of the vehicle is measured using induc-
tive wheel speed sensors and the steering wheel position is measured using an
angle sensor. External sensors are referred to as exteroceptive sensors in the lit-
erature, typical examples are radar (RAdio Detection And Ranging), lidar (LIght
Detection And Ranging) and cameras.

An example of how a radar and a camera may be mounted in a passenger car

is illustrated in Figure 1.2. These two sensors complement each other very well,
since the advantage of the radar is the disadvantage of the camera and vice versa.
A summary of the two sensors’ properties is presented in Table 1.1 and in e.g.,
Jansson (2005).

As already mentioned, the topic of this thesis is how to estimate the state vari-
ables describing the ego vehicle’s motion and the characteristics of its surround-
ings. The ego vehicle is one subsystem, labeled (E) in this work. The use of
data from the vehicle’s actuators, e.g., the transmission and the steering wheel,
to estimate a change in position over time is referred to as odometry. The ego
vehicle’s surroundings consists of other vehicles, referred to as targets (T), and
stationary objects as well as the shape and the geometry of the road (R). Map-
ping is the problem of integrating the information obtained by the sensors into a
given representation, denoted (M), see Adams et al. (2007) for a recent overview
and Thrun (2002) for an older survey. Simultaneous localization and mapping
(slam) is an approach used by autonomous vehicles to build a map while at the

Table 1.1: Properties of radar and camera for object detection

Camera Radar
Detects other vehicles, lane other vehicles, sta-
markings, pedestri- tionary objects
ans
Classifies objects yes no
Azimuth angle high accuracy medium accuracy
Range low accuracy very high accuracy
Range rate not very high accuracy
Field of View wide narrow
Weather Condi- sensitive to bad visi- less sensitive
tions bility
6 1 Introduction

(a) (b)

Figure 1.2: Figure (a) shows the camera and Figure (b) the front looking
radar in a Volvo S60 production car. Courtesy of Volvo Car Corporation.

same time keeping track of their current locations, see e.g., Durrant-Whyte and
Bailey (2006); Bailey and Durrant-Whyte (2006). This approach is not treated in
this thesis.

1.3 Sensor Fusion for Safety

Parts of the work in this thesis have been performed within the research project
Sensor Fusion for Safety (sefs), which is funded by the Swedish Intelligent Vehi-
cle Safety Systems (ivss) program. The project is a collaboration between Volvo
Technology, Volvo Cars, Volvo Trucks, Mecel, Chalmers University of Technology
and Linköping University.
The overall objective of this project is to obtain sensor fusion competence for au-
tomotive safety applications in Sweden by doing research within relevant areas.
This goal is achieved by developing a sensor fusion platform, algorithms, mod-
eling tools and a simulation platform. More specifically, the aim is to develop
general methods and algorithms for a sensor fusion system utilizing information
from all available sensors in a modern passenger car. The sensor fusion will pro-
vide a refined description of the vehicle’s environment that can be used by a num-
ber of different safety functions. The integration of the data flow requires new
specifications with respect to sensor signals, hardware, processing, architectures
and reliability.
The sefs work scope is divided into a number of work packages. These include at
a top level, fusion structure, key scenarios and the development of requirement
methods. The next level consists in work packages such as pre-processing and
modeling, the implementation of a fusion platform and research done on fusion
algorithms, into which this thesis can be classified. The use-case work package
consists of implementation of software and design of prototypes and demonstra-
tors. Finally, there is an evaluation and validation work package.
During the runtime of the sefs project, i.e., from 2005 until 2009, three PhD the-
ses (Schön, 2006; Gunnarsson, 2007; Danielsson, 2010) and three licentiate the-
1.4 Extended Target Tracking 7

ses (Bengtsson, 2008; Danielsson, 2008; Lundquist, 2009) have been produced.
An overview of the main results in the project is given in Ahrholdt et al. (2009)
and the sensor fusion framework is well described in Bengtsson and Danielsson
(2008). Furthermore it is worth mentioning some of the publications produced by
the project partners. Motion models for tracked vehicles are covered in Svensson
and Gunnarsson (2006); Gunnarsson et al. (2006); Sörstedt et al. (2011). A bet-
ter sensor model of the tracked vehicle is presented in Gunnarsson et al. (2007).
Detection of lane departures and lane changes of leading vehicles is studied in
Schön et al. (2006), with the goal to increase the accuracy of the road geometry es-
timate. Computational complexity for systems obtaining data from sensors with
different sampling rates and different noise distributions is studied in Schön et al.
(2007).
Paper D in this thesis describes a method to estimate and represent stationary
objects along the road edges. This publication is based on data collected from the
sefs prototype car and the work was carried out within the project.

1.4 Extended Target Tracking

The final parts of the work in the thesis have been performed within the frame
project grant Extended Target Tracking (621-2010-4301), founded by the Swedish
Research Council. This research project started in 2011 and lasts until 2014. The
overall goal is to study new ways of representing targets and their posterior dis-
tribution. The work is performed for different types of imagery sensors, such as
camera, radar and laser. More specifically the purpose of the project is to study
different options to extend the state vector of the target with specific and unusual
features, such as physical size and shape, physical properties, colors etc.
So far only a few articles, which relates to the work presented in this thesis, have
been published in the project. It is worth mentioning the following publications.
An approach to track bicycles from imagery sensor data is proposed by Ardeshiri
et al. (2011). It is based on detecting ellipsoids in the images, and treat these pair-
wise using a dynamic bicycle model. Wahlström et al. (2011) shows theoretically
and experimentally that the position and heading of a moving metallic target can
be tracked using two magnetometers placed on the ground, for surveillance and
traffic monitoring applications.
The Papers E and F in this thesis are produced within the extended target tracking
project. In these papers new approaches are proposed to represent and estimate
the position and size of targets which, due to their size, might generate more than
one measurement per time step.

1.5 Components of the Sensor Fusion Framework

A systematic approach to handle sensor fusion problems is provided by nonlin-
ear state estimation theory. Estimation problems are handled using discrete-time
8 1 Introduction

model based methods. The systems discussed in this thesis are primarily dy-
namic and they are modeled using stochastic difference equations. More specif-
ically, the systems are modeled using the discrete-time nonlinear state space
model
xk+1 = fk (xk , uk , wk , θk ), (1.1a)
yk = hk (xk , uk , ek , θk ), (1.1b)
where (1.1a) describes the evolution of the state variable x over time and (1.1b)
explains how the state variable x relates to the measurement y1 . The state vector
at time k is denoted by xk ∈ Rnx , with elements x1 , . . . , xnx being real numbers.
Sensor observations collected at time k are denoted by yk ∈ Rny , with elements
y1 , . . . , yny being real numbers. The model fk in (1.1a) is referred to as the pro-
cess model , the motion model , the dynamic model or the system model, and it
describes how the state propagates in time. The model hk in (1.1b) is referred to
as the measurement model or the sensor model and it describes how the state
is propagated into the measurement space. The random vector wk describes the
process noise, which models the fact that the actual state dynamics is usually
unknown. The random vector ek describes the sensor noise. Furthermore, uk
denotes the deterministic input signals and θk denotes the possibly unknown
parameter vector of the model.
The ego vehicle constitutes an important dynamic system in this thesis. The yaw
and lateral dynamics are modeled using the so called single track model. This
model will be used as an example throughout the thesis. Some of the variables
and parameters in the model are introduced in Example 1.1.
1.1 Example: Single Track Ego Vehicle Model
A so called bicycle model is obtained if the wheels at the front and the rear axle of
a passenger car are modeled as single wheels. This type of model is also referred
to as a single track model and a schematic drawing is given in Figure 1.3. Some
examples of typical variables and parameters are:
State variables x: the yaw rate ψ̇E and the body side slip angle β, i.e.,
h iT
x = ψ̇E β . (1.2)

Measurements y: the yaw rate ψ̇E and the lateral acceleration ay , i.e.,
h iT
y = ψ̇E ay , (1.3)
which both are measured by an inertial measurement unit (imu).
Input signals u: the steering wheel angle δs , which is measured with an angu-
lar sensor at the steering column and the velocity v̇, which is measured by
1 Note that the measurement vector is denoted y when the sensor measures one or several specific
quantities, e.g., acceleration and yaw rate from an imu sensor. However, when the number of sensor
measurements (observations) depends on the environmental circumstances the measurement vector
is denoted z, e.g., for a radar which observes an unknown number of targets. This usually leads to a
data association problem.
1.5 Components of the Sensor Fusion Framework 9

ρ αf
δf
v
β
ψE y W
CoG
αr
OW x

Figure 1.3: Illustration of the geometry for the single track model, describing
the motion of the ego vehicle. The ego vehicle velocity vector v is defined
from the center of gravity (CoG) and its angle to the longitudinal axis of the
vehicle is denoted by β, referred to as the body side slip angle. Furthermore,
the slip angles are referred to as αf and αr . The front wheel angle is denoted
by δf and the current driven radius is denoted by ρ.

wheel speed sensors, i.e.,

h iT
u = δs v . (1.4)

Parameters θ: the vehicle mass m, which is weighed before the tests, the steering
ratio is between the steering wheel angle and the front wheels, which has to
be estimated in advance, and the tire parameter Cα , which is estimated on-
line, since the parameter value changes due to different road and weather
conditions.

The nonlinear models f and h are specified in Chapter 2.

The model (1.1) must describe the essential properties of the system, but it must
also be simple enough to be efficiently used within a state estimation algorithm.
Chapter 3 describes algorithms that are used to compute estimates of the state xk
and the parameter θk in (1.1).

Before describing the individual steps of the sensor fusion framework another
important example is presented in Example 1.2.

1.2 Example: Object Tracking

Other objects, such as vehicles or stationary objects on and along the road, are
tracked using measurements from a radar mounted in the ego vehicle. A simple
model for one such tracked object is given by using the following variables:
10 1 Introduction

State variables x: Cartesian position of tracked targets i = 1, . . . , Nx in a world

h iT
fixed coordinate frame W , i.e., x(i) = xW yW .
Measurements z: Range and azimuth angle to objects m = 1, . . . , Nz measured by
h iT
the radar in the ego vehicle fixed coordinate frame E, i.e., z(m) = r E ψ .
At every time step k, Nz observations are obtained by the radar. Hence, the
nradar deliverso Nz range and azimuth measurements in a multi-sensor set Z =
z(1) , . . . , z(Nz ) to the sensor fusion framework. The sensor fusion framework
currently
n alsoo tracks Nx targets. The multi-target state is given by the set X =
x(1) , . . . , x(Nx ) where x(1) , . . . , x(Nx ) are the individual states.
Obviously, the total number of state variables in the present example is 2Nx and
the total number of measurements is 2Nz . This issue may be compared to Exam-
ple 1.1, where the size of the y-vector corresponds to the total number of mea-
surements at time k. Typically, the radar also observes false detections, referred
to as clutter, or receives several measurements from the same target, i.e., Nz is
seldom equal to Nx for radar sensors.

The different steps of a typical sensor fusion algorithm, as the central part of
the larger framework, are shown in Figure 1.4. The algorithm is initiated using
a prior guess of the state x0 or, if it is not the first iteration, the state estimate
x̂k−1|k−1 from the previous time step k − 1 is used. New measurements Zk are col-
lected from the sensors and preprocessed at time k. Model (1.1) is used to predict
the state estimate x̂k|k−1 and the measurement ẑk|k−1 . For Example 1.2 it is nec-
essary to associate the radar observations Zk with the predicted measurements
Zk|k−1 of the existing state estimates and to manage the tracks, i.e., initiate new
b
states and remove old, invalid states. The data association and track management
are further discussed in Section 4.2. Finally, the new measurement zk is used to
calculate the state estimate x̂k|k at time k in the so called measurement update
step. The prediction and measurement update are described in Section 3.2. This
algorithm is iterated, x̂k|k is used to predict x̂k+1|k , new measurements Zk+1 are
collected at time k + 1 and so on. The state estimation theory, as part of the sensor
fusion framework, is discussed further in Chapter 3. Note that in Example 1.1,
the data association and track management are obviously not needed, since there
the data association is assumed fixed. In the example the measurements y from
the pre-processing are fed directly to the measurement update.

1.6 Publications
Published work of the author that are of relevance to this thesis are listed below.
The publications are clustered in groups preambled by a short summary. The
publications are principally listed in chronological order.
The author has been involved in the development of an active steering system
prior to starting his PhD-studies. The active steering system superimposes an
1.6 Publications 11

Zk pre-
sensor
processing

Zk

p(xk |z1:k−1 ) data Zk , Λk track Zk , Λk measurement p(xk |z1:k )

prediction
association p(xk |z1:k−1 ) management p(xk |z1:k−1 ) update

p(xk−1 |z1:k−1 ) p(xk |z1:k )

time step

Figure 1.4: The new measurements Zk contain new information and are as-
sociated to the predicted states b
Xk|k−1 and thereafter used to update them to
obtain the improved state estimates b Xk|k .

electronically controlled angle to the driver’s steering input. The aim of the sys-
tem is to reduce the driver’s steering efforts at low velocities, to reduce the vehi-
cle’s sensibility at very high velocities and to intervene when the vehicle tends to
become unstable. The system is thoroughly described in
W. Reinelt and C. Lundquist. Mechatronische Lenksysteme: Mod-
ellbildung und Funktionalität des Active Front Steering. In R. Is-
ermann, editor, Fahrdynamik Regelung - Modellbildung, Fahrassis-
tenzsysteme, Mechatronik, pages 213–236. Vieweg Verlag, September
2006a.
W. Reinelt, W. Klier, G. Reimann, C. Lundquist, W. Schuster, and R.
Großheim. Active front steering for passenger cars: System modelling
and functions. In Proceedings of the IFAC Symposium on Advances
in Automotive Control, Salerno, Italy, April 2004.
Sensor fusion based monitoring systems are described in
W. Reinelt, C. Lundquist, and H. Johansson. On-line sensor monitor-
ing in an active front steering system using extended Kalman filtering.
In Proceedings of the SAE World Congress, SAE paper 2005-01-1271,
Detroit, MI, USA, April 2005.
W. Reinelt and C. Lundquist. Observer based sensor monitoring in
an active front steering system using explicit sensor failure modeling.
In Proceedings of the IFAC World Congress, Prague, Czech Republic,
July 2005.
S. Malinen, C. Lundquist, and W. Reinelt. Fault detection of a steering
wheel sensor signal in an active front steering system. In Preprints
of the IFAC Symposium on SAFEPROCESS, pages 547–552, Beijing,
China, August 2006.
C. Lundquist and W. Reinelt. Electric motor rotor position monitor-
ing method for electrically aided steering system e.g. steer by wire,
12 1 Introduction

for motor vehicle, involves outputting alarm when difference between

measurement value and estimated value of motor exceeds threshold.
German Patent Application DE 102005016514 October 12, 2006, Pri-
ority date April 8, 2006.

W. Reinelt, C. Lundquist, and S. Malinen. Automatic generation of a

computer program for monitoring a main program to provide opera-
tional safety. German Patent Application DE 102005049657 April 19,
2007, Priority date October 18, 2005.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Operat-

ing method for electronic servo steering system of vehicle, involves
presetting steering wheel angle by steering mechanism as measure
for desired wheel turning angle for steering wheel of vehicle. Ger-
man Patent Application DE 102006052092 May 8, 2008, Priority date
November 4, 2006.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Electronic

servo steering system operating method for motor vehicle, involves
recognizing track misalignment of vehicle when forces differentiate
around preset value from each other at preset period of time in magni-
tude and/or direction. German Patent DE 102006043069 December 3,
2009, Priority date September 14, 2006.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Motor vehi-

cle’s electronically regulated servo steering system operating method,
involves comparing actual value of measured value with stored prac-
tical value of corresponding measured value. German Patent DE 10
2006 040 443 January 27, 2011, Priority date August 29, 2006.
Sensor fusion based utility functions to improve driving comfort, safety or agility
are described in

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Operating

method for electronic power-assisted steering system of vehicle, in-
volves overlapping additional angle, which is disabled after re-start
of utility function. German Patent Application DE 102006041236
Mars 6, 2008, Priority date September 2, 2006a.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Operating

method for electronic power-assisted steering system of vehicle, in-
volves re-starting utility function, and after re-start of utility func-
tion superimposition of additional angle is unlatched. German Patent
DE 102006041237 December 3, 2009, Priority date September 2, 2006b.

G. Reimann and C. Lundquist. Method for operating electronically

controlled servo steering system of motor vehicle, involves determin-
ing steering wheel angle as measure for desired steering handle an-
gle by steering handle for steering wheels of motor vehicle. German
1.6 Publications 13

Patent Application DE 102006053029 May 15, 2008, Priority date

November 10, 2006.
C. Lundquist and R. Großheim. Method and device for determining
steering angle information. German Patent Application DE 10 2007
000 958 Mai 14, 2009, International Patent Application WO 2009
047 020 April 16, 2009 and European Patent Application EP 2205478
April 16, 2009, Priority date October 2, 2007.
A reverse driving assistant for passenger cars with trailers based on sensor data
of the angle between car and trailer is presented in
C. Lundquist and W. Reinelt. Rückwärtsfahrassistent für PKW mit
Aktive Front Steering. In Proceedings of the AUTOREG (Steuerung
und Regelung von Fahrzeugen und Motoren, VDI Bericht 1931, pages
45–54, Wiesloch, Germany, March 2006b.
C. Lundquist and W. Reinelt. Back driving assistant for passenger cars
with trailer. In Proceedings of the SAE World Congress, SAE paper
2006-01-0940, Detroit, MI, USA, April 2006a.
W. Reinelt and C. Lundquist. Method for assisting the driver of a mo-
tor vehicle with a trailer when reversing. German Patent DE 10 2006
002 294 February 24, 2011, European Patent Application EP 1810913
July 25, 2007 and Japanese Patent Application JP 2007191143 Au-
gust 2, 2007, Priority date January 18, 2006.
Furthermore, a control system which stabilizes a vehicle trailer combination, based
on the same sensor information as the publications listed above, is described in
C. Lundquist. Method for stabilizing a vehicle combination. U.S.
Patent US 8010253 August 30, 2011 and German Patent Application
DE 102007008342 August 21, 2008, Priority date February 20, 2007.
The functional safety concept for the active steering system, among others includ-
ing a sensor fusion based monitoring system, is described in
W. Reinelt and C. Lundquist. Controllability of active steering sys-
tem hazards: From standards to driving tests. In Juan R. Pimintel,
editor, Safety Critical Automotive Systems, pages 173–178. SAE In-
ternational, 400 Commonwealth Drive, Warrendale, PA, USA, August
2006b.
During his time as a PhD student the author has been involved in the following
publications. Starting with road geometry estimation, i.e., describing the curva-
ture of the lane, which is covered in
C. Lundquist and T. B. Schön. Road geometry estimation and vehicle
tracking using a single track model. In Proceedings of the IEEE Intel-
ligent Vehicles Symposium, pages 144–149, Eindhoven, The Nether-
lands, June 2008.
14 1 Introduction

C. Lundquist and T. B. Schön. Joint ego-motion and road geometry

estimation. Information Fusion, 12:253–263, October 2011.
Road edge estimation primarily aims to estimate the position and shape of sta-
tionary objects next to the road. Two approaches have been studied; road edges
modeled as extended targets are described in
C. Lundquist and T. B. Schön. Estimation of the free space in front
of a moving vehicle. In Proceedings of the SAE World Congress, SAE
paper 2009-01-1288, Detroit, MI, USA, April 2009a.
C. Lundquist, U. Orguner, and T. B. Schön. Tracking stationary ex-
tended objects for road mapping using radar measurements. In Pro-
ceedings of the IEEE Intelligent Vehicles Symposium, pages 405–410,
Xi’an, China, June 2009.
C. Lundquist, U. Orguner, and F Gustafsson. Estimating polynomial
structures from radar data. In Proceedings of the International Con-
ference on Information Fusion, Edinburgh, UK, July 2010b.
C. Lundquist, U. Orguner, and F. Gustafsson. Extended target track-
ing using polynomials with applications to road-map estimation. IEEE
Transactions on Signal Processing, 59(1):15–26, January 2011c.
and an approach where the stationary objects along the road are represented by
an intensity map is presented in
C. Lundquist, L. Danielsson, and F. Gustafsson. Random set based
road mapping using radar measurements. In Proceedings of the Eu-
ropean Signal Processing Conference, pages 219–223, Aalborg, Den-
mark, August 2010a.
C. Lundquist, L. Hammarstrand, and F. Gustafsson. Road intensity
based mapping using radar measurements with a probability hypoth-
esis density filter. IEEE Transactions on Signal Processing, 59(4):1397–
1408, April 2011b.
A method to on-line estimate the cornering stiffness parameters of the tires is
presented in
C. Lundquist and T. B. Schön. Recursive identification of cornering
stiffness parameters for an enhanced single track model. In Proceed-
ings of the IFAC Symposium on System Identification, pages 1726–
1731, Saint-Malo, France, July 2009b.
An overview of the results from the sefs project, and specifically an overview
about mapping techniques, is given in
M. Ahrholdt, F. Bengtsson, L. Danielsson, and C. Lundquist. SEFS
– results on sensor data fusion system development. In Proceedings
of the World Congress on Intelligent Transportation Systems and Ser-
vices, Stockholm, Sweden, September 2009.
1.6 Publications 15

C. Lundquist. Automotive Sensor Fusion for Situation Awareness. Li-

centiate Thesis No 1422, Department of Electrical Engineering, Lin-
köping University, Sweden, 2009.

C. Lundquist, T. B. Schön, and F. Gustafsson. Situational awareness

and road prediction for trajectory control applications. In A. Eskan-
darian, editor, Handbook of Intelligent Vehicles, chapter 24. Springer,
November 2011e.

Extended target tracking with a phd filter is described in

K. Granström, C. Lundquist, and U Orguner. A Gaussian mixture

PHD filter for extended target tracking. In Proceedings of the In-
ternational Conference on Information Fusion, Edinburgh, UK, July
2010.

C. Lundquist, K. Granström, and U. Orguner. Estimating the shape of

targets with a PHD filter. In Proceedings of the International Confer-
ence on Information Fusion, Chicago, IL, USA, July 2011a.

U. Orguner, C. Lundquist, and K. Granström. Extended target track-

ing with a cardinalized probability hypothesis density filter. In Pro-
ceedings of the International Conference on Information Fusion, pages
65–72, Chicago, IL, USA, July 2011.

K. Granström, C. Lundquist, and U Orguner. Tracking rectangular

and elliptical extended targets using laser measurements. In Proceed-
ings of the International Conference on Information Fusion, Chicago,
IL, USA, July 2011b.

K. Granström, C. Lundquist, and U. Orguner. Extended target track-

ing using a Gaussian-mixture PHD filter. IEEE Transactions on Aero-
space and Electronic Systems, 2011a. Under review.

An approach to use camera data to improve the position estimate of a vehicle is

presented in

E. Nilsson, C. Lundquist, T. B. Schön, D. Forslund, and J. Roll. Vehicle

motion estimation using an infrared camera. In Proceedings of the
World Congress of the International Federation of Automatic Control,
Milan, Italy, August 2011.

A marginalized particle filter based method to estimate the tire radii, and thereby
being able to detect pressure losses is presented in

E. Özkan, C. Lundquist, and F. Gustafsson. A Bayesian approach to

jointly estimate tire radii and vehicle trajectory. In Proceedings of the
IEEE Conference on Intelligent Transportation Systems, Washington
DC, USA, October 2011.
16 1 Introduction

C. Lundquist, E. Özkan, and F. Gustafsson. Tire radii estimation us-

ing a marginalized particle filter. IEEE Transactions on Intelligent
Transportation Systems, 2011d. Submitted.
Finally, the pedagogic contribution
C. Lundquist, M. Skoglund, K. Granström, and T. Glad. How peer-
review affects student learning. In Utvecklingskonferens för Sveriges
ingenjörsutbildingar, Norrköping, Sweden, November 2011f.
describes how peer-review can improve student’s learning. This work is imple-
mented in a course in sensor fusion.

1.7 Contributions
The main contributions of this thesis are briefly summarized and presented be-
low:
Mapping: An overview of different methods to map stationary objects in the sur-
roundings of the vehicle are summarized in Paper A.
Road curvature estimation: The estimation from camera and radar is improved
by using information about the ego vehicle motion. The results are pre-
sented in Paper B.
Road edge estimation: Errors in variables methods are applied in order to track
the road edges using point measurements, and the results are given in Pa-
per C. The edges are modeled as extended targets.
Intensity based map: An intensity function is used to represent stationary ob-
jects along the road. Structures in the road are used to improve the update
and representation of the map as described in Paper D.
Extended target tracking: A modification of the gm-phd filter, which is able to
handle extended targets, is presented in Paper E.
Target size estimation: An approach to estimate the size and the shape is intro-
duced in Paper F. The tracking framework contains a hybrid state space
where measurement generating points and the measurements are modeled
by random finite sets and target states by random vectors.
Tire radii estimation: The marginalized particle filter is applied to the problem
of estimating the tire radii of a vehicle. The Bayesian method, presented in
Paper G, is efficient and the estimate is more accurate than estimates from
comparable methods.

1.8 Thesis Outline

The thesis is divided into two parts. The first part contains an overview of the
research area and background theory for the second part, which contains edited
1.8 Thesis Outline 17

versions of published papers.

1.8.1 Outline of Part I

An overview of vehicle, road and target models is given in Chapter 2, as well
as some methods to simplify and make the models applicable for automotive
applications. The chapter is concerned with the inner part of the model based es-
timation process i.e., the process model and the measurement model illustrated
by the two white rectangles in Figure 1.1.
Chapter 3 describes estimation and filtering algorithms, which have been applied
to derive the results in Part II. The estimation process is illustrated by the gray
rectangle in Figure 1.1, and the chapter has a tutorial character.
Target tracking is the subject of Chapter 4, and it describes the framework which
takes care of incoming measurements. The framework is illustrated by the black
rectangle in Figure 1.1.
Chapter 5 describes a filtering framework which propagates the so called phd,
being an approximation of a finite random set of targets. The phd-filter unifies
the track management and the estimation process, i.e., the black and the gray box
in Figure 1.1.
Finally, the work is summarized and the next steps for future work are given in
Chapter 6.

1.8.2 Outline of Part II

Part II presents a collections of publications where the author of this thesis has
been involved. The aim of this section is to give a very brief abstract to the publi-
cations, describe how they relate to each other and emphasize the authors contri-
bution.
The first four papers are concerned about estimating and mapping the road and
the surroundings of the vehicle.
Paper A,
C. Lundquist, T. B. Schön, and F. Gustafsson. Situational awareness
and road prediction for trajectory control applications. In A. Eskan-
darian, editor, Handbook of Intelligent Vehicles, chapter 24. Springer,
November 2011e.
gives an overview of mapping methods for automotive applications. The paper
summarizes both standard approaches, of which some have been implemented
by the author, and methods developed by the author in other publications. There
is a slight overlap of the material presented in the paper with other papers in this
thesis. The author has written the major part of this paper, besides the section
covering lane tracking using a camera.
Paper B,
18 1 Introduction

C. Lundquist and T. B. Schön. Joint ego-motion and road geometry

estimation. Information Fusion, 12:253–263, October 2011.
presents early work of the author. It is shown how the road curvature estimate
can be improved and made more robust by fusing lane marking information with
the position of other vehicles on the road and knowledge about the ego vehicles
motion. The lane information is obtained by a camera, the other vehicles are
tracked by a radar and the ego vehicle’s motion is measured with e.g., an imu.
The authors contribution is the model which connects the ego vehicle’s motion to
the estimate of the curvature.
Paper C,
C. Lundquist, U. Orguner, and F. Gustafsson. Extended target track-
ing using polynomials with applications to road-map estimation. IEEE
Transactions on Signal Processing, 59(1):15–26, January 2011c.
describes a method to track the road edges as extended targets. This is the first
article in a series where the author is dealing with extended targets. In this paper
the targets are represented by polynomials, which represent e.g., guardrails along
the road. The presented ideas are primarily the author’s contribution, however,
Dr. Umut Orguner has assisted with his knowledge in the area of target tracking
to make it possible to implement the ideas.
Paper D,
C. Lundquist, L. Hammarstrand, and F. Gustafsson. Road intensity
based mapping using radar measurements with a probability hypoth-
esis density filter. IEEE Transactions on Signal Processing, 59(4):1397–
1408, April 2011b.
is the last of the four papers describing models and algorithms to represent and
estimate the road. In this paper objects along road edges are represented by an
intensity function. The intensity in each position of the map describes the density
of targets in that position. Dr. Lars Hammarstrand came up with the initial ideas
to this paper, the realization of the idea was formed in discussion with the author.
The modifications to the standard phd filter were implemented by the author,
who also wrote the major part of the paper.
The next two papers treat methods to track moving targets, e.g., vehicles, which
have an extent.
Paper E,
K. Granström, C. Lundquist, and U. Orguner. Extended target track-
ing using a Gaussian-mixture PHD filter. IEEE Transactions on Aero-
space and Electronic Systems, 2011a. Under review.
introduces a phd filter based method to track targets which potentially might
give rise to more than one measurement. The authors came up with the idea
to this paper at a conference where the mathematical framework was presented.
1.8 Thesis Outline 19

The contribution of this paper is the implementation, including the necessary

assumptions, which must be made to make the theory computationally tractable.
All authors of this paper have contributed to the presented approach, however
the major part of the implementation and the writing was made by Karl Gran-
ström.
Paper F,
C. Lundquist, K. Granström, and U. Orguner. Estimating the shape of
targets with a PHD filter. In Proceedings of the International Confer-
ence on Information Fusion, Chicago, IL, USA, July 2011a.
presents a new approach to track the size and the shape of a target. Since the out-
put of many off-the-shelf sensors are point observations, a model must be found
to relate point measurements to a shape. In this paper a random finite set de-
scribes points on the target’s surface, which are assumed to have generated the
measurements. The idea is mainly developed by the author, also the implemen-
tation and writing were primarily performed by the author. However, the results
were improved through the valuable advice regarding target tracking given by
Dr. Umut Orguner.
The last paper is concerned about the ego vehicle only.
Paper G,
C. Lundquist, E. Özkan, and F. Gustafsson. Tire radii estimation us-
ing a marginalized particle filter. IEEE Transactions on Intelligent
Transportation Systems, 2011d. Submitted.
is based on theories recently developed by among others the two co-authors Dr.
Emre Özkan and Professor Fredrik Gustafsson. In this paper these theories are
applied to the problem of estimating the tire radii of a vehicle. The primary aim is
to detect tire pressure losses. The author has contributed by implementing these
theories, by applying them on real data and by writing major part of the paper.
Dr. Emre Özkan has helped to implement the method based on his knowledge in
the area.
 Models of Dynamic Systems
2
Given measurements from several sensors the objective is to estimate one or sev-
eral state variables, either by means of improving a measured signal or by means
of estimating a signal which is not, or can not, be directly measured. In either
case the relationship between the measured signals and the state variable must
be described, and the equations describing this relationship is referred to as the
measurement model. When dealing with dynamic systems, as is commonly the
case in automotive applications, the objective might be to predict the value of the
state variable at the next time step. The prediction equation is referred to as the
process model. This section deals with these two types of models.
As mentioned in the introduction in Section 1.5, a general model of dynamic
systems is provided by the nonlinear state space model
xk+1 = fk (xk , uk , wk , θk ), (2.1a)
yk = hk (xk , uk , ek , θk ). (2.1b)
Most mechanical and physical laws are provided in continuous-time, but com-
puter implementations are made in discrete-time, i.e., the process and measure-
ment models are derived in continuous-time t according to
ẋ(t) = a(x(t), u(t), w(t), θ(t), t), (2.2a)
y(t) = c(x(t), u(t), e(t), θ(t), t), (2.2b)
and are then discretized. Discretization is the topic of Section 2.2. Special cases
of the general state space model (2.1), such as the state space model with addi-
tive noise and the linear state space model, are discussed in Section 2.3 and 2.4,
respectively.
Several models for various applications are given in the papers in Part II. This

21
22 2 Models of Dynamic Systems

chapter therefore begins with an introduction to some of these models in Sec-

tion 2.1.

2.1 Overview of the Models Used in the Thesis

The single track model was introduced in Example 1.1 and it used as an example
throughout the first three chapters of this thesis. Furthermore, the model is a vi-
tal part for deriving the results in Paper B. The process and measurement models
of the single track model are given in Example 2.1.
2.1 Example: Single Track Model
The state variables x, the input signals u and the measurement signals y of the
ego vehicle model were defined in Example 1.1, and are repeated here for conve-
nience
h iT
x = ψ̇E β , (2.3a)
h iT
u = δf v , (2.3b)
h iT
y = ψ̇E ay . (2.3c)
Note that the front wheel angle δf is used directly as an input signal to simplify
the example. The continuous-time single track process and measurement models
are given by
 C l 2 cos δ +C l 2 
 − αf f f αr r −Cαf lf cos δf +Cαr lr Cαf lf tan δf 
Izz v ψ̇ +
E  Izz β + Izz

ẋ =    , (2.4a)

C l cos δ −C
− 1 + αf f 2 f αr r ψ̇E − αf l C cos δ f +C αr C αf sin δ f 

v m mv β+ mv

" #
ψ̇E
y= −Cαf lf cos δf +Cαr lr Cαf cos δf +Cαr Cαf sin δf , (2.4b)
mv ψ̇E − m β+ m
with parameter vector
h i
θ = lf lr Izz m Cαf Cαr , (2.5)
where lf and lr denotes the distances between the center of gravity of the vehicle
and the front and rear axles, respectively. Furthermore, m denotes the mass of
the vehicle and Izz denotes the moment of inertia of the vehicle about its vertical
axis in the center of gravity. The parameters Cαf and Cαf are called cornering
stiffness and describe the road tire interaction. Typical values for the parameters
are given in Table 2.1. The model is derived in many vehicle dynamics books,
e.g., Mitschke and Wallentowitz (2004); Wong (2001); Gillespie (1992); Rajamani
(2006).

Road models are treated in Paper A to D. A good overview of available and com-
monly used road models is given in Paper A, and therefore no introduction to
road modeling in general is given in this chapter. However, road models are of-
ten combined with ego vehicle models and described in one single state space
2.1 Overview of the Models Used in the Thesis 23

Table 2.1: Typical ranges for the vehicle parameters used in the single track
model.

m Izz Cα lf + lr
kg kgm2 N/rad m

1000 − 2500 850 − 5000 45000 − 75000 2.5 − 3.0

model. One approach is presented in Paper B and compared with two other vehi-
cle models with road interaction, which are described as follows.
The first model is described in Example 2.2 and it is commonly used for au-
tonomous driving and lane keeping. This model is well described by e.g., Dick-
manns (2007) and Behringer (1997). Note that the ego vehicle’s motion is mod-
eled with respect to a road fixed coordinate frame, unlike the single track model
in Example 2.1, which is modeled in a Cartesian world coordinate frame.

2.2 Example: Vehicle Model with Road Interaction

The relative angle between the vehicle’s longitudinal axis and the tangent of the
road is denoted ψRE . The notation is illustrated in the Figures 1 to 4 in Paper B
(Pages 140-144). Ackermann’s steering geometry is used to obtain the relation
v
ψ̇RE = δf − v · c0 , (2.6)
lb
where the current curvature of the road c0 is the inverse of the road’s radius. The
lateral displacement of the vehicle in the lane is given by
l˙R = v(ψRE + β). (2.7)
A process model for the body side slip angle was given in (2.4a), but since the yaw
rate ψ̇ is not part of the model in this section, equation (2.4a) has to be rewritten
according to
Cαf cos δf + Cαr
β̇ = − β
mv
Cαf lf cos δf − Cαr lr vx Cαf
!
− 1+ 2
tan δf + sin δf , (2.8)
v m lb mv
which is further simplified by assuming small angles, to obtain a linear model
according to
Cαf + Cαr Cαf
!
v
β̇ = − β+ − δ . (2.9)
mv mv lb f

The steering wheel angle might have a bias, for example if the sensor is not cal-
ibrated, which leads to an accumulation of the side slip angle β in (2.9). Other
reasons for a steering wheel angle bias is track torsion or strong side wind, which
the driver compensates for with the steering wheel. The problem is solved by
24 2 Models of Dynamic Systems

introducing an offset to the front wheel angel as a state variable according to

δfm = δf + δfoffs . (2.10)

Note that the curvature c0 is included in (2.6). The curvature c0 is usually treated
as a state variable and in this example the vehicle motion model is augmented
with the simple clothoid road model, see Paper A.

To summarize, the state variable vector is defined as

ψRE  relative angle between vehicle and road
   
 l  
 R   lateral displacement of vehicle in lane 
 β  
vehicle body side slip angle
   

x =  δf  = 
  
front wheel angle (2.11)


 offs   
δf   front wheel angle bias offset 

  
 c  
 0   road curvature at the ego vehicle 

c1 curvature derivative
and the process model is given by
vx
lb δf − vc0
 
 
v(ψRE + β)
 
ψ̇RE  
  
 
 ˙   Cαf +Cαr Cαf v
l
 R  − mv β + mv − lb δf 


ẋ =  β̇  = 
  
wδf
 .
 (2.12)
 δ̇   
 f   0
δ̇offs   

f  vc 
 0 
wc1
With the camera it is possible mo measure ψRE , lR and c0 . The front wheel angle
δf can be derived from the steering wheel angle δs if the steering geometry is
known; i.e., the measurement vector is
h iT
y = ψRE lR c0 δs (2.13)
and the measurement model is trivial. This model is used in the approach re-
ferred to as “fusion 3” in Paper B.

Another and simpler vehicle model is obtained if the side slip angle is omitted
and the ego vehicle’s yaw rate ψ̇E is used instead of the steering wheel angle. The
model is summarized in Example 2.3, and thoroughly described together with
results in Eidehall (2007); Eidehall et al. (2007); Eidehall and Gustafsson (2006);
Gern et al. (2000, 2001); Zomotor and Franke (1997).

More advanced vehicle models with more degrees of freedom, including the two
track model, are described by Schofield (2008).
2.1 Overview of the Models Used in the Thesis 25

2.3 Example: Simplified Vehicle Model with Road Interaction

The state variable vector is here defined as
h iT
x = ψRE lR ψ̇E c0 c1 , (2.14)
and the process model is simply given by
 vx c0 + ψ̇E 
 
ψ̇

 ˙RE   vx ψRE 

 l  
ẋ =  E  =  wψ̇RE  . (2.15)

 ċ0  
   vc0 
ċ1
wc1
 

The measurement vector is

h iT
y = ψRE lR c0 ψ̇E (2.16)
where the yaw rate is measured by the vehicle’s imu. This model is used in the
approach referred to as “fusion 2” in Paper B.

In this work, only measurements from the ego vehicle’s sensors are available; that
is the target’s motion is measured using the ego vehicle’s radar and camera. This
is the reason for why the target model is simpler than the ego vehicle model. The
targets play an important role in the sensor fusion framework presented in this
work, but little effort has been spent modeling their motion. Instead standard
models from target tracking literature are used. A survey of different process
models and measurement models are given by Rong Li and Jilkov (2003) and
Rong Li and Jilkov (2001), respectively. The subject is also covered in the books
by Blackman and Popoli (1999) and Bar-Shalom et al. (2001). One typical target
model is given in Example 2.4. This model is used in the Papers F and G, and a
somewhat similar model, called constant velocity model, is used in Paper E.

2.4 Example: Coordinated Turn Model

The coordinated turn model is commonly used to model moving targets. The ego
vehicle’s radar and camera measures the range rTi Es and the azimuth angle ψTi Es
to target number i as described in the introduction in Example 1.2. The states of
the coordinated turn model in polar velocity are given by
 W  
xTi W  x-position in W -frame

 W   
yT W  y-position in W -frame
 i   
x =  vTi  =  longitudinal velocity  . (2.17)
heading angle
   
 ψTi   

ψ̇T
 yaw rate
i
26 2 Models of Dynamic Systems

The process and measurement models are given by

 W   T
ẋTi W  v i cos ψTi   0 
  
 W   Ti
ẏT W   v sin ψT   0 
 i   i
ẋ =  v̇Ti  =  0  +  w Ti  (2.18a)
  v̇ 
  0 
ψ̇Ti
  
 ψ̇T     
 i    w 
ψ̈Ti 0 ψ̈ Ti
q  
#  xW − xW − xE 2 + yW − yW − yE 2 
 
"
r  Ti W EW Es E Ti W EW Es E 
z = Ti Es =   + e (2.18b)
 
yW
ψTi Es  Ti W
arctan W − ψE − ψEs E


xT
iW

where (xEEs E , yEEs E , ψEs E )

represents the sensor mounting position and orientation
in the ego vehicle coordinate frame E. The ego vehicle’s position in the world
frame (xW W
EW , yEW ) is included in the measurement model of the target. To be able
to estimate the ego vehicles position the single track ego vehicle state variable
vector and state space model (2.4) has to be augmented with the ego vehicle’s
position. Note that the ego vehicle’s heading angle is denoted ψE here to differen-
tiate it from the heading angle of the target.

2.2 Discretizing Continuous-Time Models

The measurements dealt with in this work are sampled and handled as discrete-
time variables in computers and electronic control units (ecu). All sensor signals
are transferred in sampled form from different sensors to the log-computer using
the so called can-Bus (Controller Area Network). Hence, the systems discussed
in this thesis must also be described using discrete-time models according to the
state space model in (2.1). Nevertheless, since physical relations commonly are
given in continuous-time, the various systems presented in this thesis, such as the
single track model in Example 2.1, are derived and represented using continuous-
time state space models in the form (2.2). Thus, all continuous-time models in
this thesis have to be discretized in order to describe the measurements. Only
a few of the motion models can be discretized exactly by solving the sampling
formula
k+T
Z
xk+1 = xk + a(x(t), u(t), w(t), θ(t))dt, (2.19)
k
analytically, where T denotes the sampling time.
2.2 Discretizing Continuous-Time Models 27

2.5 Example: Exact Sampling of the Coordinated Turn Model

Consider the continuous-time coordinate turn model in Example 2.4. The ana-
lytic solution of (2.19) is
T
2vk i
!
W W ψ̇Ti ,k T ψ̇Ti ,k T
xTi W ,k+1 = xTi W ,k + sin cos ψTi ,k + (2.20a)
ψ̇Ti ,k 2 2
T
2vk i
!
ψ̇Ti ,k T ψ̇Ti ,k T
yWTi W ,k+1 = y W
Ti W ,k + sin sin ψ Ti ,k + (2.20b)
ψ̇Ti ,k 2 2
T
i T
vk+1 = vk i (2.20c)
ψTi ,k+1 = ψTi ,k + T ψ̇Ti ,k (2.20d)
ψ̇Ti ,k+1 = ψ̇Ti ,k . (2.20e)

Simpler than using the exact sampling formula (2.19) is it to make use of the
standard forward Euler method, which approximates (2.2a) according to
xk+1 ≈ xk + T a(xk , uk , wk , θk ) , fk (xk , uk , wk , θk ). (2.21)
This is a very rough approximation with many disadvantages, but it is frequently
used due to its simplicity. Example 2.6 shows the Euler approximation of the
coordinated turn model.

2.6 Example: Euler Sampling of the Coordinated Turn Model

Consider the continuous-time coordinate turn model in Example 2.4. The solu-
tion of (2.21) is
T
xW W i
Ti W ,k+1 = xTi W ,k + T vk cos ψTi ,k (2.22a)
T
yW W
Ti W ,k+1 = yTi W ,k + T vk i sin ψTi ,k (2.22b)
Ti T
vk+1 = vk i (2.22c)
ψTi ,k+1 = ψTi ,k + T ψ̇Ti ,k (2.22d)
ψ̇Ti ,k+1 = ψ̇Ti ,k . (2.22e)

Sampling of linear systems is thoroughly described by Rugh (1996). Moreover,

different options to sample and linearize non-linear continuous-time systems are
described by Gustafsson (2000). The linearization problem is treated in Chap-
ter 3, in a discussion of approximative model based filters such as the extended
Kalman filter.
28 2 Models of Dynamic Systems

2.3 Linear State Space Model

An important special case of the general state space model (2.1) is the linear Gaus-
sian state space model, where f and h are linear functions and the noise is Gaus-
sian,
xk+1 = Fk (θ)xk + Gku (θ)uk + Gkw wk (θ), (2.23a)
yk = Hk (θ)xk + Hku (θ)uk + ek (θ), (2.23b)
where wk ∼ N (0, Qk ) and ek ∼ N (0, Rk ). Note that the single track model (2.4) is
linear in the state variables, as shown in Example 2.7.

2.7 Example: Linearized Single Track Model

The front wheel angle is usually quite small at higher velocities and the assump-
tions cos δf ≈ 1, tan δf ≈ sin δf ≈ δf therefore applies. The continuous-time
single track model (2.4) may first be discretized using Euler sampling and can
then be written on the linear form (2.23) according to
2 2
 1 − T Cαf lf +Cαr lr
 
−Cαf lf +Cαr lr  C l 
Izz vt T Izz
  αf f 
ẋk+1 = 
  x +  Izz  δ + w ,

(2.24a)
Cαf lf −Cαr lr Cαf +Cαr   k  Cαf  f ,k k
−T − T 1 − T mv 
v2k m k mv
 
1 0
" #
  0
yk =  −Cαf lf +Cαr lr C +C  x + C
αf δf ,k + ek . (2.24b)
mv − αf m αr  k m
k

The model is linear in the input δf ,k . However, the input vk is implicitly modeled
in the matrices Fk (vk , θk ), Gku (vk , θk ) and Hk (vk , θk ).

Linear state space models and linear system theory in general are thoroughly de-
scribed by Rugh (1996) and Kailath (1980).

2.4 Nonlinear State Space Model with Additive Noise

A special case of the general state space model (2.1) is given by assuming that the
noise enters additively and the input signals are subsumed in the time-varying
dynamics, which leads to the form
xk+1 = fk (xk , θk ) + wk , (2.25a)
yt = hk (xk , θk ) + ek . (2.25b)

In Example 1.1 an ego vehicle model was introduced, where the steering wheel
angle and the vehicle velocity were modeled as deterministic input signals. This
consideration can be motivated by claiming that the driver controls the vehicle’s
lateral movement with the steering wheel and the longitudinal movement with
the throttle and brake pedals. Furthermore, the steering wheel angle and the
velocity are measured with less noise than the other measurement signals, and
2.4 Nonlinear State Space Model with Additive Noise 29

they are often pre-processed to improve the accuracy and remove bias. With
these arguments the resulting model, given in Example 2.1, may be employed.
The model is in some sense simpler than the case when these two signals are
assumed to be stochastic measurements, as shown in Example 2.8.

2.8 Example: Single Track Model without Deterministic Input Signals

In classical signal processing it is uncommon to allow deterministic input signals,
at least not if these are measured by sensors. The input signals in Example 1.1
should instead be modeled as stochastic measurements. Hence, the measurement
vector and the state vector are augmented and the system is remodeled. One
example is given by the state space model
!
C l 2 +C l 2

 ψ̇E,k + T − αf If v αr r ψ̇E,k + −Cαf lIf +Cαr lr βk + CαfIlf δf ,k 
ψ̇E,k+1  

zz k zz zz 
 β     
Cαf lf −Cαr lr Cαf +Cαr Cαf δf ,k 
 
 k+1  
 =  βk + T − 1 + ψ̇E,k − mv βk + mv  + wk ,

 δf ,k+1   v2k m
 k k

vk+1 
 δf ,k 

vk
(2.26a)
  ψ̇ + eψ̇,k

 ψ̇t   −C l +C lE,k
 
ay,k   αf f αr r ψ̇E,k − Cαf +Cαr βk 

  =  mvk m  + ek , (2.26b)
 δs,k  
   hδf (ψ̇k , βk , δf ,k , θ) 

vk 
v

x,t

where T is the sample time. The first two rows of the process and measurement
models are the discretized versions of the model given in (2.4). In the second row
in the measurement model the simplification cos δf ≈ 1 and sin δf ≈ 0 is made
to not let the measurement model be dependent on the measurement δf . The
third measurement signal is the steering wheel angle δs , but the third state is the
front wheel angle δf . A possible measurement model hδf will be discussed in
Example 3.1. Finally, a random walk is assumed for the front wheel angle δf and
the velocity v in the process model.

Another way to represent the state space model is given by considering the prob-
ability density function (pdf) of different signals or state variables of a system.
The transition density p(xk+1 |xk ) models the dynamics of the system and if the
process noise is assumed additive, the transition model is given by
p(xk+1 |xk ) = pw (xk+1 − f (xk , uk , θk )), (2.27)
where pw denotes the pdf of the process noise w. The density describes the state
at time k + 1 given that information about the state at the previous time step k is
known. A fundamental property of the process model is the Markov property,
p(xk+1 |x1 , . . . , xk ) = p(xk+1 |xk ). (2.28)
This means that the state of the system at time k contains all necessary informa-
tion about the past, which is needed to predict the future behavior of the system.
30 2 Models of Dynamic Systems

Furthermore, if the measurement noise is assumed additive then the likelihood

function, which describes the measurement model, is given by
p(yk |xk ) = pe (yk − h(xk , uk , θk )), (2.29)
where pe denotes the pdf of the sensor noise e. The measurement zk received by
the sensor are described by the likelihood function given that information about
the state xk at the same time step k is known. The two density functions in (2.27)
and (2.29) are often referred to as a hidden Markov model (hmm) according to
xk+1 ∼ p(xk+1 |xk ), (2.30a)
yk ∼ p(yk |xk ), (2.30b)
since xk is not directly visible in yk . It is a statistical model where one Markov pro-
cess, that represents the system, is observed through another stochastic process,
the measurement model.
 Estimation Theory
3
This thesis is concerned with estimation problems, i.e. given measurements y the
aim is to estimate the parameter θ or the state x in (1.1). Both problems rely
on the same theoretical basis and the same algorithms can be used. The parame-
ter estimation problem is a part of the system identification process, which also
includes the derivation of the model structure, discussed in the previous chap-
ter. The state estimation problem utilizes the model and its parameters to solve
for the states. When estimating x it is assumed that θ is known and vice versa.
The parameter is estimated in advance if θ is time invariant or in parallel with
the state estimation problem if θ is assumed to be time varying. Example 3.1
illustrates how the states and parameters may be estimated.
3.1 Example: Parameter and State Estimation
Consider the single track model, which was introduced in Example 1.1 and the
equations were given in Example 2.1. The front wheel angle δf is considered to
be a state variable in Example 2.8 and the steering wheel angle δs is treated as a
measurement. The measurement equation is in its simplest form a constant ratio
given by
δs,k = h(δf ,k , θ) = is δf ,k . (3.1)
The parameter θ = is is assumed to be time invariant. The state δf must be
known in order to identify the parameter θ. Usually the parameter is estimated
off-line in advance using a test rig, where the front wheel angle is measured using
highly accurate external sensors. The parameter is then used within the model in
order to estimate the states on-line while driving.
The tire parameter Cα is assumed to change with weather and road conditions,
hence it is a time varying parameter. It has to be identified on-line at time k
using the state estimates from the previous time step k − 1, which in turn were

31
32 3 Estimation Theory

estimated using the parameter estimate from time step k − 1.

For various reasons some systems are only modeled by a likelihood function. Of-
ten these systems are static and there exists no Markov transition density. How-
ever, most systems in this thesis are modeled using both a prediction and a like-
lihood function. In system identification, the model parameter is estimated with-
out physically describing the parameter’s time dependency, hence static estima-
tion theory is used. The state can be estimated in more or less the same way.
However, the process model (1.1a) is often given and its time transition informa-
tion is exploited to further improve the state estimate.
The origins of the estimation research field can be traced back to the work by
Gauss in 1795 on least squares (Abdulle and Wanner, 2002) and Bayes (1763)
on conditional probabilities. Bayes introduced an important theorem which has
come to be referred to as Bayes’ theorem,
p(y|x, θ)p(x, θ)
p(x, θ|y) = , (3.2)
p(y)
with which it is possible to calculate the posterior probability p(x, θ|y) given a
prior probability p(x, θ) and the likelihood function p(y|x, θ). Note that both
the measurement and the state or parameter are treated as random variables.
Another view of the estimation problem was introduced by Fisher (1922), who
claimed that the probability of an estimate should be seen as a relative frequency
of the state or parameter, given data from long-run experiments. Fisher also
treats the measurement as a random variable. The main difference to Bayes’ ap-
proach is that in Fisher’s approach there is a true state or parameter which is
treated as deterministic, but unknown. To accentuate the different views, the
likelihood is often written using `(x, θ) to emphasize that the likelihood is re-
garded as a function of the state x and the parameter θ.
After this brief historical background, the remainder of this chapter is outlined
as follows. In Section 3.1, static estimation methods based on both Fishers and
Bayes theories, are discussed. These methods can be used for both state and
parameter estimation. In Section 3.2, dynamic estimation methods are discussed.
These methods are, within the scope of this thesis, only used for state estimation
and are based solely on Bayes’ theories.

3.1 Static Estimation Theory

The general estimation problem consists of finding the estimates x̂ and θ̂ that
minimize a given loss function V (x, θ; y). This problem is separated into a pa-
rameter estimation problem and a state estimation problem according to
b = arg min V (θ; x, y),
θ (3.3a)
θ
x = arg min V (x; θ, y).
b (3.3b)
x
3.1 Static Estimation Theory 33

How to separate a typical estimation problem into these two parts is shown Ex-
ample 3.2.
General estimation techniques are covered by most textbooks on this topic, e.g.,
Kay (1993); Kailath et al. (2000); Ljung (1999). There are many estimation meth-
ods available, however, in this section the focus is on the methods used in Part II
of this thesis.
3.2 Example: Parameter and State Estimation
Consider the linear single track model in Example 2.7. Suppose that the state
variables are measured with external and highly accurate sensors. The yaw rate
is measured with an extra imu and the body side slip angle β is measured with
a so called Correvit® sensor, which uses correlation technology to compute the
optical flow. The vehicle in Figure 2 on Page 297 is equipped with this type of
sensor at the front. Now, the parameter θ can be estimated, according to (3.3a).
This approach has been used to find some of the vehicle parameters in Papers B
and G.
Conversely, if θ is known and y is measured, the state variables x can be estimated
using (3.3b).

This section covers estimation problems without any process model f ( · ), where
a set of measurements is related to a parameter only via the measurement model
h( · ). Furthermore, only an important and special case where the measurement
model is linear in x is considered. The linear measurement model was given
in (2.23b) and is repeated here for convenience
yk = Hk (θ)xk + ek . (3.4)

3.1.1 Least Squares Estimator

The least squares (ls) estimate is defined as the solution to the optimization prob-
lem, where the squared errors between the predicted measurements and the ac-
tual measurements are minimized according to,
k
X
x̂LS
k = arg min ||yτ − Hτ (θ)xτ ||22 . (3.5)
x τ=1
The solution for the linear case is given in Algorithm 1.
If the measurement covariance R = Cov (e) is known, or in practice at least as-
sumed to be known, then the weighted least squares (wls) estimate is given by
the optimization problem
k
X
x̂W
k
LS
= arg min (yτ − Hτ (θ)xτ )T R−1
τ (yτ − Hτ (θ)xτ ). (3.6)
x τ=1
The solution for the linear case is given in Algorithm 2, and Example 3.3 illus-
trates how the single track vehicle model can be reformulated to estimate the
34 3 Estimation Theory

Algorithm 1 Least Squares

The least squares estimate and its covariance are given by
 k −1 k
X  X  −1
x̂LS
k = 

 H T
τ H τ


 H T
τ y τ = H T
H HTY , (3.7a)
τ=1 τ=1
Cov (x̂LS ) = (H T H)−1 (H T RH)(H T H)−1 , P LS . (3.7b)
The last equality is the batch solution, where H and Y are augmented mea-
surement models and measurement vectors, respectively. Furthermore, the
measurement noises Rτ = Cov (eτ ) form the main diagonal of R according to
R = diag(R1 , . . . , Rk ).

Algorithm 2 Weighted Least Squares

The weighted least squares estimator and its covariance matrix are given by
 t −1 k
X  X  −1
W LS T −1
x̂k =  Hτ Rτ Hτ  HτT R−1 T −1
τ yτ = H R H H T R−1 Y , (3.8a)
τ=1 τ=1
W LS T −1 −1 W LS
Cov (x̂ ) = (H R H) , P , (3.8b)
where the weighting matrix is the noise covariance R.

parameters using the wls.

Another example, where both the ls and the wls estimators are applied, is given
in Paper C. The left and right borders of a road are modeled by polynomials and
the coefficients are the parameters which are estimated given a batch of measure-
ments from a radar.

3.3 Example: Parameter and State Estimation

Consider the linear single track model in Example 2.7 and the separation of the
parameter and the state estimation problems in Example 3.2. Suppose that the
vehicle’s mass m and the dimensions lf and lr are known. Furthermore, suppose
that the state variable x may be measured as described in Example 3.2. Consider
the measurement equation (2.24b); the parameter estimation problem can now
be formulated in the form (3.4) with
" #
C
y = H(x, u, lf , lr , m) αf + e, (3.9)
Cαr
and the parameters Cαf , Cαr can be solved for using e.g., wls in (3.6). Further-
more, the inverse of the moment of inertia 1/Izz may be estimated off-line by
writing the process model (2.24a) in the form (3.4) according to
1
xt+1 = H(xt , u, lv , lf , m, Cαf , Cαr ) · + w. (3.10)
Izz
3.2 Filter Theory 35

3.1.2 Probabilistic Point Estimates

The maximum likelihood (ml) estimate, first introduced by Fisher (1912, 1922),
is defined by
x̂ML
k = arg max p(y1:k |xk ). (3.11)
xk

Put into words, the estimate is chosen to be the parameter most likely to produce
the obtained measurements.

The posterior p(xk |y1:k ), when xk is random, contains all known information
about the state of the target at time k. The maximum a posteriori (map) esti-
mator is defined by
x̂MAP
k = arg max p(xk |y1:k ) = arg max p(y1:k |xk )p(xk ), (3.12)
xk xk

or put in words, find the most likely estimate of the parameter given the measure-
ments y1:k . Bayes’ theorem (3.2) and the fact that the maximization is performed
over xk is used in the second equality of (3.12). The ml estimate is for instance
used in Paper E.

3.2 Filter Theory

The topic of this section is recursive state estimation based on dynamic models.
The iteration process of the state space estimation was briefly described in words
in Section 1.5. The state estimation theory is influenced by the Bayesian view,
which implies that the solution to the estimation problem is provided by the
filtering pdf p(xk |y1:k ). The introduction to this section will be rather general
using the model defined in (2.30). Bayes’ theorem was introduced in (3.2) and is
used to derive the recursive Bayes filter equations
Z
p(xk+1 |y1:k ) = p(xk+1 |xk )p(xk |y1:k )dxk , (3.13a)

p(yk |xk )p(xk |y1:k−1 )

p(xk |y1:k ) = , (3.13b)
p(yk |y1:k−1 )
with the denominator
Z
p(yk |y1:k−1 ) = p(yk |xk )p(xk |y1:k−1 )dxk . (3.13c)

These equations describe the time evolution

· · · → xk|k → xk+1|k → xk+1|k+1 → · · · (3.14)
of the random state vector x. The Bayes posterior density function p(xk |y1:k ) con-
ditioned on the time sequence y1:k = {y1 , . . . , yk } of measurements accumulated
at time k is the probability density function of xk given measurements y1:k . The
probability density function p(xk+1 |y1:k ) is the time prediction of the posterior
p(xk |y1:k ) to the time step of the next measurement yk+1 . Note that the Bayes
36 3 Estimation Theory

normalization factor given by (3.13c) is independent of x. In practice the numer-

ator of (3.13b) is calculated and then simply normalized, since the integral of the
posterior density function must be equal to one. The single target Bayes filter
recursion is illustrated with a flow chart as follows
prediction update
· · · −→ pk|k (xk |z1:k ) −→ pk+1|k (xk+1 |z1:k ) −→ pk+1|k+1 (xk+1 |z1:k+1 ) −→ · · ·

↑ ↑

Transition density Likelihood function

p(xk+1 |xk ) p(zk+1 |xk+1 )
↑ ↑
Motion model Measurement model
xk+1 = f (xk ) + wk zk+1 = h(xk+1 ) + ek+1
After the update step is made the algorithm continues recursively, i.e., a new
prediction is performed and thereafter a update or correction with the likelihood
of the new measurements.

If p(yk |xk ), p(xk+1 |xk ) and p(xk ) are Gaussian and their corresponding process
and senor models are linear, as in (2.23), then (3.13a) and (3.13b) reduce to the
Kalman filter prediction and measurement update, respectively. The Kalman
filter is treated in Section 3.2.1. In contrast, if p(yk |xk ), p(xk+1 |xk ) and p(xk )
can be approximated by a Gaussian and their corresponding process and sensor
models are nonlinear (2.1), several approximations of (3.13a) and (3.13b) exist.
The two most common filters are the extended Kalman Filter and the unscented
Kalman filter, which are outlined in Sections 3.2.2 and 3.2.3, respectively. Other
methods, including methods that approximate other density functions than Gaus-
sian, are neatly covered by Hendeby (2008) and Schön (2006). The most pop-
ular approaches are the particle filter, which is covered in Section 3.2.4, and
the marginalized particle filter, see e.g., Arulampalam et al. (2002); Cappe et al.
(2007); Djuric et al. (2003); Karlsson (2005); Schön et al. (2005).

3.2.1 The Kalman Filter

The linear state space representation subject to Gaussian noise, which was given
in (2.23), is the simplest special case when it comes to state estimation. The model
is repeated here for convenience;
xk+1 = Fk (θ)xk + Gku (θ)uk + Gkw wk , wk ∼ N (0, Qk ), (3.15a)
yk = Hk (θ)xk + Hku (θ)uk + ek , ek ∼ N (0, Rk ). (3.15b)
The linear model (3.15) has two important properties. All density functions in-
volved in the model and state estimation are Gaussian and a Gaussian density
function is completely parametrized by the mean and the covariance, i.e. the first
and second order moment. Hence, the Bayesian recursion (3.13) is simplified
into only propagating the mean and covariance of the involved probability den-
3.2 Filter Theory 37

Algorithm 3 Kalman Filter

Consider the linear state space model (3.15). The Kalman filter is given by the
two following steps.
Prediction
u
x̂k|k−1 = Fk−1 x̂k−1|k−1 + Gk−1 uk−1 (3.16a)
T w wT
Pk|k−1 = Fk−1 Pk−1|k−1 Fk−1 + Gk−1 Qk−1 Gk−1 (3.16b)
Measurement Update
Kk = Pk|k−1 HkT (Hk Pk|k−1 HkT + Rk )−1 (3.17a)
u
x̂k|k = x̂k|k−1 + Kk (yk − Hk x̂k|k−1 − Hk uk ) (3.17b)
Pk|k = (I − Kk Hk )Pk|k−1 (3.17c)

sity functions, as illustrated in the following flow chart

prediction update
· · · −→ N (xk|k , Pk|k ) −→ N (xk+1|k , Pk+1|k ) −→ N (xk+1|k+1 , Pk+1|k+1 ) −→ · · ·

↑ ↑

Transition density Likelihood function

N (xk+1 ; Fxk , Qk ) N (zk+1 ; Hxk+1 , Rk+1 )
↑ ↑
Motion model Measurement model
xk+1 = Fxk + wk zk+1 = Hxk+1 + ek+1
The most well known estimation algorithm is the Kalman Filter (kf), derived
by Kalman (1960), and shown in Algorithm 3. Example 3.4 shows how the single
track vehicle model, introduced in Example 1.1, may be rewritten to be used with
the Kalman filter, which in turn is used to estimate the states.

3.4 Example: Linearized Single Track Model

The single track vehicle model was introduced in Example 1.1 and the model
equations were posed in Example 2.1. The process model (2.4a) and the measure-
ment model (2.4b) are linear in the state variables and can be written in the form
" # " #
ψ̇k+1 ψ̇
= Fk (vx , θ) k + Gku (vx , θ)δf + w, w ∼ N (0, Q), (3.18a)
βk+1 βk
" m# " #
ψ̇k ψ̇
= Hk (vx , θ) k + Hku (θ)δf + e, e ∼ N (0, R), (3.18b)
ay,k βk
as shown in Example 2.7. Since the input vx is present in Fk , Gku and Hk , these
matrices must be recalculated at each time step before being used in the Kalman
filter (Algorithm 3) to estimate the states.
38 3 Estimation Theory

3.2.2 The Extended Kalman Filter

In general, most complex automotive systems tend to be nonlinear. When it

comes to solving state estimation problems in a sensor fusion framework, non-
linear models are commonly applied. This holds also for the work presented in
this thesis, but the problems are restricted by the assumption that the process and
measurement noise are Gaussian. The most common representation of nonlinear
systems is the state space model given in (1.1), repeated here for convenience;
xk+1 = fk (xk , uk , wk , θ), wk ∼ N (0, Qk ), (3.19a)
yk = hk (xk , uk , ek , θ), ek ∼ N (0, Rk ). (3.19b)
The basic idea underlying the extended Kalman filter (ekf) is to approximate the
nonlinear model (3.19) by a local linear model and apply the Kalman filter to this
approximation. This local linear approximation is obtained by computing a first
order Taylor expansion around the current estimate. The result is the extended
Kalman filter, which is given in Algorithm 4. Early practical applications and
examples of the ekf are described in the works by Smith et al. (1962); Schmidt
(1966). An early reference where the ekf is treated is Jazwinski (1970), other
standard references are Anderson and Moore (1979); Kailath et al. (2000) .

The linearization used in the ekf assumes that all second and higher order terms
in the Taylor expansion are negligible. This is certainly true for many systems,
but for some systems this assumption can significantly degrade the estimation
performance. Higher order ekf are discussed by Roth and Gustafsson (2011); Bar-
Shalom and Fortmann (1988); Gustafsson (2000). This problem will be revisited
in the next section.

Algorithm 4 Extended Kalman Filter

Consider the state space model (3.19). The extended Kalman filter is given by the
two following steps.
Prediction
x̂k|k−1 = fk−1 (x̂k−1|k−1 , uk−1 , 0, θ) (3.20a)
T T
Pk|k−1 = Fk−1 Pk−1|k−1 Fk−1 + Gk−1 Qk−1 Gk−1 (3.20b)
where 
∂fk (x̂k|k , uk , wk , θ) 

∂fk (xk , uk , 0, θ) 
Fk =  Gk =  (3.20c)
∂xk xk =x̂k|k ∂wt 
wk =0
Measurement Update
Kk = Pk|k−1 HkT (Hk Pk|k−1 HkT + Rk )−1 (3.21a)
x̂k|k = x̂k|k−1 + Kk (yk − hk (x̂k|k−1 , ut , 0, θ)) (3.21b)
Pk|k = (I − Kk Hk )Pk|k−1 (3.21c)
where 
∂hk (xk , uk , 0, θ) 
Hk =  (3.21d)
∂xk xk =x̂k|k−1
3.2 Filter Theory 39

3.2.3 The Unscented Kalman Filter

The ekf is sufficient for many applications. However, to use an ekf the gradi-
ents of fk ( · ) and hk ( · ) must be calculated, which in some cases is either hard
to do analytically or computationally expensive to do numerically. An alterna-
tive approach, called the unscented Kalman filter (ukf) was proposed by Julier
et al. (1995); Julier and Uhlmann (1997) and further refined by e.g., Julier and
Uhlmann (2002, 2004); Julier (2002). Instead of linearizing fk ( · ) and hk ( · ), the
unscented transform (ut) is used to approximate the moments of the predicted
and updated states. Thereby the ukf to some extent also considers the second
order terms of the models, which is not done by the ekf.

The principle of the unscented transform is to carefully and deterministically

select a set of points, called sigma points, of the initial stochastic variable x, such
that their mean and covariance are equal to those of x. Then the sigma points are
passed through the non-linear function and based on the output the resulting
mean and covariance are derived. In case the process noise and measurement
noise are not additive, sigma points are selected from an augmented state space,
which includes the state x, the process noise w and the measurement noise e in
one augmented state vector

 x̂k|k 

x̂ak|k =  E (wk )  , (3.22)
 
E (ek+1 )
 

with dimension na = nx + nw + ne and the corresponding covariance matrix

Pk|k 0 0 
 
a  0
Pk|k =  Qk 0  . (3.23)
0 0 Rk+1
 

If the noise is additive, then the noise covariances can be added directly to the
estimated covariances of the non-augmented sigma points.

There exist many possibilities to choose the sigma points, a thorough discussion
about different alternatives is presented by Julier and Uhlmann (2004). In the
present work only the standard form is reproduced. The basic principle is to
choose one sigma point in the mean of xa and 2na points symmetrically on a
given contour, described by the state covariance P a . The sigma points χ i and the
associated weights w(i) are chosen as
χ(0) = x̂a w(0) = w(0) (3.24a)
1 − w(0)
r !
na
χ(i) = χ(0) + Pa w(i) = (3.24b)
1 − w(0) i 2na
1 − w(0)
r !
(i+na ) (0) na
χ =χ − Pa w(i+na ) = (3.24c)
1 − w(0) i 2na
√
for i = 1, . . . , na , where ( A)i is the i th column of any matrix B, such that A = BBT .
The augmented state vector makes it possible to propagate and estimate nonlin-
40 3 Estimation Theory

Algorithm 5 Unscented Kalman Filter

Consider the state space model (3.19). The unscented Kalman filter is given by
the following steps, which are iterated in the filter.
Choose sigma points according to (3.24)
Prediction
2na
x,(i)
X
x̂k|k−1 = w(i) χk|k−1 (3.25a)
i=0
2na   T
x,(i) x,(i)
X
Pk|k−1 = w(i) χk|k−1 − x̂k|k−1 χk|k−1 − x̂k|k−1 (3.25b)
i=0
where  
x,(i) x,(i) w,(i)
χk|k−1 = fk−1 χk−1|k−1 , uk−1 , χk−1|k−1 , θ (3.25c)
Measurement Update
−1
x̂k|k = x̂k|k−1 + Pxy Pyy (yk − ŷk|k−1 ) (3.26a)
−1 T
Pk|k = Pk|k−1 − Pxy Pyy Pxy (3.26b)
where  
(i) x,(i) e,(i)
yk|k−1 = hk χk|k−1 , uk , χk|k−1 , θ (3.26c)
2na
(i)
X
ŷk|k−1 = w(i) yk|k−1 (3.26d)
i=0
2n
Xa   T
(i) (i)
Pyy = w(i) yk|k−1 − ŷk|k−1 yk|k−1 − ŷk|k−1 (3.26e)
i=0
2na   T
x,(i) (i)
X
Pxy = w(i) χk|k−1 − x̂k|k−1 yk|k−1 − ŷk|k−1 (3.26f)
i=0

ear influences that the process noise and the measurement noise have on the state
vector and the measurement vector, respectively. The weight on the mean w(0) is
used for tuning and according to Julier and Uhlmann (2004) preferable proper-
ties for Gaussian density functions are obtained by choosing w(0) = 1 − n3a . After
the sigma points have been acquired, the augmented state vector can be parti-
tioned according to
 x 
 χk|k 
a
χk|k =  χkw  . (3.24d)
 
 e 
χk+1
The rest of the ukf is summarized in Algorithm 5.

An advantage of the ukf, compared to the ekf, is that the second order bias cor-
rection term is implicitly incorporated in the mean estimate. Example 3.5 shows
an important problem where the second order term should not be neglected.
3.2 Filter Theory 41

3.5 Example: Tracked Radar Object

The radar target tracking problem was introduced in Example 1.2 and the model
was defined in Example 2.4. The sensor model converts the Cartesian state vari-
ables to polar measurements. This is one of the most important and commonly
used transformations for sensors measuring range and azimuth angle. Usually
the azimuth angle error of these type of sensors is significantly larger than the
range error. This also holds for the sensors used in this thesis.

Let the sensor be located at the origin and the target at (x, y) = (0, 1) in this simple,
and commonly used example (Julier and Uhlmann, 2004). Measurements may be
simulated by adding Gaussian noise to the actual polar value (r, ψ) = (1, π/2)
of the target localization. A plot of several hundred state estimates, produced
in a Monte Carlo simulation, forms a banana shaped arc around the true value
(x, y) = (0, 1), as shown in Figure 3.1. The azimuth error causes this band of
Cartesian points to be stretched around the circumference of a circle, with the
result that the mean of these points lies somewhat closer to the origin than the
point (0, 1). In the figure it is clearly shown that that the ut estimate (×) lies
close to the mean of the measurements (◦). Furthermore, it is shown that the
linearized state estimate (+) produced by the ekf is biased and the variance in
the y component is underestimated.

As a result of the linearization in the ekf, the second order terms are neglected,
which produces a bias error in the mean as shown in Example 3.5. In Julier and
Uhlmann (2004) it is shown how the ut in some cases calculates the projected
mean and covariance correctly to the second order terms.

The unscented Kalman filter is applied in Papers C and D.

3.2.4 The Particle Filter

Numerical approximations of distribution function used in the Bayes equations
(3.13) are necessary since analytical solutions to the filtering problems do not
exist in most cases. A stochastic method called sequential Monte Carlo (smc)
or particle filter (pf), which is based on Monte Carlo integration, has become
popular during the last years. It was introduced by Gordon et al. (1993) and
theory is well described by e.g., Doucet et al. (2001, 2000); Ristic et al. (2004);
Gustafsson (2010). The idea is to approximate the pdf with a number of samples,
(i) (i)
called particles, {xk }N N
i=1 with associated weights {wk|k−1 }i=1 such that
N
(i) (i)
X
p(xk |y1:k−1 ) ≈ wk|k−1 δ(xk − xk ). (3.27)
i=1
The particles are selected to be identically and independently distributed sam-
(i)
ples from a so called proposal distribution q(xk+1 |xk , y1:k+1 ) and the weights are
called importance weights. Using Monte Carlo integration the mean value of the
42 3 Estimation Theory

0.8

0.6
y

0.4

0.2

0 Sensor

−0.2
−0.6 −0.4 −0.2 0 0.2 0.4 0.6
x
(a)

1.02

0.98

0.96
y

0.94

0.92

0.9
−0.3 −0.2 −0.1 0 0.1 0.2 0.3
x
(b)

Figure 3.1: A Monte Carlo simulation of the problem in Example 3.5 is

shown in Figure (a). The sensor, for example a radar, is in the position (0, 0)
and the true position of the target is in the position (0, 1). The mean of the
measurements is at ◦ and the uncertainty ellipse is solid. The linearized
mean is at + and its ellipse is dashed. The UT mean is at × and its uncer-
tainty ellipse is dotted. Figure (b) is a zoom. Note that the scaling in the x
and the y axis are different.
3.2 Filter Theory 43

Algorithm 6 Particle Filter

(i) (i)1
1: Initiate {x0 }N N
i=1 ∼ px0 and {w0 }i=1 = N for i = 1, . . . , N
2: k=0
3: loop
4: for i = 1 to N do
(i) (i)
5: Time Update: generate the sample, xk|k−1 ∼ q(xk |xk−1|k−1 , yk )
6: Update importance weights
(i) (i)
(i) (i) p(xk|k−1 |xk−1|k−1 )
wk|k−1 = wt−1|t−1 (i) (i)
q(xk|k−1 |xk−1|k−1 , yk )
(i) (i)
{Note, if q(xk |xk−1|k−1 , y1:k ) = p(xk |xk−1|k−1 ) this simplifies to
(i) (i)
wk|k−1 = wt−1|t−1 }
(i) (i) (i)
7: Measurement update: w̄k|k = p(yk |xk|k )wk|k−1
8: end for
(i)
(i) w̄k|k
9: Normalize weights wk|k = P (j) for i = 1, . . . , N
j w̄k|k
10: Resample
11: end loop

distribution can easily be calculated according to

N
(i) (i)
X
x̂k|k−1 = E(xk ) ≈ wk|k−1 xk . (3.28)
i=1
The pf is described in Algorithm 6.
The resampling in Line 10 rejuvenates the particles used to represent the pdf.
There exist different methods to resample, and it will not be discussed further
here; the reader is referred to Hendeby (2008); Schön (2006) instead.
The pf is used in Paper C to track extended targets. In Paper G, the pf is used
since the mean and covariance of the process noise are unknown and must be
estimated together with the states. In that case the particles are drawn from a
Student-t distribution.
 Target Tracking
4
The process of estimating over time the location and characteristics of one or
more objects of interest, denoted targets, using one or several sensors is referred
to as target tracking. The aim of the tracking algorithm is to detect the true tar-
gets, collect observations that originate from them and to estimate quantities of
interest, such as target position, velocity and other object characteristics. Typical
sensors for these applications, such as radar, laser and vision, report noisy mea-
surements. Besides the fact that the measurements are noisy another difficulty
for the algorithm is that the sensor measurements not only originate from the
targets, but also from clutter or spurious detections. The target tracking area is
well covered in the literature and recommended books are Blackman and Popoli
(1999); Bar-Shalom and Fortmann (1988); Bar-Shalom et al. (2001); Ristic et al.
(2004); Liggins et al. (2009).

This chapter begins by putting the filter, introduced in the last chapter about
estimation theory, into the target tracking framework. In Section 4.1 the focus
is on the filter and therefore only the single target case is considered. An au-
tomotive target tracking system, must obviously be able to handle multiple tar-
gets, because potentially more than one vehicle are surrounding the own vehicle.
Therefore the chapter is continued by discussing the extension to multi target
tracking in Section 4.2. When reading the first two sections, compare with the
block diagram of the target tracking framework illustrated in the introduction in
Figure 1.4. Finally, in Section 4.3 the system is further enlarged to also be able to
track the shape and size of the the targets. This research area is called extended
target tracking.

45
46 4 Target Tracking

observation space

zk+1
zk

state space

target motion
xk+1
xk

Figure 4.1: Single target tracking problem. The upper layer is the observa-
tion space with the measurements and the lower layer is the state space with
the state variables.

4.1 Single Target Tracking

The complete information required to describe the system at time k is summa-

rized in the state vector xk ∈ Rnx . At each time step the tracking algorithm is
supplied with a measurement vector z ∈ Rnz . The aim of the filter, as part of a
target tracking algorithm, is to find the posterior density p(xk |z1:k ), given a se-
quence of noisy measurement vectors up to and including time k, collected in
z1:k = {z1 , z2 , . . . zk }. (4.1)
From the posterior it is possible to calculate an estimate x̂k of the state xk . Typi-
cally, the targets or the sensors are moving and the system changes over time. The
motion of the targets in time is modeled with a Markov density, given in (2.27),
and the pdf is specified with a motion model (2.1a). The measurement zk re-
ceived by the sensor is modeled by a likelihood function (2.29), which is speci-
fied by a measurement model (2.1b). Both the process and measurement noise
variables are assumed to be white and independent. Figure 4.1 illustrates the
filtering problem. The upper layer represents the observation space, where one
measurement zk is observed at each time step k and another measurement is ob-
served at time step k + 1. The lower layer represents the state space, with the
values of the state vector xk at each time step.

The single target Bayes filter recursion consists of two steps, the prediction step
and the correction step. Since all variables are assumed to be unknown stochastic
variables in the Bayesian framework it is necessary to describe a priori informa-
tion about the state in the form of a prior density p0 (x0 ). The recursion was
illustrated with a flow chart in Section 3.2. A special case is the Kalman filter,
where the densities are assumed to be Gaussian.
4.2 Extension to Multitarget Tracking 47

4.2 Extension to Multitarget Tracking

In this chapter until now only single targets have been considered. This scenario
is quite unusual, in most target tracking applications more than one target may
appear and must be handled by the target tracking system. A multi target track-
ing system must not only provide estimates of the state variables
 
(1) (2) (N )
xk , xk , . . . , xk x (4.2)

it must also estimate the number of targets Nx , and find likely associations of the
measurements to the tracks. The latter uncertainty is known as the data associa-
tion problem.
A typical multi target system is shown in Figure 4.2, where several targets exist
at each time step. Aspects to consider in a multitarget tracking filter are listed
below:
• The number of targets Nx changes over time, for instance there are probably
more vehicles on the roads during rush hour than in the night.
• New targets appear and old targets disappear, as they enter or leave the
field of view of the sensor. Compare with Figure 4.2, where target x(4) ap-
pears at time k + 1.
• The system fails to detect targets because they are temporarily occluded or
because the sensor data is very noisy.
• The sensors receive a set of spurious measurements, also denoted clutter,
stemming from false reflections or from other type of objects in the field
of view, etc. In Figure 4.2 there are more observations than targets at each
time step.
In this section first the data association problem is treated in Section 4.2.1 and
thereafter an overview of the track management, which takes care of the prob-
lems in the list above is given in Section 4.2.2.

4.2.1 Data Association

This section would not be needed if only the state variables of the ego vehicle,
introduced in Example 1.1 are estimated, because in that case it is obvious how
the measurements are associated with the state variables. In the object tracking
problem, introduced in Example 1.2, it is no longer obvious which measurement
should update which track. There are many methods available for finding likely
measurement-to-track associations, i.e., for solving the data association problem,
see e.g., Bar-Shalom and Fortmann (1988); Blackman and Popoli (1999). However,
the task is seldom easy, due to noisy measurements, multiple reflections on each
target and erroneous detections caused by spurious reflections.
The first step in the data association process is called gating. Gates are con-
(i)
structed around the predicted measurement ẑk|k−1 of each track i to eliminate
48 4 Target Tracking

(3) (3) observation space

zk zk+1
(4) (4)
zk (2) zk+1
zk+1
(2) (1)
zk zk+1 (6)
zk+1
(1) (5)
zk zk+1 state space
target motion (2)
xk+1 (3)
xk+1
(2)
xk (3)
xk
(1)
(1)
xk xk+1 (4)
xk+1

Figure 4.2: Multi target tracking system, several state vectors are present at
each time step and producing more than one observation. Note that there
are more observations than targets. A new target x(4) appears at time k + 1.

unlikely pairings and thereby to limit the number of measurement-to-track asso-

ciations. This reduces the number of measurements that are examined by the data
association algorithm and reduces the computational load. The residual, which is
(j)
also called innovation, between a measurement zk and a predicted measurement
(i)
ẑk|k−1 is
(i,j) (j) (i)
z̃k|k−1 = zk − ẑk|k−1 , (4.3)
and it is assumed Gaussian distributed according to
(i,j) (i)
z̃k|k−1 ∼ N (0, Sk ), (4.4)
(i)
where Sk is the innovation covariance. Further, under this assumption, the statis-
tical distance between the measurement and the predicted measurement, given
by the norm of the residuals according to
2 (i,j) −1 (i,j)
di,j = (z̃k|k−1 )T Si,k (z̃k|k−1 ), (4.5)

is a χn2y random variable. The elliptical gate Gi is defined as the region

  
(i,j) −1 (i,j)
Gi , z(z̃k|k−1 )T Si,k (z̃k|k−1 ) ≤ γG , (4.6)

where γG is the gating threshold or gate size. Given a certain probability that a
true measurement produced by a target i will fall inside its gate, and that the
2
assumption di,j ∼ χn2y holds, a suitable gate threshold can be determined. The
(j) (i)
measurements zk ∈ Gi are considered as candidates for updating the track xk in
the data association algorithm. In other words the gating is a hard decision about
which measurements are feasible measurements for a target.

Now, different conflicts occur. There are several measurements falling within the
4.2 Extension to Multitarget Tracking 49

same gate and there are also measurements falling within more than one gate.
There exist many techniques to solve these conflicts, which are considered to be
the main part of the data association process. The simplest association algorithm
is called nearest neighbor (nn). This approach searches for a unique pairing,
(i) (j)
i.e., one track xk is only updated by at most one observation zk . There are
some possibilities to decide which measurement actually is the nearest. Common
(i,j)
approaches are to choose the measurement with the smallest error z̃k|k−1 or the
(i,j)
smallest statistical distance d 2 (z̃k|k−1 ), defined in (4.5), which is also known as the
Mahalanobis distance, see e.g., Bar-Shalom et al. (2001). The association distance
for each individual track is locally minimized separately with the nn approach,
which could lead to that two tracks are associated with the same measurement.
To avoid this problem it would be more beneficial to find the global minimum
distance considering all tracks simultaneously, and restrict that a measurement
can only be associated with one track. This approach is referred to as global
nearest neighbor (gnn), and its aim is to solve
Nk
X
2
min = di,λ i
(4.7)
λ
i

where λi ∈ {0, 1, . . . , N } indicates which measurement has been assigned to track

i. Note that the value λi = 0 means that track i has not been associated with any
measurement. The so called auction algorithm is the most well known method
to solve this problem, see Bertsekas (1990). Example 4.1 illustrated gating and a
comparison between nn and gnn.

4.1 Example: Gating and Nearest Neighbor Data Association

Consider the target tracking example introduced in Example 1.2. Assume that
(1)
two vehicles are currently tracked. Their predicted measurements ẑk|k−1 and
(2) (j)
ẑk|k−1 are shown in blue and green in Figure 4.3. Six measurements zk , j =
1, . . . , 6 are collected at time k and shown as red stars in the figure. The mea-
surements 2, 4 and 6 fall outside the gates, which are illustrated with ellipses.
Measurement 1 and 3 may be obtained from target 1 since they are both in its
gate. However, measurement 3 is also in the gate of target 2. The nn data asso-
ciation considers each track individually, and since measurement 3 is the closes
measurements of both targets it is used to update both targets as shown in Fig-
ure 4.3a. The gnn data association avoids this problem and considers all tracks
simultaneously. The result is shown in Figure 4.3b, where target 2 is associated
with measurement 5 instated.

There exist a further group of association methods, which uses all measurements
that fall inside the gate. In these methods each measurement j is weighted in
accordance with the probability that it originates from track i. The two most
well known algorithms are probabilistic data association (pda) (Bar-Shalom and
Tse, 1975) and joint probabilistic data association (jpda) (Fortmann et al., 1983),
50 4 Target Tracking

(4) (4)
zk (6)
zk (6)
zk zk

(3) (3)
zk
(1) (1 ) zk (1)
zk (1 ) zk
ẑk |k −1 (2)
ẑk |k −1 (2)
ẑk |k −1 ẑk |k −1

(5) (5)
(2)
zk (2)
zk
zk zk

(a) nn (b) gnn

Figure 4.3: An example with two targets (blue and green) and six measure-
ments (in red). The ellipses are the gates and the two figures show the differ-
ence between the association methods nn and gnn.

where the difference lies in that pda considers each track separately, whereas
jpda forms global hypotheses to calculate the probabilities. Note that the number
of targets is assumed fixed in the above mentioned association methods. These
methods are not further discussed in this thesis, hence a detailed description is
omitted at this place.

4.2.2 Track Management

The main task of the track management is to decide on how many true targets are
observed. Only tracks of sufficient quality should be considered as valid tracks
and used in the data association algorithms described above. According to their
different life stages, tracks can be classified into three cases.
Tentative track: A track that is in the track initiation process. It is not sure that
there is sufficient evidence that it is actually a target or not. A tentative
track is started as soon as it is likely that a measurement originates from a
new target. The tentative tracks can be associated to further measurements.
Confirmed track: A track that was decided to belong to a valid target in the
surveillance area. If several sequential arriving observations indicate that
the tentative track resembles a true target rather than noise or clutter it is
decided to be a confirmed track.
Deleted track: At the other end of the initiation process, this is a track that is
decided to come from all random false alarm. It can also be a track that is
no longer visible to any sensor when the uncertainty of the target increases
above a given threshold. All of its information should be deleted.
When starting a new track some initial guess about the properties of the track
must be made. Put into a Bayesian wording, a prior density p(x0 ), that contains
the information of the state before any observation is made, must be designed.
This can sometimes be a simple task, for instance if you expect targets to ap-
4.3 Extended Target Tracking 51

pear from a certain place, but in most cases it is not easy to know beforehand.
A practical and straightforward method is to use the first measurement as the
mean value of the prior distribution. Other statistics of the prior distribution
are still considered as design variables. A better approach may be considered in
the Kalman filter, where the prior distribution is Gaussian and may be chosen as
x0 ∼ N (0, P0 ). If P0 is chosen very large in comparison with the measurement un-
certainty, the first updated state estimate will largely be based on the associated
measurement.
In order to know if the first measurement stems from a true target or from clut-
ter, the track management system must observe the tentative track for a number
of samples. There exist different methods to decide if a tentative track shall be
transformed into a confirmed track. One of the simplest and most well known
is the so called M/N logic, where a track is confirmed if M measurements out of
N possible are associated with the track. Other common methods are based on
scoring measurements, with for example sequential probability ratio test (sprt),
which was first proposed by Wald (1945). The method is based on comparing
the ratio between the likelihood of the hypothesis that a track describes a true
target and the likelihood of the hypothesis that it is a false alarm. Furthermore,
there exist combined validation and association methods. Integrated probabilis-
tic data association (ipda) expresses both the probability of target existence and
data association based on pda.
The track deletion methods are similar to the validation methods, both the M/N
logic and the score based approaches, mentioned above, can be applied. More
information and details about track management can be found in various books
about target tracking, see e.g., Blackman and Popoli (1999).

4.3 Extended Target Tracking

In classical target tracking problems the objects are modeled as point sources and
it is assumed that only one measurement is received from each target at each time
step. In automotive applications, the targets are at a close distance and of such
a large size that individual features can be resolved by the sensor. A target is
denoted extended whenever the target extent is larger than the sensor resolution,
and it is large enough to occupy multiple resolution cells of the sensor. Put in
other words, if a target should be classified as extended does not only depend on
its physical size, but rather on the physical size relative to the sensor resolution.
The relevant target characteristics that are to be estimated form the target’s state
vector x. Generally, beside the kinematic variables as position, velocity and ori-
entation, the state vector may also contain information about the target’s spatial
extension. However, when the target’s state does not contain any variables re-
lated to the target extent, though the estimation is done as if the target was a
point, the algorithms should still take care of the multiple measurements that
originate from a target. In Paper E a generalized definition of an extended tar-
get is presented, and it is repeated below. This definition does not depend on
52 4 Target Tracking

whether the target extent is estimated or not.

4.2 Definition (Extended Target). A target which potentially gives rise to more
than one measurement per time step irrespective of whether the target’s extent is
explicitly modeled (and/or estimated) or not is referred to as an extended target.

The methods used to track extended target are very similar to the ones used for
tracking a group of targets moving in formation. Extended target tracking and
group tracking are thoroughly described in e.g., Ristic et al. (2004). The bibli-
ography of Waxman and Drummond (2004) provides a comprehensive overview
of existing literature in the area of group and cluster tracking. There exist some
different approaches to represent, i.e., to model, the extended targets, of which
five methods are described in this section.

4.3.1 Point Features

The first and most traditional method is to model the target as a set of point fea-
tures in a target reference frame, each of which may contribute at most one sensor
measurement. The exact location of a feature in the target reference frame is of-
ten assumed uncertain. However, if the appearance of the target is known and
especially if typical radar reflection points are known, then the location of the
features in the target reference frame can be assumed known. The motion of an
extended target is modeled through the process model in terms of the translation
and rotation of the target reference frame relative to a world coordinate frame,
see e.g., Dezert (1998).
For an application in two dimensions, the point features are defined as
n oNp h iT
PT = p(i) with p(i) = x(i) y(i) (4.8)
i=1
and these are usually expressed in a target fixed coordinate frame T . The position
h iT
dTWW = xW TW yW
TW of the target’s origin and the orientation ψT of the target’s
frame is tracked relative to the world coordinate frame. The state vector may be
defined as
h iT
x = dTWW ψT PT . (4.9)
The point features in the target’s coordinate frame can be mapped into a point in
the world frame through the transform
p(i),W = RW T p(i),T + dTWW . (4.10)
The equation above constitutes the measurement model if the point measure-
ments are expressed in the world coordinate frame W. The rotation matrix is
given by
" #
WT cos ψT − sin ψT
R = . (4.11)
sin ψT cos ψT

The uncertainty about the exact position of the point feature is modeled accord-
4.3 Extended Target Tracking 53

ing to
Np
Y
p(P W
|dTWW , ψT ) = N (p(i),W |RW T (ψT )p(i),T + dTWW , wp I2 ), (4.12)
i=1
which means that the uncertainty is assumed isotropic around the mean location
of the point and with known variance wp .
z N
At each time step a set of Nz measurements Z = {zi }i=1 is received and has to be as-
sociated to the states. Not all measurements arise from a point feature, some are
due to false detections (clutter). The association hypotheses are derived through
some data association algorithm. In Vermaak et al. (2005) a method is proposed
where the association hypotheses are included in the state vector and the output
of the tracking filter is a joint posterior density function of the state vector and the
association hypotheses. Furthermore, a multi-hypothesis likelihood is obtained
by marginalizing over all the association hypotheses. An alternative solution is
also proposed using a particle filter, where the unknown hypotheses are sampled
from a well designed proposal density function.
An automotive radar sensor model developed for simulation purposes is pro-
posed in Bühren and Yang (2006), where it is assumed that radar sensors often
receive measurements from specific reflection centers on a vehicle. These reflec-
tion centers can be tracked in a filter and valuable information regarding the
vehicle’s orientation can be extracted as shown by Gunnarsson et al. (2007). A
difficulty in solving the data association problem is the large number of associ-
ation hypotheses available. To reduce the complexity Gunnarsson et al. (2007)
proposes an approach where detections are associated with reflector groups. The
spatial Poisson distribution, discussed in the subsequent section, is considered to
be inappropriate, since the number of vehicle detections is assumed essentially
known and not adequately modeled by a Poisson process.
In paper F the point features are denoted measurement generating points (mgp),
and they are positioned on the surface of the target. In the referred publication,
the mgps are not considered having a fixed position on the surface, but they are
instead defined as a random finite set of points on the one-dimensional surface of
the target. The positions of the points on the surface are estimated in the target
tracking filter, but they are of less importance, since they only serve as a means
to estimate the position, shape and size of the entire target.

4.3.2 Spatial Distribution

Instead of modeling the target as a number of point features, which are assumed
to be explicit measurement sources, the target may also be represented by a spa-
tial probability distribution. It is more likely that a measurement comes from a
region of high spatial density than from a sparse region. In Gilholm and Salmond
(2005); Gilholm et al. (2005) it is assumed that the number of received target and
clutter measurements are Poisson distributed, hence several measurements may
originate from the same target. Each target related measurement is an indepen-
54 4 Target Tracking

dent sample from the spatial distribution. The spatial model could be a bounded
distribution, such as a uniform pdf or an unbounded distribution, such as a Gaus-
sian. The Poisson assumption allows the problem, or more specifically the evalu-
ation of the likelihood, to be solved without association hypotheses. The spatial
distribution is preferable where the point source models are poor representations
of reality, that is in cases where the measurement generation is diffuse.
In Gilholm and Salmond (2005) two simple examples are given. One where the
principle axis of the extended target is aligned with the velocity vector, i.e., the
target is represented by a one dimensional uniform stick model. In the other
example, a Gaussian mixture model is assumed for the target. A Kalman filter
implementation with explicit constructions of assignment hypotheses is derived
from the likelihood in Gilholm and Salmond (2005), whereas in Gilholm et al.
(2005), a particle filter is applied directly given the likelihood which is repre-
sented by the Poisson spatial model of the stick. Hence, the need to construct
explicit measurement-target assignment hypotheses is avoided in Gilholm et al.
(2005). Swain and Clark (2010) proposes instead a standard measurement model
but represents instead the extended targets as a spacial cluster process.
Boers et al. (2006) presents a similar approach, but since raw data is considered,
no data association hypotheses are needed. The method to use raw data, i.e., con-
sider the measurements without applying a threshold, is referred to as track be-
fore detect. A one dimensional stick target is assumed also by Boers et al. (2006),
but unlike Gilholm and Salmond (2005), the target extent is assumed unknown.
The state vector is given by the stick’s center position and velocity as well as the
stick’s extension according to
h iT
x = x y ẋ ẏ l . (4.13)
The process model is a simple constant velocity model and the length l is modeled
as a random walk. The likelihood function is given by the probability distribution
Z
p(z|x) = p(z|x̃)p(x̃|x)d x̃, (4.14)

where the spatial extension is modeled by the pdf p(x̃|x) and x̃ is assumed to be
a point source from an extended target with center given by the state vector x.
Hence, a measurement is received from a source x̃ with likelihood p(z|x̃).

4.3.3 Elliptical Shaped Target

In many papers dealing with the shape of a target it is assumed that the sensor,
e.g., radar, is also able to measure one or more dimensions of the target’s extent. A
high-resolution radar sensor may provide measurements of a targets down-range
extent, i.e., the extension of the objects along the line-of-sight. The information
of the target’s extent is incorporated in the tracking filter and aids the tracking
process to maintain track on the target when it is close to other objects.
An elliptical target model, to represent an extended target or a group of targets,
is proposed in Drummond et al. (1990). The idea was improved by Salmond and
4.3 Extended Target Tracking 55

Parr (2003), where the sensor not only provides measurements of point observa-
tions, but rather range, bearing and down-range extent. The prime motivation of
the study is to aid track retention for closely spaced moving targets. Furthermore,
the state vector includes the position, velocity and the size of the ellipse. An ekf
is used in Salmond and Parr (2003), but it is concluded that the filter may di-
verge under certain conditions, since the relation between the down-range extent
measurement of the target and the position and velocity coordinates in the state
vector is highly nonlinear. The same problem is studied in Ristic and Salmond
(2004), where a ukf is implemented and tested. Even though the ukf shows bet-
ter performance it is concluded that neither the ekf nor the ukf are suitable for
this problem. The problem is further studied by Angelova and Mihaylova (2008),
where other filter techniques, based on Monte Carlo algorithms, are proposed. In
this paper the size of the ellipse takes values from a set of standard values, i.e.,
the algorithm estimates the type of object from a list, under the assumption that
typical target sizes are known.

A group of objects moving collectively may also be modeled as an extended target.

The ellipse model is used to model a formation of aircraft in Koch (2008). The
object extension is represented by a symmetric positive definite random matrix,
however, the measurement error is not considered in this publication. Improve-
ments of this approach, including the consideration of sensor error, has been
published by Feldmann et al. (2011). An alternative measurement model and
an extesion using principal components is proposed by Degerman et al. (2011).
Wieneke and Davey (2011) show how the random matrix approach can be used
to track multiple extended targets directly from image data.

The concept of random hypersurface model, introduced by Baum and Hanebeck

(2009), assumes that each measurement source is an element of a randomly gen-
erated hypersurface. In this publication an elliptical target shape is used to exem-
plify the approach. Improvements of the approach has been published in Baum
et al. (2010), and a generalization which allows for tracking detail shapes, based
on star convex shaped extended targets, is presented in Baum and Hanebeck
(2011).

4.3.4 Curved Target

In Paper C the road borders are modeled as extended targets in the form of curved
lines. A curved line is expressed as a third order polynomial in its coordinate
frame. Since the road borders are assumed to be stationary, the frames are not in-
cluded in the state vector. Furthermore, stationary points such as delineators and
lamp posts are also modeled in Paper C. The nearest neighbor algorithm is used
to associate measurements from stationary observations z(m) to the targets. Here
it is assumed that an extended line target L(j) can give rise to several measure-
ments, but a point target P(i) can only contribute to one measurement. Since the
likelihood of a line `m,j is a one dimensional spatial density function, but the like-
lihood of a point `m,i is given by a two dimensional density function, a likelihood
ratio test is applied to determine the measurement-to-track association problem.
56 4 Target Tracking

The likelihood ratio for a measurement z(m) is given by

p
(m) `m,i
Λ(z ) , . (4.15)
`m,j
where the square root of the point likelihood is taken for unit matching, since the
unit of the point likelihood is (distance)−2 and the unit of the line likelihood is
(distance)−1 . The corresponding likelihood ratio test is
H0
Λ(z(m) ) ≷ η, (4.16)
H1

where H0 and H1 correspond to hypotheses that the measurement z(m) is asso-

ciated to the point Pi and to the line Lj , respectively. The threshold is selected
experimentally. More theory about likelihood ratio test is given by e.g., van Trees
(1968).

4.3.5 Extended Target Tracking and PHD filter

In the recent work Mahler (2009a) an extension of the (phd) filter to also han-
dle extended targets of the type presented in Gilholm et al. (2005) is given. The
phd filter is the topic of next section and therefore it will only be briefly summa-
rized here. In Paper E a Gaussian-mixture implementation of the phd-filter for
extended targets called the extended target gm-phd-filter (et-gm-phd) is pre-
sented. It is shown that this filter works well in most situation, but that it is
sensitive in estimating the number of targets in a few situation e.g., when occlu-
sion occurs. Therefore, generalization of Mahler’s work has been made to derive
the cardinalized phd (cphd) filter for extended targets, presented by Orguner
et al. (2011). In addition to the derivation, a Gaussian mixture implementation
for the derived cphd filter is presented. This filter has less sensitive estimates
of the number of targets. Early results on laser data are shown which illustrates
robust characteristics of the cphd filter compared to its phd version.
Initial step have also been taken towards including estimation of target extent in
the gm-phd-filter, see Granström et al. (2011b). The state vector is augmented
with a discrete state describing the type of object e.g., rectangle, ellipse etc. In Pa-
per F a hybrid state space is introduced, where mgps and the measurements are
modeled by random finite sets and target states by random vectors. For each re-
alization of the state vector, a phd filter is utilized for estimating the conditional
set of mgps given the target states.
 Probability Hypothesis Density Filter
5
and Its Implementation

In the previous chapter it was shown how single target tracking filters can be
extended and used to handle multiple targets by enclosing the filters with a tar-
get management framework. In this way measurements are associated to targets,
which are tracked as isolated entities. To be able to perform the data association
it is assumed that the number of present targets is known. This is rarely true, and
therefore a track management logic estimated the number of valid and confirmed
tracks. These methods explicitly separate the estimation of the number of targets
and estimation of target states, which is suboptimal. Random finite set (rfs)
formalism introduces a more rigorous approach to the multi target tracking prob-
lem. A set contains the random number of single target tracks, of which each is a
random vector. The state vectors are elements of the rfs and their change in time
is described with a motion model. The overall tracking problem is to compute
the posterior density of the set-valued quantity, which also enables the approach
to describe the uncertainty in the number of objects. The ordering of each single
target state is not of importance in the the set notation; the estimation problem is
reduced to capture the most essential part of the multi target tracking problem,
estimating the number of objects and their individual states.
This chapter is split into three sections, which takes the estimation problem from
a pure theoretical view into a practical implementation. The chapter begins in
Section 5.1 with an introduction to the Bayes formulation of the rfs filter, which
propagates the complete density of the rfs. This approach is not practically im-
plementable, and therefore a solution is to only propagate the first order mo-
ment, called the probability hypothesis density (phd), as described in Section 5.2.
There exist a few practical representations of the phd, of which the Gaussian mix-
ture has turned out to be the most well-used during the last years. The Gaussian
mixture phd filter (gm-phd) is the topic of Section 5.3. This chapter aims at sum-

57
58 5 PHD Filter and Its Implementation

observation space

Zk+1
Zk

state space

meta target motion

Xk+1
Xk

Figure 5.1: Illustration of the rfs of states and measurements at time k and
k + 1. Note that this is the same setup as previously shown for the standard
multitarget case in Figure 4.2.

marizing the basic ideas and implementation methods, more details can be found
in the textbook by Mahler (2007a), and other overviews are given by Mahler
(2009b); Challa et al. (2011). Point process theory is described by e.g., Daley
and Vere-Jones (2003); Streit (2010).

5.1 Introduction to Finite Set Statistics

The rfs estimation problem is in general solved in the same way as the single
target tracking problem is approached, i.e., using Bayes’ theorem. However, with
the difference that the density is defined on a set rather than on a vector. Finite
set statistics (fisst), introduced by Goodman et al. (1997); Mahler (2004), is a
multisource-multitarget differential and integral calculus based on the so called
belief-mass function, and the fact that it is the multisensor-multitarget counter-
part of the probability-mass function for single target.

The conceptual idea of fisst is to redefine the target set as a single target, a so
called meta target, with multitarget state
X = {x(1) , x(2) , . . . , x(Nx ) }, (5.1)
a so called meta state; and similarly to redefine the observation set
Z = {z(1) , z(2) , . . . , z(Nz ) }, (5.2)
as a single measurement, a meta measurement, of the observed meta targets. The
concept is illustrated in Figure 5.1, where meta targets are shown in the state
space in the lower layer and meta observations in the observation space in the
upper layer. Furthermore, the multitarget multisensor data can be modeled using
a multisensor-multitarget measurement model
Zk = H(Xk ) ∪ Ck , (5.3)
5.1 Introduction to Finite Set Statistics 59

where H(Xk ) is the rfs of measurements originating from the true target and C
is the clutter rfs. To see the similarity, compare this multitarget model with the
single target measurement model (2.1a). The motion of the meta target can be
modeled using a multitarget motion model
Xk+1 = F(Xk ) ∪ Bk (5.4)
where F( · ) models the change of a rfs from time k to k + 1, and B is the rfs
of born targets. Compare, also here, with the single target motion model (2.1b).
Given this the multisensor, multitarget estimation problem can be reformulated
into a single-sensor, single-target problem. The rfs is formally defined in Sec-
tion 5.1.1, followed by a definition of the belief-mass function and multitarget
density function in Section 5.1.2. The multisource multitarget Bayes filter is de-
scribed in Section 5.1.3.

5.1.1 Random Finite Set

Before continuing describing the fisst and phd a formal definition of a rfs is
given below.
5.1 Definition (random finite set). A random finite set (rfs) Ψ is a random
variable that has realizations Ψ = Y ∈ Y where the hyperspace Y is the set of all
finite subsets of some underlying space Y0 .

Since the definition may seem a bit abstract it is illustrated with a simple example,
related to target tracking.
5.2 Example: Target Tracking
Consider the target tracking example, which was introduced in Example 1.2. In
the example the state vector represents the Cartesian position of a target, i.e.,
nx = 2. The state space is defined as an Euclidean vector space, i.e., x ∈ R2 . This
state space is the underlying space Y0 in the definition above. The hyperspace
Y includes all finite subsets of Y0 = Rnx . Let the state variable vectors be x(i) ∈
Y0 = R2 for i = 1, . . . , ∞, then some realizations X of the random set X can be
X = ∅ (no targets), X = {x(1) , x(2) } (two targets with states x(1) and x(2) ), or X =
{x(1) , . . . , x(Nx ) } (Nx targets with states x(1) , . . . , x(Nx ) ).

5.1.2 Belief-Mass and Multitarget Density Function

The reformulation of the multisensor multitarget estimation problem as a single-
metasensor, single-metatarget problem is based on the concept of belief-mass.
Belief-mass functions are nonadditive generalizations of probability-mass func-
tion. In order to let the content be more easily digested, the multitarget notation
is accompanied with the single target analogous notation.
Starting with some simple notation taken from single target tracking (cf. Sec-
tion 4.1). For a random vector y the probability mass function py (S) gives the
probability of y being in some subset of the region S ⊆ Y0 , i.e.,
py = Pr(y ∈ S). (5.5)
60 5 PHD Filter and Its Implementation

The probability density function py (y) describes the likelihood of y to occur at a

given point y. The relation between the mass function and the density function
is given by
dpy
py (y) = . (5.6)
dy
The probability mass function for a random vector can be generalized to the
belief-mass function for rfs. The belief-mass function is denoted βΨ (S), and
is defined as the probability that the random finite set Ψ on Y0 is within some
region S, βΨ (S) = Pr(Ψ ⊆ S). Similarly, the probability density function for a
random vector can be generalized to the probability density function pΨ (Y ) of
a random finite set Ψ . The relation between the probability density function
Ψ pΨ (Y ) of a random finite set and the belief-mass function βΨ (S) is given by
Z
βΨ (S) pΨ (Y ) δY = Pr (Ψ ⊆ S) (5.7)
S

and
δβΨ
pΨ (Y ) =(∅). (5.8)
δY
δ·
R
Here, · δY denotes the set integral, and δY denotes the set derivative. The
definitions of the set integral and the set derivative are too complicated to present
here, the interested reader can consult Mahler (2007a).

These definitions have the following practical use when applied to the likelihood
function and the Markov transition density

• In the single target case the probabilistic-mass function of the sensor model
p(S|x) = Pr(z ∈ S|x) is the probability that the random observation z will
be found in a given region S if the target state is x. Analogously, in the
multitarget case the belief-mass function β(S|X) = Pr(Z ⊆ S|X) is the total
probability that all observations will be found in any given region S, if the
multitarget state is X. In the single target case the likelihood function is
obtained by differentiation of the probabilistic-mass function. The same
concept holds for the multitarget case, i.e., the true multitarget likelihood
function p(Z|X) can, using fisst, be derived from β(S|X) according to
δβk|k (∅|Xk )
pk|k (Zk |Xk ) = (5.9)
δZk
δ
where δZk is the set derivative.

• Similarly, the probability-mass pk+1|k (S|x) = Pr(xk+1 ∈ S|xk ) of a single tar-

get motion model is the probability that the target will be found in region S
at time step k + 1, given that it had state xk at time step k. In the multitarget
case the belief-mass function βk+1|k (S|X) = Pr(Xk+1 ⊆ S|Xk ) of a multitar-
get motion model is the total probability of finding all targets in region S
at time k + 1, if at time k they had a multi object state Xk . By differentiat-
5.1 Introduction to Finite Set Statistics 61

ing p(S|xk ) the Markov transition density pk+1|k (xk+1 |xk ) can be derived in
the single target case. Using fisst the multitarget Markov density can be
derived by taking the set derivative of the belief-mass function
δβk+1|k (∅|Xk )
pk+1|k (Xk+1 |Xk ) = (5.10)
δXk+1

5.1.3 The Multitarget Bayes Filter

Consider the single target state propagation summarized in Section 4.1. The filter
recursion is extended to the multitarget case under the fisst assumptions in this
section. The single target random state variables x are substituted with the rfs
of a set of targets. Before giving the filter recursion, the interpretation of a set
probability function is discussed. Consider the multitarget state propagation
predictor corrector
· · · → pk|k (Xk |Z1:k ) → pk+1|k (Xk+1 |Z1:k ) → pk+1|k+1 (Xk+1 |Z1:k+1 ) → · · ·

The meaning of the underlined prior is exemplified below,

p(∅|Z1:k ) probability that there are no targets present

(1) (1)
p({xk }|Z1:k ) likelihood of one target with state xk
(1) (2) (1) (2)
p({xk , xk }|Z1:k ) likelihood of two targets with state xk , xk
..
.
(1) (Nx ) (1) (Nx )
p({xk , . . . , xk }|Z1:k ) likelihood of Nx targets with state xk , . . . , xk

i.e., the set density is not only a function of the state variable values, but also a
function of the number of targets Nx in the set.

The single target Bayes filter equations were stated in (3.13), the multitarget
Bayes filter counterpart has the form
pk (Z1:k |Xk ) · pk|k−1 (Xk |Z1:k−1 )
pk|k (Xk |Z1:k ) = R (5.11a)
pk (Z1:k |Xk ) · pk|k−1 (Xk |Z1:k−1 ) δXk
Z
pk+1|k (Xk+1 |Z1:k ) = pk+1|k (Xk+1 |Xk ) · pk|k (Xk |Z1:k )δXk (5.11b)

where pk (Zk |Xk ) is the multisource likelihood function, pk+1|k (Xk+1 |Xk ) is the
multitarget Markov transition function, pk|k−1 (Xk |Zk−1 ) is the multitarget prior,
pk|k (Xk |Zk ) is the multitarget posterior and pk+1|k (Xk+1 |Zk ) is the multitarget pre-
62 5 PHD Filter and Its Implementation

diction. The filter recursion is given by,

predictor corrector
· · · → pk|k (Xk |Z1:k ) → pk+1|k (Xk+1 |Z1:k ) → pk+1|k+1 (Xk+1 |Z1:k+1 ) → ···

↑ ↑

Markov density Likelihood function

p(Xk+1 |Xk ) p(Zk+1 |Xk+1 )
↑ ↑
Motion Model
S Measurement Model
S
Xk+1 = F(Xk ) Bk Zk+1 = H(Xk+1 ) Ck+1
|{z} |{z} | {z } |{z}
persisting new generated clutter
observations

compare with the single target filter recursions given on the same form in Sec-
tion 3.2. Note that the motion model describes a union of the set of persisting
targets and the set of new born, or detected, targets. In the same manner, the
measurement model describes a union of the set of target generated observations
and the set of clutter measurement. Here, the measurement set Z is the set of all
observations received by the sensor.
The conclusions of this section can be summarized in a few items, which will be
furher addressed in the next section:
• The Bayesian filter on sets is very similar to standard Bayesian density re-
cursion.
• Related single target densities and operations are replaced with their set
equivalents, compare (5.11) with (3.13).
R
• A set integral · δX is necessary to derive the posterior
Z
p(Xk |Z1:k ) ∝ p(Zk |Xk ) p(Xk |Xk−1 )p(Xk−1 |Z1:k−1 )δXk−1

• Therefore, the filter is computationally prohibitive.

5.2 Introduction to the PHD filter

The multisource multitarget filter as described in the last section is computation-
ally demanding to implement. This section describes a new approach, first pro-
posed by Mahler (2003), for approximate multitarget nonlinear filters called the
probability hypothesis density (phd) filter. The phd is a first order moment ap-
proximation of the rfs. To describe this approximation the single target case will
first be repeated in Section 5.2.1 and then compared with the multitarget case in
Section 5.2.2, where also the phd is defined. The phd filter recursion is explained
in Section 5.2.3. The section is concluded in Section 5.2.4 with a short literature
survey of generalizations and extensions of the phd filter.
5.2 Introduction to the PHD filter 63

5.2.1 Approximations in Single-Target Tracking

The single target density propagation is illustrated below

predictor corrector
· · · →pk|k (xk |z1:k ) → pk+1|k (xk+1 |z1:k ) → pk+1|k+1 (xk+1 |z1:k+1 )→ · · ·
↓ ↓ ↓
predictor corrector
x̂k|k → x̂k+1|k → x̂k+1|k+1
At each time step the first order moment, i.e., the expected value may be col-
lapsed from the density function. In the so called constant-gain filter, of which
the alpha-beta filter is the most well known, only the expected value is propa-
gated over time. This makes the filter computationally faster. Both the first and
the second order moments are propagated in the standard Kalman filter, com-
pare with Section 3.2.1. If the density functions are Gaussian, the first and sec-
ond order moments are the sufficient statistics, hence propagating these variables
describes the density functions sufficiently. However, if the density functions are
not Gaussian, propagating the moments is only an approximation of the density.

5.2.2 Approximations in Multitarget Tracking

Based on the same analogy as with the single target tracking the density of the
rfs my be compressed to the first order moments, and these may be propagated
as follows:
predictor corrector
· · · →pk|k (Xk |Z1:k ) → pk+1|k (Xk+1 |Z1:k ) → pk+1|k+1 (Xk+1 |Z1:k+1 )→ · · ·
↓ ↓ ↓
predictor corrector
Dk|k (xk |Z1:k ) → Dk+1|k (xk+1 |Z1:k ) → Dk+1|k+1 (xk+1 |Z1:k+1 )
The first order moment is called a phd and denoted D. However, it is not obvious
what the multitarget counterpart of an expected value is; a naïve defininition
would be
Z
E(Ψ ) = X · pΨ (X)δX, (5.12)

where pΨ (X) is the multitarget probability density of a rfs Ψ . However, this inte-
gral is not mathematically defined, since addition of finite subsets is not usefully
defined. A strategy is to transform subsets X into vectors TX in some vector space.
It is important that this transformation X → TX preserves set-theoretic structures,
i.e., transforming unions into sums TX∪X 0 = TX + TX 0 , whenever X ∩ X 0 = ∅. An
indirect expected value can now be defined according to
Z
E(Ψ ) = TX · pΨ (X)δX. (5.13)

The vector is commonly chosen as TX = δX (x) with


0 if X = ∅


TX = P (5.14)
 y∈X δy (x) otherwise

64 5 PHD Filter and Its Implementation

where δy (x) is the Dirac delta function with peak at y. Given these assumptions,
the multitarget analog of the expected value is given by
Z
DΨ (x) = δX (x) · pΨ (X)δX. (5.15)

To exemplify this rather theoretical discussion, consider a realization Xk of the

(1) (2) (N )
rfs Xk = {xk , xk , . . . , xk x }. Define a scalar valued function of Xk according to
N
X
δXk = δ (i) (x), (5.16)
xk
i=1

as illustrated in Figure 5.2a. Then, the probability hypothesis density (phd) is

the expectation of δXk with respect to Xk
Z
Dk|k (x) = E(δXk ) = δXk · pΨ (X)δX. (5.17)

The expected value is illustrated in Figure 5.2b. Note also that the expected num-
ber of targets in the volume S is
Z
bx = Dk|k (x)dx
N (5.18)
S

In fact one can define the phd using the expected number of Rtargets as follows:
Given any region S of single target state space X0 the integral S Dk|k (x)dx is the
expected number of targets in S.

The phd is an intensity function, which models the joint density over the states.
The phd is multi-modal, with peaks near the actual targets, see Figure 5.3. This
figure shows the phd approximation of the rfs in Figure 5.1. The intensity func-
tion is used in Paper D to represent the density of stationary targets along the
edges of a road. An autonomous driving vehicle should avoid areas with high
intensity and the objective should be to keep the vehicle in the valleys of the
intensity function.

5.2.3 The PHD Filter

The phd filter consists of two equations, the predictor equation, which extrapo-
lates the current phd to the predicted phd at the time of the new observation,
and the corrector equation, which updates the predicted phd with the new obser-
vations.

The phd predictor equations takes care of the motion of existing targets, birth
and spawn of new targets and the death of existing targets. The components of
the prediction model are summarized below:
5.2 Introduction to the PHD filter 65

10

0
1
1
0.5
0.5

0 0
(a) Sum of Dirac functions δXk

10

0
1
1
0.5
0.5

0 0
(b) Expectation E(δXk )

Figure 5.2: Definition and example of the phd, the Dirac functions repre-
sents the rfs and the surface the first order moment, i.e., the phd.
66 5 PHD Filter and Its Implementation

Figure 5.3: Illustration of the phd for the time steps k and k + 1. This is the
pdf of the rfs in Figure 5.1.

model time step description

motion: p(xk+1 |xk ) xk → xk+1 likelihood that target will have state
xk+1 if it had state xk
death: 1 − PS (xk ) xk → ∅ probability that target will vanish if
it had state xk
spawn: b(Xk+1 |xk ) xk → Xk+1 likelihood that a target will spawn
target set Xk+1 if it had state xk
birth: b(Xk+1 ) ∅ → Xk+1 likelihood that a target set Xk+1 will
appear in scene

In the table above PS (x) denotes the probability that a target with state x at time k
will survive at time k + 1, p(xk+1 |xk ) is the single target Markov transition density,
b(Xk+1 |xk ) is the phd of targets spawned by other targets with state x, and b(Xk+1 )
is the phd of new appearing targets. The phd predictor equation is given by
Dk+1|k (xk+1 |z1:k ) =
| {z }
time updated PHD
Z h i
= bk+1|k (xk+1 ) + PS (ξk ) p(xk+1 |ξk ) + b(xk+1 |ξk ) Dk|k (ξk |z1:k ) dξk .
| {z } | {z } | {z } | {z } | {z }
target birth PHD probability of Markov transition target spawn by PHD from previous k
survival density existing targets
(5.19)
The parts of the model are illustrated with a number of plots in Figure 5.4. One
example of a phd Dk|k (ξ|z1:k ) from the previous time step k is illustrated in Fig-
ure 5.4a. The magnitude of the phd is reduced a bit when multiplied with the
probability of survival PS (ξ) in Figure 5.4b. Thereafter, multiplying with the
Markov transition density and then integrating changes the mean value and the
covariance, see Figure 5.4c. Note, that all target motions are assumed statistically
independent. In Figure 5.4d, the spawn phd b(xk+1 |ξk ) is added in red, and in
Figure 5.4e the birth phd bk+1|k (xk+1 ) in green. Finally, the total predicted phd
Dk+1|k (xk+1 |z1:k ), after time update, is shown in Figure 5.5a.
5.2 Introduction to the PHD filter 67

0.5 0.5

0.4 0.4

PD Dk−1
0.3 0.3
Dk−1

0.2 0.2

0.1 0.1

0 0
−5 0 5 10 −5 0 5 10
x x

(a) phd from previous k (b) multiplied with PS

0.5 0.5
∫ (PD p(xk|ξ)+b(xk|ξ) Dk−1 d ξ

0.4 0.4
∫ PD p(xk|ξ)Dk−1 d ξ

0.3 0.3

0.2 0.2

0.1 0.1

0 0
−5 0 5 10 −5 0 5 10
x x

(c) multiplied with transition density (d) adding spawn phd

and integrated

0.5 0.5
∫ (PD p(xk|ξ)+b(xk|ξ) Dk−1 d ξ + b

0.4 0.4

0.3 0.3
Dk|k−1

0.2 0.2

0.1 0.1

0 0
−5 0 5 10 −5 0 5 10
x x

(e) adding birth phd (f) predicted phd

Figure 5.4: An example showing the components of the predictor equa-

tion (5.19) of phd filter.
68 5 PHD Filter and Its Implementation

The phd corrector equations takes care of the likelihood of the existing targets,
misdetection of targets and clutter. The components of the prediction model are
summarized below:
model update description
likelihood: p(zk |xk ) xk → zk likelihood that target will gener-
ate observation zk if it has state
xk
misdetection: (1 − PD (xk )) xk → ∅ state dependent probability that
target will not generate an obser-
vation
clutter: c(Zk ) ∅ → Zk likelihood that a set Zk =
(1) (M)
{zk , . . . , zk } of clutter observa-
tions will be generated

In the table above PD (xk ) is the probability of detection of a target with the state
x at time step k, p(zk |xk ) is the single target likelihood function and c(Zk ) is the
density of Poisson false alarms, due to clutter. The modeling is done under the
assumption that the observations and the clutter are statistically independent.
Furthermore it is assumed that the multitarget prediction is approximately Pois-
(1) (M)
son. Given a new scan of data Zk = {zk , . . . , zk } the corrector equation is given
by
X Λk|k (x|z)
Dk|k (x|Z1:k ) ≈ R + (1 − PD (x)) Dk|k−1 (x|Z1:k−1 ) (5.20)
| {z } z∈Zk λk ck (z) + Λk|k (x|z)dx | {z }
updated PHD | {z } predicted PHD
pseudo-likelihood

where λk is the average number of false alarms. Furthermore

Λk|k (x|z) = PD (x)p(z|x)Dk|k−1 (x|z1:k ). (5.21)
The corrector equation is again exemplified by a series of plots in Figure 5.5. Fig-
ure 5.5a shows the predicted phd. The non-detected targets are modeled by mul-
tiplying the predicted phd with (1 − PD (x)), as illustrated in red in Figure 5.5b.
Assume that two detections are made; the likelihood functions p(z|x) of these indi-
vidual observations are shown in Figure 5.5c. The pseudo-likelihood function of
the detections multiplied with the predicted phd is shown in blue in Figure 5.5d.
Finally, the updated posterior phd is illustrated in Figure 5.5e.

In Paper E, the pseudo-likelihood function is modified to handle extended targets.

This modification is one of the major contributions of that paper.

5.2.4 Generalizations of the PHD filter

There exists an improvement of the phd filter, called the cardinalized probabil-
ity hypothesis density (cphd) filter, which also propagates the probability dis-
tribution on the target number. This filter generally performs better, but with
increased computational load. The cphd filter is too complex to be described in
5.2 Introduction to the PHD filter 69

0.5 0.5

0.4 0.4

(1−PD)Dk|k−1
0.3 0.3
Dk|k−1

0.2 0.2

0.1 0.1

0 0
−5 0 5 10 −5 0 5 10
x x

(a) phd from prediction (b) Non-detected targets

0.5 0.5

0.4 0.4
Σ Dk|k(x|z)

0.3
p(z|x)

0.3

0.2 0.2

0.1 0.1

0 0
−5 0 5 10 −5 0 5 10
x x

(c) likelihood functions of two measure- (d) Detected and non-detected targets
ments

0.5

0.4

0.3
Dk

0.2

0.1

0
−5 0 5 10
x

(e) Updated phd

Figure 5.5: Steps in the phd update and corrector equations (5.20).
70 5 PHD Filter and Its Implementation

the present chapter, and it is also not used in any of the publications in Part II.
Details about the cphd filter can be found in Mahler (2007b,a), and a Gaussian
mixture implementation is described in Vo et al. (2007).
The standard phd filter as described in this section does not maintain track labels
from time step to time step. A so called peak to track association technique has
been proposed by Lin et al. (2004); Panta et al. (2004) in order to maintain track
labels.
Furthermore, phd smoother has been studied in Mahler et al. (2010); Clark (2010),
and a Gaussian mixture implementation is presented by Vo et al. (2010, 2011).
Again, since the smoother is not further used in Part II, it is also not described
here.
The so called intensity filter, developed by Streit, is another generalization of the
phd filter. The difference is that in the phd filter certain prior assumptions re-
garding target birth and measurement clutter are made. More specifically the
intensity filter uses an augmented single target state space, while the phd filter
uses only the standard single target state space. The augmented state space rep-
resents the absent target hypothesis, and it therefore allows the filter to on-line
estimate the intensities of the target birth and measurement clutter Poisson point
processes. Note that the phd is an intensity function, and in the phd filter the
birth and clutter intensities are known a priori. A thorough description of the
intensity filter is given in the textbook by Streit (2010) and a summary is given in
the publications Streit and Stone (2008); Streit (2008).

5.3 Gaussian Mixture Implementation

There exist primarily two different implementations of the phd filter, the smc
approximation, see Vo et al. (2003); Sidenbladh (2003); Zajic and Mahler (2003),
and the Gaussian mixture (gm) approximation, introduced in Vo and Ma (2006),
and hence called gm-phd filter. A physical interpretation of the phd filter is
proposed by Erdinc et al. (2009). Both approximations of the phd are exemplified
in Figure 5.6.
The advantage of the smc approximation is that it can accommodate highly non-
linear motion and measurement models. The disadvantages are that it is com-
putational demanding. The smc based approximation is not further used in this
thesis and it is therefore not described in further detail here, the interested reader
is referred to Vo et al. (2003) or the textbook Mahler (2007a). The gm implemen-
tation is discussed in more detail in this section.
The gm-phd filter has the following properties,
• It is less computationally demanding than the smc solution.
• It is exact, i.e., it provides a true closed-form algebraic solution.
• It is easy to implement,
5.3 Gaussian Mixture Implementation 71

0.7 0.7

0.6 0.6

0.5 0.5

0.4 0.4
Dk

Dk
0.3 0.3

0.2 0.2

0.1 0.1

0 0
−5 −4 −3 −2 −1 0 1 2 3 4 5 −5 −4 −3 −2 −1 0 1 2 3 4 5
state space state space

(a) Sequential Monte Carlo phd approx- (b) Gaussian mixture phd approxima-
imations (smc-phd) tion (gm-phd)

Figure 5.6: Practical implementation methods of the phd.

• however, measurement and motion models must be linear-Gaussian.

This section is outlined as follows. The approximations made in the gm-phd
filter are summarized in Section 5.3.1 and the algorithm is given in Section 5.3.2.
Some practical details regarding the implementation, e.g., target extraction are
described in Section 5.3.3.

5.3.1 Gaussian Mixture PHD Approximation

Similarly, as first shown for the theoretical Bayesian multitarget filter in Sec-
tion 5.1.3 and then for the more practical standard phd filter in Section 5.2.2,
the easily implementable filter recursion of the gm-phd filter is summarized be-
low:
predictor corrector
··· → Dk|k (x|Z1:k ) → Dk+1|k (x|Z1:k ) → Dk+1|k+1 (x|Z1:k+1 )

↓ ↓ ↓

PJk|k (i) 
(i) (i)
 PJk+1|k (i) 
(i) (i)
 PJk+1|k+1 (i) 
(i) (i)

··· → w N x; µ , P → w N x; µ ,P → w N x; µ ,P
i=1 k|k k|k k|k i=1 k+1|k k+1|k k+1|k i=1 k+1|k+1 k+1|k+1 k+1|k+1
↓ ↓ ↓

(1) (Jk|k ) (1) (Jk+1|k ) (1) (Jk+1|k+1 )

µ ,...,µ → µ ,...,µ → µ ,...,µ
k|k k|k k+1|k k+1|k k+1|k+1 k+1|k+1
(1) (Jk|k ) (1) (Jk+1|k ) (1) (Jk+1|k+1 )
P ,...,P → P ,...,P → P ,...,P
k|k k|k k+1|k k+1|k k+1|k+1 k+1|k+1
(1) (Jk|k ) (1) (Jk+1|k ) (1) (Jk+1|k+1 )
w ,...,w → w ,...,w → w ,...,w
k|k k|k k+1|k k+1|k k+1|k+1 k+1|k+1

The phd is represented by a Gaussian mixture and the summary statistics of the
Gaussian components, i.e., the mean value µ and the covariance P are propagated
together with a weight w for each Gaussian component. The number of Gaussian
components at each time step is denoted J. The prior, predicted and posterior
gm-phd are shown above.
When deriving the gm-phd filter some assumptions are made, see Vo and Ma
(2006). Since these assumptions are formally repeated in Paper E, on Page 230,
72 5 PHD Filter and Its Implementation

these are only summarized here. It is assumed that each target follows a linear
Gaussian motion model,
pk+1|k (xk+1 |xk ) = N (xk+1 ; Fk xk , Qk ), (5.22)
where Fk is the state transition matrix and Qk is the process noise covariance, and
it is assumed that all target motions are statistically independent. Observations
and clutter are assumed statistically independent and the sensor is assumed to
have a linear Gaussian measurement model, i.e.,
pk|k (zk |xk ) = N (zk ; Hk xk , Rk ), (5.23)
where Hk is the measurement model matrix and Rk is the observation noise co-
variance. Furthermore it is assumed that the survival and detection probabilities
are state independent, i.e., PS (x) = PS and PD (x) = PD . The phd of the birth and
the spawn are Gaussian mixtures
Jb,k
(i) (i) (i)
X
b(Xk+1 ) = wb,k N (x; µb,k , Pb,k ), (5.24)
i=1
Jβ,k+1
(i) (`) (i,`) (i,`)
X
b(Xk+1 |xk ) = wβ,k+1 N (x; µβ,k+1|k , Pβ,k+1|k ), (5.25)
`=1
(i) (i) (i)
where wb,k , µb,k and Pb,k are the weight, the mean and the covariance of the ith
born component, i = 1, . . . , Jb,k , and Jb,k is the number of components. Further,
(`) (i,`) (i,`)
wβ,k+1 , µβ,k+1|k and Pβ,k+1|k are the weight, mean and covariance of the `th com-
ponent ` = 1, . . . , Jβ,k+1 at time k + 1 spawned from the ith component at time k,
and Jβ,k+1 is the total number of components spawned from component i. It is
also assumed that the predicted multitarget rfs are Poisson.

5.3.2 GM-PHD Filter Algorithm

In this section the gm-phd filter algorithm including the predictor and the cor-
rector equations, are given. The prediction equation is an approximation of the
general phd prediction equation (5.19), with the difference that the phd compo-
nents are substituted with Gaussian mixtures according to
Jb,k Jk−1
(j) (j) (j) (j) (j) (j)
X X
Dk|k−1 (x|Z1:k−1 ) = wb,k N (x; µb,k , Pb,k ) + PS wk−1 N (x; µk|k−1 , Pk|k−1 )
j=1 j=1
Jβ,k
Jk−1 X
(j,`) (j,`) (j,`)
X
+ wβ,k|k−1 N (x; µβ,k|k−1 , Pβ,k|k−1 ). (5.26)
j=1 `=1

In the case when the motion and sensor models are linear, the Gaussian compo-
nents can be predicted using the prediction step of the Kalman filter (3.16). The
illustration in Figure 5.4 still holds for this Gaussian case, and the discussion is
therefore not repeated again. The corrector equation is an approximation of the
5.3 Gaussian Mixture Implementation 73

general phd corrector equation (5.20), with the difference that the phd compo-
nents are substituted with Gaussian mixtures according to
JX
k|k−1   XNz JX
k|k−1  
(i) (i) (i) (i,j) (i,j) (i,j)
Dk|k (x|Z1:k ) ≈ wk|k N µk|k , Pk|k + wk|k N µk|k , Pk|k (5.27)
i=1 j=1 i=1
| {z } | {z }
not detected targets detected targets

The complete filter recursion, as it is presented by Vo and Ma (2006), is given in

Algorithm 7.

Algorithm 7 Gaussian Mixture phd filter

k−1(i) (i) (i) J
Require: the Gaussians {wk−1 , µk−1 , Pk−1 }i=1 and the measurement set Zk
Prediction for birth and spawn targets
1: i = 0
2: for j = 1 to Jb,k do
3: i = i+1
(i) (j) (i) (j) (i) (j)
4: wk|k−1 = wb,k , µk|k−1 = µb,k , Pk|k−1 = Pb,k
5: end for
6: for j = 1 to Jβ,k do
7: for ` = 1 to Jk−1 do
8: i = i+1
(i) (`) (j)
9: wk|k−1 = wk−1 wβ,k
(i) (j) (`) (j) (i) (j) (j) (`) (j)
10: µk|k−1 = Fβ,k−1 µk−1 + dβ,k−1 , Pk|k−1 = Qβ,k−1 + Fβ,k−1 Pβ,k−1 (Fβ,k−1 )T
11: end for
12: end for
Prediction for existing targets
13: for j = 1 to Jk−1 do
14: i = i+1
(i) (j)
15: wk|k−1 = PS wk−1
(i) (j) (i)
T (j)
16: µk|k−1 = Fk−1 µk−1 , Pk|k−1 = Fk−1 Pk−1 Fk−1 + Qk−1
17: end for
18: Jk|k−1 = i
Construction of phd update components
19: for j = 1 to Jk|k−1 do
(j) (j) (j) (j)
20: ηk|k−1 = Hk µk|k−1 , Sk = Rk + Hk Pk|k−1 HkT
(j) (j) (j) (j) (j) (j)
21: Kk = Pk|k−1 HkT (Sk )−1 , Pk|k = (I − Kk Hk )Pk|k−1
22: end for
Update non-detected targets
23: for j = 1 to Jk|k−1 do
(j) (j)
24: wk = (1 − PD )wk|k−1
(j) (j) (j) (j)
25: µk = µk|k−1 , Pk = Pk|k−1
26: end for
74 5 PHD Filter and Its Implementation

Update detected targets

27: `=0
28: for each z ∈ Zk do
29: ` = `+1
30: for j = 1 to Jk|k−1 do
(`Jk|k−1 +j) (j) (j)
31: wk = PD wk|k−1 N (z; ηk|k−1 , Sk )
(`Jk|k−1 +j) (j) (j) (j)
32: µk = µk|k−1 + Kk (zk − ηk|k−1 )
(`J +j) (j)
33: Pk k|k−1 = Pk|k
34: end for
35: for j = 1 to Jk|k−1 do
(`Jk|k−1 +j)
(`Jk|k−1 +j) wk
36: wk = PJk|k−1 (`Jk|k−1 +i)
λc(z)+ i=1 wk
37: end for
38: end for
39: Jk = `Jk|k−1 + Jk|k−1
(i) (i)
k (i) J
40: return {wk , µk , Pk }i=1

To allow for nonlinear motion and sensor models, the ekf or ukf can be utilized,
in that case the prediction and update of the single Gaussian components are in-
stead performed as described in Algorithm 4 and 5, respectively. The ukf version
of the gm-phd filter is used in Paper D to track the edges of a road and thereby
to create an intensity map of the environment. An extension of the kf based
gm-phd used to track extended targets, is presented Paper E. In that paper mod-
ifications are made to the update step to account for the extended targets, which
may give rise to more than one measurement per target and time step. In Paper F
the gm-phd filter is used as an inner core in a tracking filter which aims at esti-
mating the size and shape of targets. The outer shell of the approach represents
the position and motion of the target, whereas the phd is defined on the surface
of the target as a means to estimate its shape.

5.3.3 Merge, Prune and Extract Targets

A few approximations and modifications to the gm-phd filter must be made in or-
der to obtain a computationally realizable filter. The number of Gaussian compo-
nents grows rapidly, since each predicted Gaussian component is updated with
each measurement. To handle this situation components with low weights are
pruned and similar components are merged. The procedure is described in Vo
and Ma (2006) and in Algorithm 8. A model based approach to merge compo-
nents is presented in Paper D.
The extraction of multiple-target state estimates is straightforward from the pos-
terior phd. It can be assumed that closely spaced components are reasonable
well separated after the prune and merge step of the algorithm. The means of
the remaining Gaussian components are the local maxima of the phd. The states
are extracted by selecting the means of the Gaussians that have weights greater
5.3 Gaussian Mixture Implementation 75

Algorithm 8 Merging and Pruning for gm-phd filter

(i)
k|k (i) (i) J
Require: the Gaussian components {wk , µk , Pk }i=1 , the truncation threshold δt
and the merging threshold δm
(i)
1: initiate: by setting ` = 0, I = {i = 1, . . . , Jk|k |wk > δt } and then
2: repeat
3: ` := ` + 1
(i)
4: j := arg max wk
( i∈I   T  −1   )
 (i) (j) (i) (i) (j)
5: L := i ∈ I  µk − µk Pk µ k − µ k ≤ δm ,
(`) P (i)
6: w̃k = i∈L wk
(`) 1 P (i) (i)
7: µ̃k = (`) i∈L wk µk
w̃k
  T !
(`) (i) (i) (`) (i) (`) (i)
P˜k = 1 P
8: (`) i∈L wk Pk + µ̃k − µk µ̃k − µk
w̃k
9: I := I\L
10: until I = ∅
(i) (i) (i)
11: if ` > Jmax , the then chose the Gaussians {w̃k , µ̃k , P˜k }`i=1 with the largest
weights.
(i) (i) (i)
12: return the Gaussian components {w̃k , µ̃k , P˜k }`i=1

than some threshold, see Algorithm 9. An advantage with the phd filter is that
a hard decision, i.e., extraction of targets, is only performed for obtaining an out-
put from the filter at each time step. The extracted targets are not used within the
filter; the information about the intensity of targets remains in the filter without
the need to make hard decisions, which would destroy information.
76 5 PHD Filter and Its Implementation

Algorithm 9 Multitarget State Extraction

(i) (i) k(i) J
Require: {wk , µk , Pk }i=1 and a threshold δe
1: Set b
Xk = ∅
2: for i = 1 to Jk do
(i)
3: if wk > δe then
(i)
4: for j = 1 to round(wk ) do
h i
(i)
5: update b Xk = bXk µk
6: end for
7: end if
8: end for
9: return the multitarget state estimate b
Xk
 Concluding Remarks
6
In the first part an overview of the basics behind the research reported in this
thesis has been presented. This part also aims at explaining how the papers in
Part II relate to each other and to the existing theory. A conclusion of the re-
sults presented in this thesis is given in Section 6.1 and ideas for future work are
discussed in Section 6.2.

6.1 Conclusion
The work presented in this thesis has dealt with the problem of estimating the
motion and parameters of a vehicle, as well as representing and estimating its
surroundings. In this context the surroundings consist of other vehicles, station-
ary objects, and the road. Here, a major part of the work is not only the estima-
tion problem itself, but also the way in which to represent the environment, i.e.,
building maps of stationary objects and describing the size of other vehicles.
The second part of the thesis begins in Paper A with an overview of mapping
techniques used to describe a road and its closer environment. Four different
types of mapping philosophies are described; beginning with so called feature
based maps, where each element of the map both described the properties and
location of the specific map-element. In location based maps each map-element
already corresponds to a certain location and the value of the element describes
only the properties in that position. Hence, feature based maps are usually rather
sparse. Furthermore, road maps and intensity based maps are also summarized.
The intensity based map is the main topic of paper D.
Paper B is concerned with estimating the lane geometry. The lane markings are
described by a polynomial and the coefficients are the states to estimate. This

77
78 6 Concluding Remarks

problem can be solved with a camera and computer vision, but by fusing the
data obtained from the image processing with information about the ego vehi-
cle’s motion and the other vehicles’ movement on the road, the road geometry
estimate can be improved. The other vehicles are tracked primarily by using
measurements from a radar. The motion of the ego vehicle is estimated by com-
bining measurements from the vehicle’s imu, steering wheel angle sensor and
wheel velocity sensors in a model based filter. The model is in this case the so
called single track model or bicycle model.

Paper C and D both deal with the problems of estimating and representing sta-
tionary objects along the edges of a road. Paper C, associates the radar measure-
ments to extended stationary objects in the form of curved lines and tracks these
lines as extended targets. The lines are modeled as polynomials and one novelty
in this paper is that the approach leads to a so called errors in variables problem
when noisy point measurements are associated to polynomials. The standard ap-
proach is to assume that the measurements are only noisy in one direction when
associated to a line, here however noise is assumed to be present in all directions.
This assumption is of course more realistic and the results are more accurate.

Paper D describes a new type of map, the intensity based map. In this paper the
environment, i.e., the stationary objects, is represented by an intensity function
or probability hypothesis density (phd) as it is also called. The intensity function
is described by a Gaussian mixture. This makes the representation very sparse,
since only a number of sufficient statistics must be tracked, but also very rich
since the extension of the Gaussian components can cover and model larger areas.
It is shown how prior knowledge of the road construction can be used to both
improve the update of the filter and to simplify the map representation.

Paper E and F deal with the tracking of extended moving targets. In Paper E the
target is still modeled as a point target, however, it allows for obtaining more than
one measurement per target. A modification of the pseudo-likelihood function
used in the update step of the so called phd filter is presented. In the standard
filter one measurement can only be obtained from each target per time step. The
novelty of the paper is the practical implementation of the pseudo-likelihood
function and partitioning of the measurement data. In a number of simulations
and experiments the advantages of the modifications are shown. A drawback is
that the wrong number of targets is estimated in some rare cases, this is further
discussed in the next section outlining some possible directions of future work.

In Paper F, the size and the shape of an extended target are also estimated. It
is still assumed that the sensor obtains a number of point measurements. In or-
der to associate the point measurements to the shaped target it is assumed that
the measurements have been produced by a number of measurement generating
points on the surface of the target. The measurement generating points are repre-
sented by a random finite set and these are only used as a means to estimate the
size and the shape of the target. Only one simple simulation example is shown
and also here future work will cover more situations.
6.2 Future Research 79

The last paper, Paper G, aims at estimating the wheel radii of a vehicle. This is
made under the assumption that a change in the wheel radii is related to a change
in tire pressure, and that tire pressure losses can be detected using this approach.
The proposed method is only based on measurements from the wheel speed sen-
sors and the gps position information. The wheel radii are represented with noise
parameters in the state space model. The novelty lies in a Bayesian approach to
estimate these time-varying noise parameters on-line using a marginalized parti-
cle filter. Experimental results of the proposed approach show many advantages,
both regarding the accuracy and the reduced computational complexity, when it
is compared with other methods.
The approaches in Papers A-D, and G have been evaluated on real data from both
freeways and rural roads in Sweden. In Paper E data of moving pedestrians has
been used. Of the seven publications in Part II six are journal papers and one is
a peer-reviewed conference paper. Five of the papers are published and two are
still under review.

6.2 Future Research

The radar and camera data used in this thesis is generally preprocessed. How-
ever, the preprocessing is not covered in this thesis. Specifically, more effort can
be spent on the image processing to increase the information content. For exam-
ple within the area of odometry the estimate could be more accurate if the camera
information is used in addition to the imu measurements. This is called visual
odometry and it would probably improve the estimate of the body side slip angles,
especially during extreme maneuvers where the tire road interaction is strongly
nonlinear. Since only one camera is used, the inverse depth parametrization in-
troduced by Civera et al. (2008) is an interesting approach, see e.g., Schön and
Roll (2009); Nilsson et al. (2011) for automotive examples on visual odometry.
To verify the state estimates, more accurate reference values are needed as well.
Furthermore, the methods presented in Paper G could possibly be used as a ba-
sis to more accurately describe the road-tire interaction and thereby improve the
estimate of the slip of the vehicle.
Different aspects to estimate extended objects have been discussed in this thesis,
however an efficient solution on how to estimate the shape of any possible objects
has not been found. Paper F, is closest to this goal, but more experimental studies
must be performed and the computational time must be reduced by finding more
efficient forms of representation. Also, as concluded in Paper E, the number of
targets is falsely estimated in some rare cases, e.g., in the case of occlusion or not
well separated targets. In that specific case, the so called cardinalized phd filter
(cphd) is assumed to improve the results drastically, see Orguner et al. (2011).
However, more experimental results must be obtained before conclusions can be
drawn.
Currently there is a lot of activity within the computer vision community to en-
able non-planar road models, making use of parametric road models similar to
80 6 Concluding Remarks

the ones used in this thesis. A very interesting avenue for future work is to com-
bine the ideas presented in this thesis with information from a camera about the
height differences on the road side within a sensor fusion framework. This would
probably improve the estimates, especially in situations when there are too few
radar measurements available.
 Bibliography

A. Abdulle and G. Wanner. 200 years of least squares method. Elemente der
Mathematik, 57:45–60, May 2002.

M. Adams, W. S. Wijesoma, and A. Shacklock. Autonomous navigation: Achieve-

ments in complex environments. IEEE Instrumentation & Measurement Mag-
azine, 10(3):15–21, June 2007.

M. Ahrholdt, F. Bengtsson, L. Danielsson, and C. Lundquist. SEFS – results on sen-

sor data fusion system development. In Proceedings of the World Congress on
Intelligent Transportation Systems and Services, Stockholm, Sweden, Septem-
ber 2009.

B. D. O. Anderson and J. B. Moore. Optimal Filtering. Information and system

science series. Prentice Hall, Englewood Cliffs, NJ, USA, 1979.

D. Angelova and L. Mihaylova. Extended object tracking using Monte Carlo meth-
ods. IEEE Transactions on Signal Processing, 56(2):825–832, February 2008.

T. Ardeshiri, F. Larsson, F. Gustafsson, T. B. Schön, and M. Felsberg. Bicycle track-

ing using ellipse extraction. In Proceedings of the International Conference on
Information Fusion, pages 1–8, Chicago, IL, USA, July 2011.

M. S. Arulampalam, S. Maskell, N. Gordon, and T. Clapp. A tutorial on particle

filters for online nonlinear/non-Gaussian Bayesian tracking. IEEE Transactions
on Signal Processing, 50(2):174–188, February 2002.

T. Bailey and H. Durrant-Whyte. Simultaneous localization and mapping (SLAM):

Part II. IEEE Robotics & Automation Magazine, 13(3):108–117, September
2006.

Y. Bar-Shalom and T. E. Fortmann. Tracking and Data Association. Mathematics

in science and engineering. Academic Press, Orlando, FL, USA, 1988.

Y. Bar-Shalom and E. Tse. Tracking in a cluttered environment with probabilistic

data association. Automatica, 11(5):451–460, September 1975.

81
82 Bibliography

Y. Bar-Shalom, X. Rong Li, and T. Kirubarajan. Estimation with Applications to

Tracking and Navigation. John Wiley & Sons, New York, NY, USA, 2001.

M. Baum and U. D. Hanebeck. Random hypersurface models for extended object

tracking. In IEEE International Symposium on Signal Processing and Infor-
mation Technology, pages 178–183, Ajman, United Arab Emirates, December
2009.

M. Baum and U. D. Hanebeck. Shape tracking of extended objects and group

targets with star-convex RHMs. In Proceedings of the International Conference
on Information Fusion, pages 338–345, Chicago, IL, USA, July 2011.

M. Baum, B. Noack, and U. D. Hanebeck. Extended object and group tracking

with elliptic random hypersurface models. In Proceedings of the International
Conference on Information Fusion, pages 1–8, Edinburgh, UK, July 2010.

T. Bayes. An essay towards solving a problem in the doctrine of chances. The

Philosophical Transactions, 53:370–418, 1763.

R. Behringer. Visuelle Erkennung und Interpretation des Fahrspurverlaufes

durch Rechnersehen für ein autonomes Straßenfahrzeug, volume 310 of
Fortschrittsberichte VDI, Reihe 12. VDI Verlag, Düsseldorf, Germany, 1997.
Also as: PhD Thesis, Universität der Bundeswehr, 1996.

F. Bengtsson. Models for tracking in automotive safety systems. Licentiate Thesis

No R012/2008, Department of Signals and Systems, Chalmers University of
Technology, 2008.

F. Bengtsson and L. Danielsson. Designing a real time sensor data fusion system
with application to automotive safety. In Proceedings of the World Congress
on Intelligent Transportation Systems and Services, New York, USA, November
2008.

D. P. Bertsekas. The auction algorithm for assignment and other network flow
problem: a tutorial. Interfaces, 20(4):133–149, July 1990.

S. S. Blackman and R. Popoli. Design and Analysis of Modern Tracking Systems.

Artech House, Norwood, MA, USA, 1999.

Y. Boers, H. Driessen, J. Torstensson, M. Trieb, R. Karlsson, and F. Gustafsson.

Track-before-detect algorithm for tracking extended targets. IEE Proceedings
of Radar, Sonar and Navigation, 153(4):345–351, August 2006.

M. Bühren and B. Yang. Simulation of automotive radar target lists using a novel
approach of object representation. In Proceedings of the IEEE Intelligent Vehi-
cles Symposium, pages 314–319, Tokyo, Japan, 2006.

O. Cappe, S. J. Godsill, and E. Moulines. An overview of existing methods and

recent advances in sequential Monte Carlo. Proceedings of the IEEE, 95(5):
899–924, May 2007.
Bibliography 83

S. Challa, M. R. Morelande, D. Musicki, and R. J. Evans. Fundamentals of Object

Tracking. Cambridge University Press, Cambridge, UK, 2011.
J. Civera, A. J. Davison, and J. Montiel. Inverse depth parametrization for monoc-
ular SLAM. IEEE Transactions on Robotics, 24(5):932–945, October 2008.
D. E. Clark. First-moment multi-object forward-backward smoothing. In Pro-
ceedings of the International Conference on Information Fusion, pages 1–6,
Edinburgh, UK, July 2010.
D. J. Daley and D. Vere-Jones. An introduction to the theory of point processes.
Vol. 1 , Elementary theory and method. Springer, New York, NY, USA, 2 edition,
2003.
L. Danielsson. Tracking Theory for Preventive Safety Systems. Licentiate Thesis
No R004/2008, Department of Signals and Systems, Chalmers University of
Technology, 2008.
L. Danielsson. Tracking and radar sensor modelling for automotive safety sys-
tems. PhD thesis No 3064, Department of Signals and Systems, Chalmers Uni-
versity of Technology, 2010.
J. Degerman, J. Wintenby, and D. Svensson. Extended target tracking using prin-
cipal components. In Proceedings of the International Conference on Informa-
tion Fusion, pages 1–8, Chicago, IL, USA, July 2011.
J. C. Dezert. Tracking maneuvering and bending extended target in cluttered
environment. In Proceedings of Signal and Data Processing of Small Targets,
volume 3373, pages 283–294, Orlando, FL, USA, April 1998. SPIE.
E. D. Dickmanns. Dynamic Vision for Perception and Control of Motion.
Springer, London, UK, 2007.
P. M. Djuric, J. H. Kotecha, J. Zhang, Y. Huang, T. Ghirmai, M. F. Bugallo, and
J. Miguez. Particle filtering. Signal Processing Magazine, IEEE, 20(5):19–38,
September 2003.
A. Doucet, S. J. Godsill, and C. Andrieu. On sequential Monte Carlo sampling
methods for Bayesian filtering. Statistics and Computing, 10(3):197–208, 2000.
A. Doucet, N. de Freitas, and N. Gordon, editors. Sequential Monte Carlo Meth-
ods in Practice. Springer Verlag, New York, USA, 2001.
O. E. Drummond, S. S. Blackman, and G. C. Pretrisor. Tracking clusters and
extended objects with multiple sensors. In Proceedings of Signal and Data
Processing of Small Targets, volume 1305, pages 362–375, Orlando, FL, USA,
January 1990. SPIE.
H. Durrant-Whyte and T. Bailey. Simultaneous localization and mapping (SLAM):
Part I. IEEE Robotics & Automation Magazine, 13(2):99–110, June 2006.
A. Eidehall. Tracking and threat assessment for automotive collision avoidance.
84 Bibliography

PhD thesis No 1066, Linköping Studies in Science and Technology, Linköping,

Sweden, January 2007.
A. Eidehall and F. Gustafsson. Obtaining reference road geometry parameters
from recorded sensor data. In Proceedings of the IEEE Intelligent Vehicles
Symposium, pages 256–260, Tokyo, Japan, June 2006.
A. Eidehall, J. Pohl, and F. Gustafsson. Joint road geometry estimation and vehicle
tracking. Control Engineering Practice, 15(12):1484–1494, December 2007.
O. Erdinc, P. Willett, and Y. Bar-Shalom. The bin-occupancy filter and its con-
nection to the PHD filters. IEEE Transactions on Signal Processing, 57(11):
4232–4246, November 2009.
M. Feldmann, D. Fränken, and W. Koch. Tracking of extended objects and group
targets using random matrices. IEEE Transactions on Signal Processing, 59(4):
1409–1420, April 2011.
R. A. Fisher. On an absolute criterion for fitting frequency curves. Messenger of
Mathematics, 41:155–160, 1912.
R. A. Fisher. On the mathematical foundations of theoretical statistics. Philo-
sophical Transactions of the Royal Society Series A, 222:309–368, 1922.
T. Fortmann, Y. Bar-Shalom, and M. Scheffe. Sonar tracking of multiple targets
using joint probabilistic data association. IEEE Journal of Ocean Engineering,
8(3):173–184, July 1983.
A. Gern, U. Franke, and P. Levi. Advanced lane recognition - fusing vision and
radar. In Proceedings of the IEEE Intelligent Vehicles Symposium, pages 45–51,
Dearborn, MI, USA, October 2000.
A. Gern, U. Franke, and P. Levi. Robust vehicle tracking fusing radar and vision.
In Proceedings of the international conference of multisensor fusion and inte-
gration for intelligent systems, pages 323–328, Baden-Baden, Germany, August
2001.
K. Gilholm and D. Salmond. Spatial distribution model for tracking extended
objects. IEE Proceedings of Radar, Sonar and Navigation, 152(5):364–371, Oc-
tober 2005.
K. Gilholm, S. Godsill, S. Maskell, and D. Salmond. Poisson models for extended
target and group tracking. In Proceedings of Signal and Data Processing of
Small Targets, volume 5913, pages 230–241, San Diego, CA, USA, August 2005.
SPIE.
T. D. Gillespie. Fundamentals of Vehicle Dynamics. SAE Society of Automotive
Engineers, Warrendale, PA, USA, 1992.
I. R. Goodman, R. P. S. Mahler, and H. T. Nguyen. Mathematics of data fusion.
Kluwer Academic, Dordrecht, 1997.
Bibliography 85

N. J. Gordon, D. J. Salmond, and A. F. M. Smith. Novel approach to

nonlinear/non-Gaussian Bayesian state estimation. IEE Proceedings on Radar
and Signal Processing, 140(5):107–113, April 1993.
K. Granström, C. Lundquist, and U Orguner. A Gaussian mixture PHD filter for
extended target tracking. In Proceedings of the International Conference on
Information Fusion, Edinburgh, UK, July 2010.
K. Granström, C. Lundquist, and U. Orguner. Extended target tracking using a
Gaussian-mixture PHD filter. IEEE Transactions on Aerospace and Electronic
Systems, 2011a. Under review.
K. Granström, C. Lundquist, and U Orguner. Tracking rectangular and elliptical
extended targets using laser measurements. In Proceedings of the International
Conference on Information Fusion, Chicago, IL, USA, July 2011b.
J. Gunnarsson. Models and Algorithms - with applications to vehicle tracking
and frequency estimation. PhD thesis No 2628, Department of Signals and
Systems, Chalmers University of Technology, June 2007.
J. Gunnarsson, L. Svensson, E Bengtsson, and L. Danielsson. Joint driver inten-
tion classification and tracking of vehicles. In IEEE Nonlinear Statistical Signal
Processing Workshop, pages 95–98, September 2006.
J. Gunnarsson, L. Svensson, L. Danielsson, and F. Bengtsson. Tracking vehicles
using radar detections. In Proceedings of the IEEE Intelligent Vehicles Sympo-
sium, pages 296–302, Istanbul, Turkey, June 2007.
F. Gustafsson. Adaptive Filtering and Change Detection. John Wiley & Sons, New
York, USA, 2000.
F. Gustafsson. Automotive safety systems. IEEE Signal Processing Magazine, 26
(4):32–47, July 2009.
F. Gustafsson. Particle filter theory and practice with positioning applications.
IEEE Transactions on Aerospace and Electronic Systems, 25(7):53–82, July
2010.
G. Hendeby. Performance and Implementation Aspects of Nonlinear Filtering.
PhD thesis No 1161, Linköping Studies in Science and Technology, Linköping,
Sweden, February 2008.
J. Jansson. Collision Avoidance Theory with Applications to Automotive Colli-
sion Mitigation. PhD thesis No 950, Linköping Studies in Science and Technol-
ogy, Linköping, Sweden, June 2005.
A. H. Jazwinski. Stochastic processes and filtering theory. Mathematics in science
and engineering. Academic Press, New York, USA, 1970.
K. H. Johansson, M. Törngren, and L. Nielsen. Vehicle applications of controller
area network. In D. Hristu-Varsakelis and W. S. Levine, editors, Handbook of
Networked and Embedded Control Systems, pages 741–765. Birkhäuser, 2005.
86 Bibliography

S. J. Julier. The scaled unscented transformation. In Proceedings of the American

Control Conference, volume 6, pages 4555–4559, 2002.

S. J. Julier and J. K. Uhlmann. New extension of the Kalman filter to nonlinear sys-
tems. In Signal Processing, Sensor Fusion, and Target Recognition VI, volume
3068, pages 182–193, Orlando, FL, USA, 1997. SPIE.

S. J. Julier and J. K. Uhlmann. Reduced sigma point filters for the propagation of
means and covariances through nonlinear transformations. In Proceedings of
the American Control Conference, volume 2, pages 887–892, 2002.

S. J. Julier and J. K. Uhlmann. Unscented filtering and nonlinear estimation. Pro-

ceedings of the IEEE, 92(3):401–422, March 2004.

S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte. A new approach for filtering

nonlinear systems. In American Control Conference, 1995. Proceedings of the,
volume 3, pages 1628–1632, Jun 1995.

T. Kailath. Linear systems. Prentice Hall, Englewood Cliffs, NJ, USA, 1980.

T. Kailath, A. H. Sayed, and B. Hassibi. Linear Estimation. Information and

System Sciences Series. Prentice Hall, Upper Saddle River, NJ, USA, 2000.

R. E. Kalman. A new approach to linear filtering and prediction problems. Trans-

actions of the ASME, Journal of Basic Engineering, 82:35–45, 1960.

R. Karlsson. Particle Filtering for Positioning and Tracking Applications. PhD

thesis No 924, Linköping Studies in Science and Technology, Linköping, Swe-
den, March 2005.

S. M. Kay. Fundamentals of Statistical Signal Processing, Volume I: Estimation

Theory. Prentice Hall Signal Processing. Prentice Hall, Upper Saddle River, NJ,
USA, 1993.

U. Kiencke, S. Dais, and M. Litschel. Automotive serial controller area network.

Technical Report 860391, SAE International Congress, 1986.

J. W. Koch. Bayesian approach to extended object and cluster tracking using

random matrices. IEEE Transactions on Aerospace and Electronic Systems, 44
(3):1042–1059, July 2008.

M. E. Liggins, D. L. Hall, and J. Llinas. Handbook of multisensor data fusion :

theory and practice. CRC Press, Boca Raton, FL, USA, 2 edition, 2009.

L. Lin, Y. Bar-Shalom, and T. Kirubarajan. Data association combined with the

probability hypothesis density filter for multitarget tracking. In Proceedings
of Signal and Data Processing of Small Targets, volume 5428, pages 464–475,
Orlando, FL, USA, 2004. SPIE.

L. Ljung. System identification, Theory for the user. System sciences series. Pren-
tice Hall, Upper Saddle River, NJ, USA, 2 edition, 1999.
Bibliography 87

C. Lundquist. Automotive Sensor Fusion for Situation Awareness. Licentiate

Thesis No 1422, Department of Electrical Engineering, Linköping University,
Sweden, 2009.

C. Lundquist. Method for stabilizing a vehicle combination. U.S.

Patent US 8010253 August 30, 2011 and German Patent Application
DE 102007008342 August 21, 2008, Priority date February 20, 2007.

C. Lundquist and R. Großheim. Method and device for determining steering an-
gle information. German Patent Application DE 10 2007 000 958 Mai 14, 2009,
International Patent Application WO 2009 047 020 April 16, 2009 and Euro-
pean Patent Application EP 2205478 April 16, 2009, Priority date October 2,
2007.

C. Lundquist and W. Reinelt. Back driving assistant for passenger cars with
trailer. In Proceedings of the SAE World Congress, SAE paper 2006-01-0940,
Detroit, MI, USA, April 2006a.

C. Lundquist and W. Reinelt. Rückwärtsfahrassistent für PKW mit Aktive Front

Steering. In Proceedings of the AUTOREG (Steuerung und Regelung von
Fahrzeugen und Motoren, VDI Bericht 1931, pages 45–54, Wiesloch, Germany,
March 2006b.

C. Lundquist and W. Reinelt. Electric motor rotor position monitoring method

for electrically aided steering system e.g. steer by wire, for motor vehicle,
involves outputting alarm when difference between measurement value and
estimated value of motor exceeds threshold. German Patent Application
DE 102005016514 October 12, 2006, Priority date April 8, 2006.

C. Lundquist and T. B. Schön. Road geometry estimation and vehicle tracking

using a single track model. In Proceedings of the IEEE Intelligent Vehicles
Symposium, pages 144–149, Eindhoven, The Netherlands, June 2008.

C. Lundquist and T. B. Schön. Estimation of the free space in front of a moving

vehicle. In Proceedings of the SAE World Congress, SAE paper 2009-01-1288,
Detroit, MI, USA, April 2009a.

C. Lundquist and T. B. Schön. Recursive identification of cornering stiffness pa-

rameters for an enhanced single track model. In Proceedings of the IFAC Sym-
posium on System Identification, pages 1726–1731, Saint-Malo, France, July
2009b.

C. Lundquist and T. B. Schön. Joint ego-motion and road geometry estimation.

Information Fusion, 12:253–263, October 2011.

C. Lundquist, U. Orguner, and T. B. Schön. Tracking stationary extended objects

for road mapping using radar measurements. In Proceedings of the IEEE In-
telligent Vehicles Symposium, pages 405–410, Xi’an, China, June 2009.

C. Lundquist, L. Danielsson, and F. Gustafsson. Random set based road mapping

88 Bibliography

using radar measurements. In Proceedings of the European Signal Processing

Conference, pages 219–223, Aalborg, Denmark, August 2010a.
C. Lundquist, U. Orguner, and F Gustafsson. Estimating polynomial structures
from radar data. In Proceedings of the International Conference on Informa-
tion Fusion, Edinburgh, UK, July 2010b.
C. Lundquist, K. Granström, and U. Orguner. Estimating the shape of targets with
a PHD filter. In Proceedings of the International Conference on Information
Fusion, Chicago, IL, USA, July 2011a.
C. Lundquist, L. Hammarstrand, and F. Gustafsson. Road intensity based map-
ping using radar measurements with a probability hypothesis density filter.
IEEE Transactions on Signal Processing, 59(4):1397–1408, April 2011b.
C. Lundquist, U. Orguner, and F. Gustafsson. Extended target tracking using
polynomials with applications to road-map estimation. IEEE Transactions on
Signal Processing, 59(1):15–26, January 2011c.
C. Lundquist, E. Özkan, and F. Gustafsson. Tire radii estimation using a marginal-
ized particle filter. IEEE Transactions on Intelligent Transportation Systems,
2011d. Submitted.
C. Lundquist, T. B. Schön, and F. Gustafsson. Situational awareness and road
prediction for trajectory control applications. In A. Eskandarian, editor, Hand-
book of Intelligent Vehicles, chapter 24. Springer, November 2011e.
C. Lundquist, M. Skoglund, K. Granström, and T. Glad. How peer-review affects
student learning. In Utvecklingskonferens för Sveriges ingenjörsutbildingar,
Norrköping, Sweden, November 2011f.
R. P. S. Mahler. Multitarget Bayes filtering via first-order multitarget moments.
IEEE Transactions on Aerospace and Electronic Systems, 39(4):1152–1178, Oc-
tober 2003.
R. P. S. Mahler. Statistics 101 for multisensor, multitarget data fusion. IEEE
Transactions on Aerospace and Electronic Systems, 19(1):53–64, January 2004.
R. P. S. Mahler. Statistical Multisource-Multitarget Information Fusion. Artech
House, Boston, MA, USA, 2007a.
R. P. S. Mahler. PHD filters of higher order in target number. IEEE Transactions
on Aerospace and Electronic Systems, 43(4):1523–1543, October 2007b.
R. P. S. Mahler. PHD filters for nonstandard targets, I: Extended targets. In
Proceedings of the International Conference on Information Fusion, pages 915–
921, Seattle, WA, USA, July 2009a.
R. P. S. Mahler. Random set theory for multisource-multitarget information fu-
sion. In M. E. Liggins, D. L. Hall, and J. Llinas, editors, Handbook of multisen-
sor data fusion: theory and practice, chapter 16. CRC Press, Boca Raton, FL, 2
edition, November 2009b.
Bibliography 89

R. P. S. Mahler, B.-N. Vo, and B.-T. Vo. The forward-backward probability hy-
pothesis density smoother. In Proceedings of the International Conference on
Information Fusion, pages 1–8, Edinburgh, UK, July 2010.
S. Malinen, C. Lundquist, and W. Reinelt. Fault detection of a steering wheel
sensor signal in an active front steering system. In Preprints of the IFAC Sym-
posium on SAFEPROCESS, pages 547–552, Beijing, China, August 2006.
M. Mitschke and H. Wallentowitz. Dynamik der Kraftfahrzeuge. Springer, Berlin,
Heidelberg, 4 edition, 2004.
E. Nilsson, C. Lundquist, T. B. Schön, D. Forslund, and J. Roll. Vehicle motion
estimation using an infrared camera. In Proceedings of the World Congress of
the International Federation of Automatic Control, Milan, Italy, August 2011.
U. Orguner, C. Lundquist, and K. Granström. Extended target tracking with a
cardinalized probability hypothesis density filter. In Proceedings of the In-
ternational Conference on Information Fusion, pages 65–72, Chicago, IL, USA,
July 2011.
E. Özkan, C. Lundquist, and F. Gustafsson. A Bayesian approach to jointly esti-
mate tire radii and vehicle trajectory. In Proceedings of the IEEE Conference
on Intelligent Transportation Systems, Washington DC, USA, October 2011.
K. Panta, B.-N. Vo, S. Singh, and A. Doucet. Probability hypothesis density filter
versus multiple hypothesis tracking. In Signal Processing, Sensor Fusion, and
Target Recognition XIII, volume 5429, pages 284–295, Orlando, FL, USA, 2004.
SPIE.
R. Rajamani. Vehicle Dynamics and Control. Springer, Boston, MA, USA, 2006.
ISBN 978-0-387-28823-9.
G. Reimann and C. Lundquist. Method for operating electronically controlled
servo steering system of motor vehicle, involves determining steering wheel
angle as measure for desired steering handle angle by steering handle for steer-
ing wheels of motor vehicle. German Patent Application DE 102006053029
May 15, 2008, Priority date November 10, 2006.
W. Reinelt and C. Lundquist. Observer based sensor monitoring in an active front
steering system using explicit sensor failure modeling. In Proceedings of the
IFAC World Congress, Prague, Czech Republic, July 2005.
W. Reinelt and C. Lundquist. Mechatronische Lenksysteme: Modellbildung und
Funktionalität des Active Front Steering. In R. Isermann, editor, Fahrdy-
namik Regelung - Modellbildung, Fahrassistenzsysteme, Mechatronik, pages
213–236. Vieweg Verlag, September 2006a.
W. Reinelt and C. Lundquist. Controllability of active steering system hazards:
From standards to driving tests. In Juan R. Pimintel, editor, Safety Critical
Automotive Systems, pages 173–178. SAE International, 400 Commonwealth
Drive, Warrendale, PA, USA, August 2006b.
90 Bibliography

W. Reinelt and C. Lundquist. Method for assisting the driver of a motor vehicle
with a trailer when reversing. German Patent DE 10 2006 002 294 February 24,
2011, European Patent Application EP 1810913 July 25, 2007 and Japanese
Patent Application JP 2007191143 August 2, 2007, Priority date January 18,
2006.

W. Reinelt, W. Klier, G. Reimann, C. Lundquist, W. Schuster, and R. Großheim.

Active front steering for passenger cars: System modelling and functions.
In Proceedings of the IFAC Symposium on Advances in Automotive Control,
Salerno, Italy, April 2004.

W. Reinelt, C. Lundquist, and H. Johansson. On-line sensor monitoring in an

active front steering system using extended Kalman filtering. In Proceedings
of the SAE World Congress, SAE paper 2005-01-1271, Detroit, MI, USA, April
2005.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Motor vehicle’s electron-

ically regulated servo steering system operating method, involves comparing
actual value of measured value with stored practical value of corresponding
measured value. German Patent DE 10 2006 040 443 January 27, 2011, Prior-
ity date August 29, 2006.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Operating method for

electronic servo steering system of vehicle, involves presetting steering wheel
angle by steering mechanism as measure for desired wheel turning angle for
steering wheel of vehicle. German Patent Application DE 102006052092 May 8,
2008, Priority date November 4, 2006.

W. Reinelt, C. Lundquist, and S. Malinen. Automatic generation of a computer

program for monitoring a main program to provide operational safety. German
Patent Application DE 102005049657 April 19, 2007, Priority date October 18,
2005.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Electronic servo steer-

ing system operating method for motor vehicle, involves recognizing track mis-
alignment of vehicle when forces differentiate around preset value from each
other at preset period of time in magnitude and/or direction. German Patent
DE 102006043069 December 3, 2009, Priority date September 14, 2006.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Operating method for

electronic power-assisted steering system of vehicle, involves overlapping addi-
tional angle, which is disabled after re-start of utility function. German Patent
Application DE 102006041236 Mars 6, 2008, Priority date September 2, 2006a.

W. Reinelt, W. Schuster, R. Großheim, and C. Lundquist. Operating method for

electronic power-assisted steering system of vehicle, involves re-starting utility
function, and after re-start of utility function superimposition of additional an-
gle is unlatched. German Patent DE 102006041237 December 3, 2009, Priority
date September 2, 2006b.
Bibliography 91

B. Ristic and D. J. Salmond. A study of a nonlinear filtering problem for tracking

an extended target. In Proceedings of the International Conference on Infor-
mation Fusion, pages 503–509, Stockholm, Sweden, June 2004.
B. Ristic, S. Arulampalam, and N. Gordon. Beyond the Kalman Filter: Particle
filters for tracking applications. Artech House, London, UK, 2004.
X. Rong Li and V. P. Jilkov. Survey of maneuvering target tracking: Part III. Mea-
surement models. In Proceedings of Signal and Data Processing of Small Tar-
gets, volume 4473, pages 423–446, San Diego, CA, USA, 2001. SPIE.
X. Rong Li and V. P. Jilkov. Survey of maneuvering target tracking: Part I. Dy-
namic models. IEEE Transactions on Aerospace and Electronic Systems, 39(4):
1333–1364, October 2003.
M. Roth and F. Gustafsson. An efficient implementation of the second order ex-
tended Kalman filter. In Proceedings of the International Conference on Infor-
mation Fusion, pages 1–6, July 2011.
W. J. Rugh. Linear System Theory. Information and system sciences series. Pren-
tice Hall, Upper Saddle River, NJ, USA, 2 edition, 1996.
D. J. Salmond and M. C. Parr. Track maintenance using measurements of tar-
get extent. IEE Proceedings of Radar, Sonar and Navigation, 150(6):389–395,
December 2003.
S. F. Schmidt. Application of state-space methods to navigation problems. Ad-
vances in Control Systems, 3:293–340, 1966.
B. Schofield. Model-Based Vehicle Dynamics Control for Active Safety. PhD
thesis, Department of Automatic Control, Lund University, Sweden, September
2008.
T. B. Schön. Estimation of Nonlinear Dynamic Systems – Theory and Applica-
tions. PhD thesis No 998, Linköping Studies in Science and Technology, De-
partment of Electrical Engineering, Linköping University, Sweden, February
2006.
T. B. Schön and J. Roll. Ego-motion and indirect road geometry estimation using
night vision. In Proceedings of the IEEE Intelligent Vehicles Symposium, pages
30–35, Xi’an, China, June 2009.
T. B. Schön, F. Gustafsson, and P.-J. Nordlund. Marginalized particle filters for
mixed linear/nonlinear state-space models. IEEE Transactions on Signal Pro-
cessing, 53(7):2279–2289, July 2005.
T. B. Schön, A. Eidehall, and F. Gustafsson. Lane departure detection for im-
proved road geometry estimation. In Proceedings of the IEEE Intelligent Vehi-
cles Symposium, pages 546–551, Tokyo, Japan, June 2006.
T. B. Schön, D. Törnqvist, and F. Gustafsson. Fast particle filters for multi-rate
92 Bibliography

sensors. In Proceedings of the 15th European Signal Processing Conference,

Poznań, Poland, September 2007.
H. Sidenbladh. Multi-target particle filtering for the probability hypothesis den-
sity. In Proceedings of the International Conference on Information Fusion,
volume 2, pages 800–806, Cairns, Australia, March 2003.
G. L. Smith, S. F. Schmidt, and L. A. McGee. Application of statistical filter the-
ory to the optimal estimation of position and velocity on board a circumlunar
vehicle. Technical Report TR R-135, NASA, 1962.
J. Sörstedt, L. Svensson, F. Sandblom, and L. Hammarstrand. A new vehicle mo-
tion model for improved predictions and situation assessment. IEEE Trans-
actions on Intelligent Transportation Systems, 2011. ISSN 1524-9050. doi:
10.1109/TITS.2011.2160342.
R. L. Streit. Multisensor multitarget intensity filter. In Proceedings of the In-
ternational Conference on Information Fusion, pages 1–8, Cologne, Germany,
July 2008.
R. L. Streit. Poisson Point Processes. Springer, 1 edition, 2010.
R. L. Streit and L. D. Stone. Bayes derivation of multitarget intensity filters. In
Proceedings of the International Conference on Information Fusion, pages 1–8,
Cologne, Germany, July 2008.
L. Svensson and J. Gunnarsson. A new motion model for tracking of vehicles. In
Proceedings of the 14th IFAC Symposium on System Identification, Newcastle,
Australia, 2006.
A. Swain and D. Clark. Extended object filtering using spatial independent clus-
ter processes. In Proceedings of the International Conference on Information
Fusion, pages 1–8, Edinburgh, UK, July 2010.
S. Thrun. Robotic mapping: A survey. In Exploring Artificial Intelligence in the
New Millenium. Morgan Kaufmann, 2002.
H. L. van Trees. Detection, Estimation, and Modulation Theory. John Wiley &
Sons, New York, NY, USA, 1968.
J. Vermaak, N. Ikoma, and S. J. Godsill. Sequential Monte Carlo framework for
extended object tracking. IEE Proceedings of Radar, Sonar and Navigation, 152
(5):353–363, October 2005.
B.-N. Vo and W.-K. Ma. The Gaussian mixture probability hypothesis density
filter. IEEE Transactions on Signal Processing, 54(11):4091–4104, November
2006.
B.-N. Vo, S. Singh, and A. Doucet. Random finite sets and sequential Monte Carlo
methods in multi-target tracking. In Proceedings of the International Radar
Conference, pages 486–491, Adelaide, Australia, September 2003.
Bibliography 93

B.-N. Vo, B.-T. Vo, and R. P. S. Mahler. A closed form solution to the probability
hypothesis density smoother. In Proceedings of the International Conference
on Information Fusion, pages 1–8, Edinburgh, UK, July 2010.
B.-N. Vo, B.-T. Vo, and R. P. S. Mahler. Closed form solutions to forward-backward
smoothing. IEEE Transactions on Signal Processing, 2011. doi: 10.1109/TSP.
2011.2168519.
B.-T. Vo, B.-N. Vo, and A. Cantoni. Analytic implementations of the cardinalized
probability hypothesis density filter. IEEE Transactions on Signal Processing,
55(7):3553–3567, July 2007.
N. Wahlström, J. Callmer, and F. Gustafsson. Single target tracking using vector
magnetometers. In IEEE Conference on Acoustics, Speech and Signal Process-
ing, pages 4332–4335, Prague, Czech Republic, May 2011.
A. Wald. Sequential tests of statistical hypotheses. The Annals of Mathematical
Statistics, 16(2):117–186, June 1945.
M. J. Waxman and O. E. Drummond. A bibliography of cluster (group) tracking.
In Proceedings of Signal and Data Processing of Small Targets, volume 5428,
pages 551–560, Orlando, FL, USA, April 2004. SPIE.
M. Wieneke and S. J. Davey. Histogram PMHT with target extent estimates based
on random matrices. In Proceedings of the International Conference on Infor-
mation Fusion, pages 1–8, Chicago, IL, USA, July 2011.
J. Y. Wong. Theory Of Ground Vehicles. John Wiley & Sons, New York, USA, 3
edition, 2001.
T. Zajic and R. P. S. Mahler. Particle-systems implementation of the PHD
multitarget-tracking filter. In Signal Processing, Sensor Fusion, and Target
Recognition XII, volume 5096, pages 291–299, Orlando, FL, USA, April 2003.
SPIE.
Z. Zomotor and U. Franke. Sensor fusion for improved vision based lane recogni-
tion and object tracking with range-finders. In Proceedings of the IEEE Confer-
ence on Intelligent Transportation Systems, pages 595–600, Boston, MA, USA,
November 1997.