Untersuchungen zur kooperativen Fahrzeuglokalisierungin

Fakultät für Elektrotechnik und Informationstechnik
Professur für Nachrichtentechnik
Diplomarbeit
Untersuchungen zur kooperativen Fahrzeuglokalisierung
in dezentralen Sensornetzen
Marcus Obst
Chemnitz, den 5. Februar 2009
Prüfer:
Prof. Dr.-Ing. Gerd Wanielik
Betreuer: Dipl.-Ing. Eric Richter
Obst, Marcus
Untersuchungen zur kooperativen Fahrzeuglokalisierung
in dezentralen Sensornetzen
Diplomarbeit, Fakultät für Elektrotechnik und Informationstechnik
Technische Universität Chemnitz, Februar 2009
Zusammenfassung
Die dynamische Schätzung der Fahrzeugposition durch Sensordatenfusion ist eine der
grundlegenden Aufgaben für moderne Verkehrsanwendungen wie zum Beispiel fahrerlose Transportsysteme oder Pre-Crash-Sicherheitssysteme.
In dieser Arbeit wird ein Verfahren zur dezentralen kooperativen Fahrzeuglokalisierung
vorgestellt, das auf einer allgemeinen Methode zur Fusion von Informationen mehrerer
Teilnehmer beruht. Sowohl die lokale als auch die übertragene Schätzung wird durch
Partikel dargestellt.
Innerhalb einer Simulation wird gezeigt, dass sich die Positionsschätzung der einzelnen
Teilnehmer im Netzwerk im Vergleich zu einer reinen GPS-basierten Lösung verbessert.
Inhaltsverzeichnis
Abbildungsverzeichnis
v
Tabellenverzeichnis
vii
Abkürzungsverzeichnis
ix
Verwendete Formelzeichen
xi
1 Einleitung
1.1 Motivation . . . . . .
1.2 Ziel der Arbeit . . .
1.3 Stand der Forschung
1.4 Aufbau der Arbeit .
.
.
.
.
1
1
2
3
4
.
.
.
.
.
.
.
.
.
.
.
.
.
5
5
7
7
8
10
11
12
13
14
15
17
18
21
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
2 Grundlagen
2.1 Prinzip der Positionsschätzung unter Unsicherheit
2.2 Wahrscheinlichkeitstheorie . . . . . . . . . . . . .
2.2.1 Wahrscheinlichkeitsdichte . . . . . . . . .
2.2.2 Normalverteilung . . . . . . . . . . . . . .
2.2.3 Bedingte Wahrscheinlichkeit . . . . . . . .
2.2.4 Satz von Bayes . . . . . . . . . . . . . . .
2.3 Markovprozess . . . . . . . . . . . . . . . . . . . .
2.4 Bayes-Filter . . . . . . . . . . . . . . . . . . . . .
2.4.1 Chapman-Kolmogorov-Gleichung . . . . .
2.4.2 Berücksichtigung von Beobachtungen . . .
2.4.3 Kalman-Filter . . . . . . . . . . . . . . . .
2.5 Globales Navigationssatellitensystem . . . . . . .
2.6 Zustandsraum und Bewegungsmodell . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
3 Partikel-Filter
23
3.1 Grundgedanke . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
3.1.1 Repräsentation der Wahrscheinlichkeitsdichtefunktion . . . . . . 28
i
Inhaltsverzeichnis
3.2
3.3
3.4
3.5
Sequentielle Monte-Carlo-Methode . . . .
3.2.1 Monte-Carlo-Integration . . . . . .
3.2.2 Importance Sampling . . . . . . . .
3.2.3 Sequentielles Importance Sampling
3.2.4 Wahl der Proposal Density . . . . .
Resampling . . . . . . . . . . . . . . . . .
3.3.1 Resampling-Verfahren . . . . . . .
3.3.2 Weitere Verfahren und Probleme .
Bestimmung des Erwartungswertes . . . .
Varianten des Partikel-Filters . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
4 Dezentralisierte Datenfusion
4.1 Problembeschreibung . . . . . . . . . . . . . . . . . . . . . . . .
4.2 Covariance Intersection . . . . . . . . . . . . . . . . . . . . . . .
4.3 DDF im Partikel-Raum . . . . . . . . . . . . . . . . . . . . . . .
4.3.1 Umwandlung in eine kontinuierliche Verteilungsfunktion
4.3.2 Fusion durch Multiplikation und Division . . . . . . . . .
5 Umsetzung – CoVelLoc
5.1 Ground Truth-Simulation . . . . . . . . . . . . . . . . . . .
5.1.1 Parameter der Ground Truth-Simulation . . . . . . .
5.2 Filter und Modelle . . . . . . . . . . . . . . . . . . . . . . .
5.2.1 Zeitsynchronität . . . . . . . . . . . . . . . . . . . . .
5.2.2 Initialisierungsproblem . . . . . . . . . . . . . . . . .
5.2.3 Ablauf der klassischen Positionsschätzung . . . . . .
5.2.4 Ablauf der dezentralisierten Positionsschätzung . . .
5.3 Einarbeiten des Differenzvektors . . . . . . . . . . . . . . . .
5.3.1 Verschiebung einer Gaußverteilung unter Unsicherheit
5.3.2 Verschiebung der Parzenverteilung . . . . . . . . . .
5.3.3 Verschiebung im Partikel-Raum . . . . . . . . . . . .
5.3.4 Verschiebung im Partikel-Raum mit Sigma-Punkten .
5.3.5 Vergleich und Auswahl . . . . . . . . . . . . . . . . .
5.4 Vermeiden von Dateninzest . . . . . . . . . . . . . . . . . .
5.4.1 Zwei unabhängige Filter pro Fahrzeug . . . . . . . .
5.4.2 Teilweise kooperative Lokalisierung . . . . . . . . . .
5.4.3 DDF im Partikel-Raum . . . . . . . . . . . . . . . .
5.5 Adaptive Anzahl der Partikel . . . . . . . . . . . . . . . . .
ii
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
29
29
31
32
35
37
40
43
44
45
.
.
.
.
.
47
48
49
51
52
54
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
57
58
59
60
60
61
62
62
64
65
68
69
71
76
77
77
77
78
78
Inhaltsverzeichnis
6 Ergebnisse
6.1 Initialisierungsphase . . . . . . . . . . . . . . . . .
6.2 Klassische Positionsschätzung . . . . . . . . . . . .
6.3 Dezentralisierte Positionsschätzung . . . . . . . . .
6.3.1 Einfluss der Anzahl von Fahrzeugen . . . . .
6.3.2 Einfluss der Genauigkeit des Differnzvektors
6.4 Einfluss der simulierten GPS-Messung . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
81
81
83
86
86
89
89
7 Zusammenfassung
91
8 Schlussbemerkung und Ausblick
92
A Mathematische Gleichungen
93
A.1 Mehrdimensionale Parzen Density Estimation . . . . . . . . . . . . . . 93
B Matlab-Skripte
95
Literaturverzeichnis
97
iii
Abbildungsverzeichnis
1.1
Schematische Darstellung der kooperativen Fahrzeuglokalisierung. . . .
2
2.1
2.2
2.3
2.4
2.5
2.6
2.7
Prinzip der Positionsschätzung durch Sensordaten-Fusion. . . . . . . .
Wahrscheinlichkeit als Fläche unter der Dichtefunktion interpretiert. . .
Dichtefunktion der Standardnormalverteilung. . . . . . . . . . . . . . .
Schematische Darstellung eines Hidden Markov Modells erster Ordnung.
Das Kalman-Filter als optimale Lösung im Sinne von Bayes. . . . . . .
Positionsbestimmung eines GNSS. . . . . . . . . . . . . . . . . . . . . .
Darstellung der Komponenten des Zustandsvektors. . . . . . . . . . . .
6
9
9
13
18
19
21
3.1
3.2
3.3
24
25
3.4
3.5
3.6
3.7
3.8
3.9
Nichtlineare Transformation einer normalverteilten Zufallsvariable. . . .
Einordnung des Partikel-Filters innerhalb der Bayes-Filter. . . . . . . .
Beschreibung und Transformation einer Wahrscheinlichkeitsdichteverteilung durch Partikel . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Importance Sampling-Algorithmus . . . . . . . . . . . . . . . . . . . . .
Graphische Darstellung der Partikel-Degeneration. . . . . . . . . . . . .
Schematische Darstellung des Resampling-Schrittes. . . . . . . . . . . .
Selektionsschritt des Multinomialen Resampling. . . . . . . . . . . . . .
Systematisches Resampling anhand einer diskreten Verteilung. . . . . .
Ablaufdiagramm des allgemeinen Partikel-Filter-Algorithmus. . . . . .
27
33
37
39
41
42
46
4.1
4.2
4.3
4.4
Einfluss des Parameters ω auf den CI-Algorithmus. . . . . . . . . . . .
Intuitive Multiplikation zweier Partikel-Verteilungen . . . . . . . . . . .
Multiplikation zweier Partikel-Verteilungen. . . . . . . . . . . . . . . .
Einfluss der Bandbreite h auf das Ergebnis der Parzen Density Estimation.
50
51
52
55
5.1
5.2
5.3
5.4
Systemarchitektur der kooperativen Fahrzeuglokalisierung (CoVelLoc).
Ablauf der klassischen Positionsschätzung. . . . . . . . . . . . . . . . .
Verschiebung der marginalisierten Verteilung zwischen zwei Fahrzeugen
Verschiebung der kontinuierlichen gaußschen Schätzung um den gaußschen Differenzvektor . . . . . . . . . . . . . . . . . . . . . . . . . . . .
58
63
66
v
67
Abbildungsverzeichnis
5.5
Verschiebung einer Partikel-Verteilung durch Uncertain Parzen Density
Estimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5.6 Schematische Darstellung der Addition zweier Partikel-Verteilungen . .
5.7 Addition zweier Partikel-Verteilungen. . . . . . . . . . . . . . . . . . .
5.8 Beschreibung einer 1d-Normalverteilung durch Sigma-Punkte. . . . . .
5.9 Addition zweier Partikel-Verteilungen mit Sigma-Punkten. . . . . . . .
5.10 Baumartige Netztopologie . . . . . . . . . . . . . . . . . . . . . . . . .
6.1
6.2
6.3
6.4
6.5
6.6
6.7
Partikel-Verteilung nach der Initialisierung . . . . . . . . . . . . . . . .
Zeitliche Entwicklung der Partikel nach der Initialisierung. . . . . . . .
Geschätzte Position und Ground Truth eines Fahrzeuges auf Basis der
klassischen Positionsbestimmung. . . . . . . . . . . . . . . . . . . . . .
Geschätzte Position und Ground Truth eines Fahrzeuges auf Basis der
dezentralisierten Positionsschätzung. . . . . . . . . . . . . . . . . . . .
Mittlerer quadratischer Fehler des kooperativen Filters für drei Fahrzeuge.
Einschwingvorgang des Filters am Beispiel zweier Fahrzeuge. . . . . . .
Darstellung der GPS-Rohdaten . . . . . . . . . . . . . . . . . . . . . .
vi
69
71
72
73
74
79
82
84
85
87
88
88
90
Tabellenverzeichnis
3.1
Gegenüberstellung der wichtigsten Partikel-Filter-Varianten. . . . . . .
45
5.1
5.2
Standardwerte für die Simulation der Ground Truth. . . . . . . . . . .
Gegenüberstellung der Komplexität der vorgestellten Algorithmen zur
Verschiebung einer diskreten Partikel-Verteilung unter Unsicherheit. . .
59
6.1
6.2
Mittlerer quadratischer Fehler für eine verschiedene Anzahl von kommunizierenden Fahrzeugen nach 300 Zeitschritten . . . . . . . . . . . .
Mittelwert des mittleren quadratischen Fehlers von drei Fahrzeugen zu
verschiedenen Zeitpunkten für unterschiedliche Werte von σ. . . . . . .
vii
76
89
89
Abkürzungsverzeichnis
CDF
Cumulative Density Function
CI
Covariance Intersection
CoVelLoc
Cooperative Vehicle Localization
CPF
Centralised Particle Filter
CTRV
Constant Turn Rate and Velocity
DGPS
Differential Global Positioning System
DDF
Dezentrale Daten Fusion
EKF
Extended Kalman Filter
EGNOS
European Geostationary Navigation Overlay Service
GNSS
Globales Navigationssatellitensystem
GPS
Global Positioning System
HMM
Hidden Markov Model
IMU
Inertial Measurement Unit
INS
Inertiales Navigationsssytem
ITS
Intelligente Transport Systeme
KLD
Kullback-Leibler-Divergenz
PF
Partikel-Filter
KF
Kalman Filter
RMSE
Root Mean Squared Error. Mittlerer quadratischer Fehler
SIS
Sequential Importance Sampling
SIR
Sequential Importance Resampling
SMCM
Sequentielle Monte-Carlo-Methode
UKF
Unscented Kalman Filter
WDF
Wahrscheinlichkeitsdichtefunktion
ix
Verwendete Formelzeichen
X
Zufallsvariable X
Ω
Raum der Elementarereignisse einer Zufallsvariable X
Fx (x)
Verteilungsfunktion
fx (x)
Wahrscheinlichkeitsdichtefunktion
FN
approximiertes Integral durch Monte-Carlo-Methode
e
Fehler der Monte-Carlo-Integration
µ
Mittelwert
σ
Standardabweichung
σ2
Varianz
Σ
Kovarianzmatrix
N (µ, σ)
Normalverteilung mit den gegebenen Parametern
E
Erwartungswert
P (x)
Wahrscheinlichkeit
P (x, y)
Verbundwahrscheinlichkeit
P (x|y)
Bedingte Wahrscheinlichkeit
P ∗ (x)
zu P (x) proportionale Wahrscheinlichkeit
Q(x)
Proposal Density
η
Normierungskonstante
~x
Zustandsvektor
k
Zeitpunkt k
Zk
alle Messungen bis zum Zeitpunkt k
xk
Systemzustand zum Zeitpunkt k
xi
Verwendete Formelzeichen
(i,j)
∆pk,m
Doppeldifferenz zwischen Knoten k und m für Satelliten i und j
S
Partikel-Set
N
Anzahl der Partikel
Nef f
Anzahl der effektiven Partikel
ωi
Gewicht des i-ten Partikel
δ(x)
Kronecker-Delta
f~(~xk−1 )
nichtlineares Systemmodell
~h(x~k )
nichtlineares Messmodell
ωk
Prozessrauschen
vk
Messrauschen
F
Übergangswahrscheinlichkeit
K(PN , PM )
Kullback-Leibler-Abstand zwischen den Verteilungen PN und PM
h
Bandbreite für Parzen Density Estimation
hopt
optimale Bandbreite für Parzen Density Estimation
h∗
modifizierte Bandbreite für Parzen Density Estimation
d
Anzahl der Dimensionen
Vn
Fahrzeug/Vehicle n
∆d~12
gaußscher Differenzvektor
K(x)
skalierter Gaußkern
xii
1 Einleitung
Die Position eines Fahrzeuges möglichst genau zu bestimmen, ist auf dem Forschungsgebiet der Intelligenten Transport Systeme (ITS) sowohl für Sicherheitssysteme (PreCrash-Analyse) als auch Navigationssysteme ein wichtiges zu lösendes Teilproblem.
Ungeachtet der vielschichtigen Forschungsbemühungen der letzten Jahre zur Verbesserung der Genauigkeit und Zuverlässigkeit von Positionierungssystemen, existiert weiterhin eine Vielzahl ungelöster Probleme. Beispielsweise lässt sich eine hochgenaue
Positionsschätzung mit der aktuell verfügbaren Technik nur durch den Einsatz teurer Sensoren (z.B. Mehrfrequenzempfänger oder zusätzlich Fahrzeugsensoren) realisieren.
1.1 Motivation
Eine mögliche Verbesserung lässt sich durch die Zuhilfenahme kooperativer Algorithmen erreichen. Im Moment gibt es vielseitige Bestrebungen auf dem Gebiet der
IT-System, die Positionsbestimmung durch kooperative Ansätze zu verbessern. Der
Grundgedanke besteht dabei darin, eine sogenannte Fahrzug-zu-Fahrzeug-Kommunikation
(Car2Car) einzusetzen. Eine große Anzahl verschiedener Forschungsprojekte innerhalb
der EU1 und der Bundesrepublik beschäftigen sich mit dieser Problematik, um die Verkehrssicherheit und Effizienz zu erhöhen.
In der vorliegenden Arbeit soll ein möglicher Ansatz zur kooperativen Fahrzeuglokalisierung mit dem Ziel einer genaueren Positionsbestimmung vorgestellt werden.
1
An dieser Stelle sei auf die Bemühungen der Car2Car-Initiative der Europäischen Union verwiesen:
http://www.car-to-car.org/
1
1 Einleitung
1.2 Ziel der Arbeit
Der Grundgedanke dieser Arbeit besteht im Austausch der Messungen eines globalen Navigationssatellitensystems (GNSS) zwischen den einzelnen Teilnehmern über
ein kabelloses Übertragungsmedium. Unter der Annahme, dass alle Teilnehmer eine
Teilmenge der verfügbaren Satelliten nutzen, können systematische Fehler des GNSS
teilweise kompensiert und ein hochgenauer Differenzvektor zwischen den jeweiligen
Teilnehmern berechnet werden. Alle verfügbaren Informationen (Differenzvektor, Positionsmessung des GNSS, Geschwindigkeitsmessung der Fahrzeugsensoren) können
anschließend mit einem komplexen Datenfusionsalgorithmus zusammengeführt werden. Der in dieser Arbeit beschriebene Algorithmus basiert auf einem Partikel-Filter
in Kombination mit einem nichtlinearen Bewegungsmodell der Fahrzeuge. Mit den
verfügbaren Messwerten kann das Filter für jedes Fahrzeug im Netzwerk eine Position
schätzen, die hinsichtlich der Genauigkeit einer GNSS-Position überlegen ist. Abbildung 1.1 verdeutlicht das beschriebenen System schematisch.
GPS
GPS
GPS
Abbildung 1.1: Schematische Darstellung des in dieser Arbeit unter dem Namen CoVelLoc vorgestellten Systems zur kooperativen Fahrzeuglokalierung. Jedes Fahrzeug besitzt einen
lokalen GPS-Empfänger sowie einen Partikel-Filter. Die daraus resultierende geschätzte Position wird ständig ausgetauscht.
2
1.3 Stand der Forschung
Diese Arbeit beschreibt die Details der vorgeschlagenen kooperativen Lokalisierungsmethode mit dem Schwerpunkt auf der dezentralen Datenfusion. Die Ergebnisse werden durch eine rechnergestützte Simulation erlangt und anschließend diskutiert. Auch
der Einfluss von limitierenden Faktoren wie der verfügbaren Bandbreite wird betrachtet.
1.3 Stand der Forschung
Bisherige Forschungen zur kooperativen Fahrzeuglokalisierung werden nachfolgend vorgestellt und deren jeweilige Kernaussage erläutert. Prinzipiell lassen sich die vorgeschlagenen Lösungen in zentrale und dezentrale Verfahren einordnen. In [Sanguino,
2008] wird ein zentraler Ansatz zur Schätzung der Position von Fußgängern mit Hilfe
eines Kalman-Filters vorgestellt. Die auftretenden Korrelationen werden somit implizit
durch die Verarbeitung in einem großen Zustandsraum beachtet. Das in [Edelmayer
et al., 2008] vorgestellte System benutzt für jeden Teilnehmer ein eigenes KalmanFilter, führt die eigentlich Datenfusion aber auch auf einer zentralen Instanz, dem
Fusions-Filter, durch. In den Arbeiten [Karam et al., 2006b] und [Karam et al., 2006a]
wird ein dezentraler Ansatz vorgestellt, bei dem jedes Fahrzeug einen großen Zustandsvektor mit den Schätzungen aller Teilnehmer vorhält. Dieser wird nur durch Messwerte der lokalen Sensoren aktualisiert. Zusätzlich existiert ein weiterer Zustandsvektor,
in den empfangene Informationen eingearbeitet werden. Eine dezentrale Datenfusion
mit einem Partikel-Filter wird in [Rosencrantz et al., 2003] gezeigt. Allerdings wird
dort keine Rücksicht auf eine Korrelation der empfangenen Daten genommen. In den
Arbeiten [Ong et al., 2008], [Ong et al., 2006a] und [Ong et al., 2005b] einer australischen Forschergruppe wird erstmals ein konsistenter dezentraler Fusionsalgorithmus
auf Basis eines Partikel-Filters vorgestellt. Dabei schätzen die einzelnen Teilnehmer
den Zustand eines Objektes, es handelt sich um einen Multiple Target Tracker.
Diese Arbeit grenzt sich durch folgende Eigenschaften von den eben vorgestellten Verfahren ab:
ˆ Durch den Einsatz eines Partikel-Filters kann ein nichtlineares Bewegungsmodell
ohne Einschränkungen benutzt werden.
ˆ Der Ansatz ist vollständig dezentral. Jeder Teilnehmer schätzt nur seinen Zustand, kann aber Schätzungen von anderen Teilnehmern verarbeiten.
3
1 Einleitung
ˆ Die Anforderungen an die Netztopologie sind sehr gering. Teilnehmer können
dynamisch hinzukommen oder das Netzwerk verlassen.
ˆ Durch eine geschickte Transformation kann die empfangene Schätzung als Schätzung des eigenen Fahrzeuges interpretiert werden. Somit schätzt jeder Teilnehmer
implizit auch alle anderen Fahrzeuge.
ˆ Reduzierung der notwendigen Bandbreite zur Laufzeit möglich.
1.4 Aufbau der Arbeit
Der Aufbau der Arbeit gliedert sich wie folgt:
Im zweiten Kapitel werden grundlegende mathematische Kenntnisse, insbesondere der
Wahrscheinlichkeitstheorie, kurz erklärt. Des weiteren wird das Bayes-Filter als Grundlage des Partikel-Filters vorgestellt. Die grundsätzlichen Eigenschaften eines globalen
Navigationssatellitensystems werden genauso wie die Bedeutung eines Bewegungsmodells erläutert.
Aufbauend darauf folgt im dritten Kapitel eine umfangreiche Vorstellung des PartikelFilters. Dabei wird sowohl auf die zu Grunde liegende Monte-Carlo-Theorie eingegangen als auch eine Abgrenzung zu anderen Bayes-Filter-Implementierungen wie dem
Kalman-Filter gezogen. Abgerundet wird dieses Kapitel durch die Vorstellung von
drei in dieser Arbeit umgesetzten Resampling-Verfahren.
Im vierten Kapitel werden verschiedene Methoden zur dezentralen Datenfusion vorgestellt. Ziel ist es dabei, das Problem des Dateninzest zu vermeiden und somit ständig
eine konsistente Positionsschätzung zu gewährleisten. Neben der Covariance Intersection als grundlegende Methode, werden verschieden Verfahren zur Datenfusion im
Partikel-Raum erläutert.
Kapitel fünf gibt einen Überblick der konkreten Umsetzung innerhalb dieser Arbeit.
Das mit dem Namen CoVelLoc bezeichnet System wird vorgestellt. Die im Laufe dieser
Arbeit aufgetretenen Probleme sowie deren Lösungen werden erklärt.
Die Ergebnisse der Simulation des CoVelLoc-Systems sowie theoretische Schlussfolgerungen werden im sechsten Kapitel dargelegt.
Abschließend folgt eine Zusammenfassung sowie ein Ausblick auf zukünftige Herausforderungen.
4
2 Grundlagen
In diesem Kapitel werden mathematische Grundlagen sowie Verfahren, die für das
weitere Verständnis der Arbeit notwendig sind, erklärt. Ein besonderes Augenmerk
wird dabei auf die Wahrscheinlichkeitsrechnung, als Basis der Filtertechnologie, gelegt.
Die prinzipielle Funktionsweise der Positionsbestimmung durch GPS wird genauso wie
die Bedeutung eines Bewegungsmodells vorgestellt.
2.1 Prinzip der Positionsschätzung unter Unsicherheit
In einem Fahrzeug stehen in der Regel die Daten des ABS-Sensors, Geschwindigkeit
und Drehrate, permanent zur Verfügung. Diese Informationen werden mit einer Frequenz von 50 Hz über den Fahrzeugbus (z.B. CAN-Bus oder FlexRay) verschickt und
können anschließend ausgelesen werden. Ist die initiale Position eines Fahrzeugs bekannt, kann durch das Verfahren der Inertialen Navigation (siehe INS in [Prasad et al.,
2005] und [Grewal et al., 2001]) in Kombination mit einem Bewegungsmodell für das
Fahrzeug die Positionsschätzung erfolgen. Dazu werden die Messwerte der Sensoren in
das Bewegungsmodell eingesetzt und anschließend die neue Position bestimmt.
Im Zusammenhang mit einem Inertialen Navigations-System kann ein globales Satellitennavigationssystem (GNSS ) wie GPS oder Galileo wertvolle Dienste leisten.
Ein GNSS liefert periodisch (mit ca. 4 Hz) eine absolute Position in einem globalen
Kooardinatensystem. Diese Information kann zum einen zur Bestimmung der inertialen Position im INS-Algorithmus genutzt werden, andererseits aber auch ständig die
Schätzungen des Bewegungsmodells beurteilen und beeinflussen. Die Verschmelzung
beider Sensordaten wird in einem Filter durchgeführt. Abbildung 2.1 verdeutlicht die
Integration von INS und GNSS graphisch.
Da sowohl die Messwerte des ABS-Sensors als auch die Koordinaten, die ein GNSS
liefert, immer mit Unsicherheit behaftet sind, muss dies im Fusionsschritt berücksichtigt werden. Ein wahrscheinlichkeitstheoretischer (probabilistischer) Ansatz stellt ein
leistungsfähiges Mittel zur Bewältigung dieser Aufgabe bereit.
5
2 Grundlagen
Geschwindigkeit
Bewegungsmodell
x-/y-Position
Drehrate
Filter
(Partikel-Filter)
Ausrichtung
Eingang
(IMU)
Ausgang
(Schätzung)
x-/y-Position
Beobachtung
(GNSS/GPS)
IMU + GNSS = Sensordaten-Fusion
Abbildung 2.1: Prinzip der Positionsschätzung durch Sensordaten-Fusion. Die aus den
ABS-Sensoren (IMU) des Fahrzeugs gewonnenen Informationen werden mit Hilfe des Bewegungsmodells vorhergesagt und anhand von Beobachtungen durch ein Satellitennavigationssystem beurteilt. Zu jedem beliebigen Zeitpunkt steht eine (mit Unsicherheit behaftete)
Schätzung der Position und Ausrichtung bereit.
6
2.2 Wahrscheinlichkeitstheorie
2.2 Wahrscheinlichkeitstheorie
Die Wahrscheinlichkeitsrechnung ist ein Werkzeug, das innerhalb der vorliegenden Arbeit zur Modellierung unsicherer Prozesse genutzt wird. Die Bewegung eines Fahrzeuges unter Unsicherheit verknüpft mit der fehlerbehafteten Messung eines Positionssensors führt beispielsweise auf einen neuen Zustand. Durch die Verknüpfung dieser
beiden Informationen kann eine genauere Aussage über den Aufenthaltsort des Fahrzeuges getroffen werden, als wenn die Informationen einzeln genutzt würden. Für das
weitere Verständnis ist es deswegen wichtig, dass sowohl wahrscheinlichkeitstheoretische Notationen als auch Begriffe eingeführt werden.
Eine Zufallsvariable X kann verschiedene Werte annehmen. Der genau Zusammenhang
wird dabei durch eine reelle Funktion X(ω), die auf dem Raum Ω der Elementarereignisse definiert ist, angegeben. Die Werte X(ω), die die Zufallsvariable X annehmen
kann, nennt man Realisierungsmenge von X.
X = {x : x = X(ω), ω ∈ Ω}
(2.1)
Man unterscheidet zwischen diskreten und kontinuierlichen Zufallsvariablen. Diskrete
Zufallsvariablen besitzen ein abzählbare Menge von Realisierungen, während kontinuierliche Zufallsvariablen auf beliebige Werte abbilden. In der weiteren Betrachtung
wird ausschließlich auf kontinuierliche Zufallsvariablen eingegangen.
2.2.1 Wahrscheinlichkeitsdichte
Im Zusammenhang mit Zufallsvariablen stellt sich oft die Frage, wie wahrscheinlich
ist es, dass ein bestimmtes Ereignis x eintritt? Eine Antwort auf diese Frage liefert die
kumulative Verteilungsfunktion
Fx (x) = P (X ≤ x)
− ∞ < x < ∞.
(2.2)
Die Verteilungsfunktion Fx (x) gibt an, mit welcher Wahrscheinlichkeit P Realisierungen kleiner als eine Schranke x sind. An den Begriff der Wahrscheinlichkeit sind laut
Definition von Kolmogorov folgende drei Bedingungen [Wendel, 2007] geknüpft:
1. Die Wahrscheinlichkeit eines Ereignisses ist immer größer oder gleich Null.
2. Die Wahrscheinlichkeit eines sicheren Ereignisses ist Eins.
7
2 Grundlagen
3. Für zwei sich gegenseitig ausschließende Ereignisse ist die Wahrscheinlichkeit,
dass eines eintritt, gleich der Summe der Einzelwahrscheinlichkeiten.
Die erste Ableitung der Verteilungsfunktion ergibt die Wahrscheinlichkeitsdichtefunktion (oft abgekürzt als Dichtefunktion):
fx (x) =
dFx (x)
.
dx
(2.3)
Im Falle einer kontinuierlichen Zufallsvariable ist die Wahrscheinlichkeit dafür, dass
sie einen bestimmten Wert xi annimmt, gleich 0. Daher betrachtet man immer die
Wahrscheinlichkeit dafür, dass X in einem endlichen Intervall [a, b] liegt. Das Integral
über die Dichtefunktion liefert den gesuchten Wert.
Zb
P (a ≤ X ≤ b) =
fx (x) dx
(2.4)
a
Die Verteilungsfunktion Fx (x) erhält man als Integral über die Dichtefunktion:
Zx
Fx (x) =
fx (x) dx.
(2.5)
−∞
Die Wahrscheinlichkeit P (X ≤ x) kann mit Berücksichtigung der Grenze x als Fläche
unter der Dichtefunktion interpretiert werden (siehe Abbildung 2.2). Da Dichtefunktionen normiert sind, beträgt die Fläche unter der gesamten Kurve Eins:
Z∞
fx (x) dx = 1
(2.6)
−∞
2.2.2 Normalverteilung
Die Normal - oder Gaußverteilung (Glockenkurve) ist eine der wichtigsten kontinuierlichen Wahrscheinlichkeitsverteilungen und spielt im Rahmen der Filterung eine zentrale
Rolle. Einerseits beschreibt sie das Verhalten bestimmter Vorgänge, z.B. das einer fehlerbehafteten Sensormessung, sehr gut und zum anderen lässt sie sich mathematisch
einfach handhaben. Selbst in Anwendungsfällen, in dennen streng genommen keine
Normalverteilung vorliegt, wird man auf Grund der fehlenden Informationen oft eine
8
2.2 Wahrscheinlichkeitstheorie
fx (x)
P (X ≤ x)
x
x
0
Abbildung 2.2: Wahrscheinlichkeit als Fläche unter der Dichtefunktion interpretiert.
fx (x)
0, 4
x
µ
−σ
σ
Abbildung 2.3: Dichtefunktion der Standardnormalverteilung mit den Parametern µ = 0
und σ 2 = 1.
Normalverteilung annehmen. Die Dichte- und die Verteilungsfunktion der Normalverteilung sind durch
fx (x) = √
Fx (x) = √
1
e−
2πσ 2
Zx
1
2πσ 2
(x−µ)2
2σ 2
e−
(t−µ)2
2σ 2
(2.7)
dx
(2.8)
−∞
gegeben. Hierbei repräsentiert x die normalverteilte Zufallsvariable, σ 2 die Varianz und
µ den Mittelwert der Verteilung. Eine Besonderheit der Normalverteilung ist, dass man
sie durch die beiden Parameter µ und σ 2 vollständig parametrisch beschreiben kann.
In Abbildung 2.3 ist die Dichtefunktion einer typischen Normalverteilung mit ihren
Kennwerten dargestellt.
Die in der Literatur gebräuchliche Notation
X ∼ N (µx , σx2 )
9
2 Grundlagen
ist eine kompakte Schreibweise für die Definition einer normalverteilten Zufallsvariable
X mit dem Mittelwert µx und der Varianz σx2 .
Bisher wurde lediglich der eindimensionale Fall (x ist eine skalare Zufallsvariable)
betrachtet. Die Normalverteilung erlaubt ebenfalls eine Ausdehnung auf beliebig viele
~
Dimensionen und wird somit zu einer multivariaten Verteilung des Zufallsvektors X.
~ ∼ N (~µ, Σ)
X
(2.9)
T
1
1
~ ) Σ−1 (~x − µ
~)
fX (~x) = det(2πΣ)− 2 e 2 (~x − µ
(2.10)
Die Mittelwerte werden im Mittelwertsvektor µ
~ zusammengefasst, die Varianzen entsprechend in der Kovarianzmatrix Σ, einer positiv semidefiniten, symmetrischen Matrix, abgebildet.
2.2.3 Bedingte Wahrscheinlichkeit
Die gemeinsame Wahrscheinlichkeit zweier Zufallsvariablen X und Y bezeichnet man
als Verbundwahrscheinlichkeit, bei der für X der Wert x und für Y der Wert y eintritt.
Sie ist durch die Gleichung
P (x, y) = P (X = x und Y = y)
(2.11)
definiert. Sind beide Zufallsvariablen X und Y stochastisch voneinander unabhängig,
ergibt sich die Verbundwahrscheinlichkeit zu:
P (x, y) = P (x) · P (y)
(2.12)
Um auszudrücken, wie wahrscheinlich es ist, dass X den Wert x annimmt, unter der
Voraussetzung, dass Y bereits den Wert y angenommen hat, dient die Bedingte Wahrscheinlichkeit. Die Abhängigkeit wird wie folgt ausgedrückt:
P (x|y) = P (X = x|Y = y)
(2.13)
Sind X und Y voneinander abhängig, ergibt sich
P (x|y) =
P (x, y)
P (y)
mit P (y) > 0
P (x, y) =P (x|y) · P (y) = P (y|x) · P (x)
10
(2.14)
(2.15)
2.2 Wahrscheinlichkeitstheorie
ansonsten, für Unabhängigkeit
P (x|y) =
P (x) · P (y)
= P (x).
P (y)
(2.16)
Sind X und Y voneinander unabhängig, erhält man durch die Bedingte Wahrscheinlichkeit keine weiteren Informationen.
2.2.4 Satz von Bayes
Durch Umformen von Gleichung (2.14) und anschließender Integration über y der
Verbunddichte P (x, y) lässt sich die Dichte P (x) berechnen.
Z
Z
P (x) = P (x, y) dy = P (x|y) P (y) dy
(2.17)
Dieser Schritt wir als Marginalisierung bezeichnet und stellt zugleich die Anwendung
des Gesetzes der Totalen Wahrscheinlichkeit dar.
Aus Gleichung (2.14) und (2.15), der Bedingten Wahrscheinlichkeit, folgt direkt der
Satz von Bayes, der einen Zusammenhang zwischen P (x|y) und der Inversen“ P (y|x)
”
herstellt:
P (y|x) P (x)
P (x, y)
=
P (y)
P (y)
P (x|y) P (y)
P (x, y)
P (y|x) =
=
P (x)
P (x)
P (x, y) = P (x|y) P (y) = P (y|x) P (x)
P (x|y) =
(2.18)
(2.19)
(2.20)
Gleichungen (2.19) und (2.20) sind alternative Formulierungen des Satz von Bayes, die
für später Ableitungen in dieser Arbeit benötigt werden.
Der Satz von Bayes spielt bei der Schätzung von Systemzuständen durch Fusionieren
von Modell und Sensormessungen eine zentrale Rolle. Nimmt man an, dass x eine
Zustandsgröße ist, die aus der Messung y abgeleitet wurde, dann bezeichnet man die
Wahrscheinlichkeit P (x) als a priori-Wahrscheinlichkeitsverteilung. In P (x) sind alle
Informationen enthalten, die bis zum Zeitpunkt vor der Datenfusion mit P (y) zur Verfügung standen. Die Wahrscheinlichkeit P (x|y) wird a posteriori-Wahrscheinlichkeit
genannt. Mit dem Satz von Bayes ist es nun möglich, die gesuchte a posteriori-Wahrscheinlichkeit P (x|y) aus der inversen“ Wahrscheinlichkeitsverteilung P (y|x) und der
”
11
2 Grundlagen
a priori-Wahrscheinlichkeitsverteilung P (x) zu berechnen. Für Problemstellungen, wie
die Positionsschätzung eines sich bewegenden Objekts, gibt P (y|x) die Wahrscheinlichkeit dafür an, dass die Messung y für den Fall des Zustandes x eintritt. Die im
Zähler von Gleichung (2.18) enthaltene Wahrscheinlichkeit P (y) ist von x unabhängig
und wird deshalb als Normierungsfaktor η des Satz von Bayes bezeichnet.
P (x|y) = η P (y|x) P (x)
(2.21)
2.3 Markovprozess
Als Markovkette oder Markovprozess bezeichnet man einen stochastischen Vorgang
dessen zukünftige Entwicklung sich aus Kenntnis einer begrenzten Vorgeschichte bzw.
dem gegenwärtigen Zustand vorhersagen lässt.
P (Xn = xn |Xn−1 = xn−1 , Xn−2 = xn−2 , . . . , X1 = x1) =
P (Xn = xn |Xn−1 = xn−1 , Xn−2 = xn−2 , . . . , Xn−m = xn−m )
(2.22)
Ist für eine Vorhersage des nächsten Zustandes allein der gegenwärtige Zustand ausreichend, spricht man von einem Markov-Prozess erster Ordnung (m = 1). Der aktuelle
Zustand beinhaltet somit die gesamte Historie des Prozesses.
P (Xn = xn |Xn−1 = xn−1 )
(2.23)
Die Entwicklung von einem Zustand in den nächsten, ist durch die Übergangswahrscheinlichkeit festgelegt.
In Abbildung 2.4 ist die schematische Darstellung eines Hidden Markov Modells (HMM)
erster Ordnung zu sehen. Hidden“ bedeutet dabei, dass der Zustand xk nicht direkt
”
sichtbar ist. Die Beobachtung zk lässt allerdings Rückschlüsse auf xk zu. Der verborgene Zustand xk ist nur von seinem direkten Vorgänger xk−1 abhängig, weitere
Abhängigkeiten bestehen nicht. Für die Messung zk trifft die gleiche Annahme zu, sie
ist ausschließlich von xk abhängig.
12
2.4 Bayes-Filter
k−1
k
k+1
Zeit
zk−1
zk
zk+1
Messung
xk+1
Zustand
p(zk |xk )
xk−1
p(xk |xk−1 )
xk
Abbildung 2.4: Schematische Darstellung eines Hidden Markov Modells erster Ordnung.
2.4 Bayes-Filter
Unter dem Bayes-Filter oder Bayes-Algorithmus versteht man ein Verfahren zur dynamischen Schätzung der a posteriori-Wahrscheinlichkeitsdichteverteilung eines Systemzustandes unter Berücksichtigung von Stellgrößen und Beobachtungen. Oftmals ist es
dabei notwendig, dass in jedem Zeitschritt k, in dem eine Beobachtung beziehungsweise Messung erfolgt, eine neue Schätzung berechnet wird. In diesem Falle spricht man
von einem rekursiven Filter. Ein rekursives Filter verarbeitet die Daten sequentiell, so
dass es nicht zwingend ist, die gesamte Historie vorzuhalten und in jedem Zeitschritt
wiederholt zu berechnen. Der übliche Ablauf gliedert sich in zwei Schritte: die Vorhersage (Prediction) und die Aktualisierung (Update). Im Vorhersageschritt wird das
Systemmodell zur Evolution der Zustandswahrscheinlichkeitsdichte von einem Beobachtungszeitpunkt zum nächsten unter Berücksichtigung von Stellgrößen verwendet.
Da der Zustandsübergang in der Regel mit Unsicherheiten behaftet ist, wird die vorhergesagte Wahrscheinlichkeitsdichtefunktion eine höhere Streuung (verglichen mit der
a posteriori-Wahrscheinlichkeitsdichte) aufweisen. Erst der Aktualisierungsschritt, der
die letzte Beobachtung einarbeitet, liefert die endgültige (meist schmalere) a posteriori Wahrscheinlichkeitsdichte des Systemzustandes. Allgemein lässt sich ein Schritt des
Bayes-Filters durch
a posteriori WDF =
Messwahrscheinlichkeit × a priori WDF
Normierungskonstante
a posteriori WDF ∝ Messwahrscheinlichkeit × a priori WDF
formulieren ( [Lefebvre et al., 2005]).
13
2 Grundlagen
Das Bayes-Filter ermittelt die Schätzung des Zustandes xk in Abhängigkeit aller verfügbaren Messungen Zk = {zi |i = 1, . . . , k} bis zum Zeitpunkt k. Dabei werden rekursiv die beiden Schritte der Vorhersage und der Aktualisierung ausgeführt. Das
Ableiten der Wahrscheinlichkeit eines neuen Zustandes aus einer Messung und dem a
priori Wissen wird oft auch als Bayes-Inferenz bezeichnet.
2.4.1 Chapman-Kolmogorov-Gleichung
Die zeitliche Entwicklung einer Dichtefunktion, also die Vorhersage von xk−1 zu xk ,
lässt sich über die Chapman-Kolmogorov-Gleichung beschreiben. Der zu schätzende
Systemzustand xk soll ein Markov-Prozess erster Ordnung sein. Daraus ergibt sich,
dass der Zustand vollständig durch xk−1 beschrieben ist, eine Abhängigkeit von weiter
zurückliegenden Systemzuständen besteht nicht. Die Eigenschaft der Vollständigkeit
wird durch folgende Gleichungen ausgedrückt:
P (xk |xk−1 , xk−2 , . . . , x0 ) = P (xk |xk−1 )
(2.24)
P (xk |xk−1 , Zk−1 ) = P (xk |xk−1 )
(2.25)
Mit Zk−1 sind alle bis zum Zeitpunkt k − 1 aufgetretenen Beobachtungen gemeint.
Hinsichtlich der Evolution von xk stellt Zk−1 allerdings keine zusätzliche Bedingung
dar, da bereits alle vorliegenden Beobachtungen in xk−1 eingeflossen sind.
Die Wahrscheinlichkeitsdichte P (xk |Zk−1 ) soll nun als Funktion von P (xk−1 |Zk−1 ) angegeben werden. Durch Integration der Verbunddichte, erhält man die marginale Dichte
Z
P (xk , Zk−1 ) = P (xk , xk−1 , Zk−1 ) dxk−1 .
(2.26)
Aufgrund der bayeschen Regel P (A, B) = P (A)P (B|A) = P (B)P (A|B) kann der
Integrand umgeschrieben werden:
Z
P (xk |xk−1 , Zk−1 )P (xk−1 , Zk−1 ) dxk−1
P (xk , Zk−1 ) =
(2.27)
Durch erneutes Anwenden der bayeschen Regel auf den zweiten Term ergibt sich
Z
P (xk , Zk−1 ) =
P (xk |xk−1 , Zk−1 )P (xk−1 |Zk−1 )P (Zk−1 ) dxk−1 .
14
(2.28)
2.4 Bayes-Filter
Da P (Zk−1 ) von Xk−1 unabhängig ist, kann die Dichte vor das Integral gezogen und
durch sie dividiert werden:
P (xk , Zk−1 )
=
P (Zk−1 )
Z
P (xk |xk−1 , Zk−1 )P (xk−1 |Zk−1 ) dxk−1
(2.29)
Die bayesche Regel auf der linken Seite der Gleichung erneut anwenden und die
Markov-Eigenschaft erster Ordnung auf der rechten Seite ausnutzen:
Z
P (xk |Zk−1 ) =
P (xk |xk−1 )P (xk−1 |Zk−1 ) dxk−1 .
(2.30)
Gleichung (2.30) wird als Chapman-Kolmogorov-Gleichung bezeichnet und beschreibt
die zeitliche Entwicklung der Wahrscheinlichkeitsdichtefunktion eines Zufallsvektors
xk .
2.4.2 Berücksichtigung von Beobachtungen
Tritt zum Zeitpunkt k eine neue Beobachtung auf, erfolgt das sogenannte BayesUpdate. Dabei wird die mittels der Chapman-Kolmogorov-Gleichung vorhergesagte
Wahrscheinlichkeitsdichtefunktion P (xk |Zk−1 ) aufgrund der letzten Beobachtung zk
aktualisiert. Die a posteriori-Wahrscheinlichkeitsdichte P (xk |Zk ) kann unter Verwendung der bayesschen Regel wie folgt berechnet werden:
P (xk |Zk ) = P (xk |zk , Zk−1 )
(2.31)
P (xk , zk , Zk−1 )
P (zk , Zk−1 )
(2.32)
=
Für Zähler und Nenner des Quotienten werden die ebenfalls über die bayessche Regel
gewonnen Zusammenhänge
P (xk , zk , Zk−1 ) = P (zk |xk , Zk−1 )P (xk , Zk−1 )
(2.33)
und
P (zk , Zk−1 ) = P (zk |Zk−1 )P (Zk−1 )
15
(2.34)
2 Grundlagen
eingesetzt:
P (xk |Zk ) =
P (zk |xk , Zk−1 )P (xk , Zk−1 )
.
P (zk |Zk−1 )P (Zk−1 )
(2.35)
Das erneute Anwenden der bayesschen Regel auf den Term P (xk , Zk−1 ) des Zählers
und anschließendes Kürzen von P (Zk−1 ) führt zu
P (xk |Zk ) =
P (zk |xk , Zk−1 )P (xk |Zk−1 )
P (zk |Zk−1 )
(2.36)
Aus der Annahme von Gleichung (2.25) folgt schließlich die vollständige Formel für
das Bayes-Update:
P (xk |Zk ) =
P (zk |xk )P (xk |Zk−1 )
P (zk |Zk−1 )
(2.37)
wobei der Term des Nenners die Normalisierungskonstate
Z
η = P (zk |Zk−1 ) =
P (zk |xk )P (xk |Zk−1 ) dxk
(2.38)
darstellt, die von der Wahrscheinlichkeit P (zk |xk ), dem Messmodell, abhängt.
Gleichung (2.37) beschreibt den Einfluss von Beobachtungen auf die Dichtefunktion
des Systemzustandes. Zusammen mit Gleichung (2.30), der zeitlichen Vorhersage des
Systemzustandes, ist damit das allgemeine zeitdiskrete Filterproblem im Sinne von
Bayes formal gelöst.
Zusammenfassend lässt sich sagen, dass der optimale bayessche Filteralgorithmus die
a posteriori-Wahrscheinlichkeitsdichte des Zustandes xk in Abhängigkeit von Beobachtungen und Steuerinformationen bis zum Zeitpunkt k schätzt. Dabei wird vorausgesetzt, dass das System als Markovprozess beschreibbar und der Zustand somit
vollständig ist. Für eine konkrete Umsetzung dieses Algorithmus sind drei Wahrscheinlichkeitsdichtefunktionen notwendig: die Initialverteilung P (x0 ), die Messwahrscheinlichkeit P (zk |xk ) sowie die Zustandsübergangswahrscheinlichkeit P (xk |xk−1 ).
Die rekursive Vorhersage der a posteriori-Wahrscheinlichkeitsdichte durch Gleichung
(2.30) und (2.37) ist allerdings nur eine konzeptionelle Lösung, die sich im Allgemeinen
nicht analytisch bestimmen lässt. Dazu wäre es notwendig, die gesamt Wahrscheinlichkeitsdichte in Form eines unendlich großen Vektors vorzuhalten. Nur für den Spezial-
16
2.4 Bayes-Filter
fall eines linearen System- und Messmodells sowie gaußverteilter Zufallsvariablen ist
eine exakte und vollständige Lösung möglich: das Kalman-Filter. Allgemeine Verfahren beruhen entweder auf Approximationen (Extended Kalman Filter oder Gaußschen
Summen-Filter) oder suboptimalen Bayes-Algorithmen wie dem Partikel-Filter.
2.4.3 Kalman-Filter
Das Kalman-Filter setzt voraus, dass sich die Wahrscheinlichkeitsdichte zu jedem Zeitpunkt durch eine Normalverteilung beschreiben lässt und damit eindeutig aufgrund
der beiden Parameter Mittelwert und Kovarianz charakterisiert ist. Wird zusätzlich
ein lineares System- und Messmodell angenommen, gelingt mit dem Kalman-Filter
eine vollständige analytische Lösung des Bayes-Algorithmus aus Abschnitt 2.4.2 . Hinsichtlich jeden Kriteriums (z.B. des mittleren quadratischen Fehlers) ist die Lösung
optimal.
Die vollständige Herleitung für das Kalman-Filter in n Dimensionen findet sich u.a.
in [Plessis, 1997] und [Zarchan und Musoff, 2005]. In diesem Abschnitt soll lediglich die
konkrete Gleichung des Bayes-Algorithmus für eine Dimension unter der Bedingung
des Kalman-Filters gezeigt und graphisch verdeutlicht werden. Nimmt man an, dass
die a priori-Wahrscheinlichkeitsdichte und die Messwahrscheinlichkeit mit
P (x) ∼ N (µ1 , σ12 )
P (z|x) ∼ N (µ2 , σ22 )
gegeben sind, dann ergbit sich die a posteriori-Wahrscheinlichkeitsdichte zu
P (x|z) ∼ N (µ̂, σ̂ 2 )
wobei
σ̂ 2 =
σ12 σ22
σ12 + σ22
µ̂ =
µ2 σ12 + µ1 σ22
.
σ12 + σ22
Damit ist eine geschlossene analytische Lösung des Bayes-Algorithmus gefunden (eine
ausführliche Herleitung findet sich in [Köhler, 2005]). In Abbildung 2.5 ist der beschriebene Sachverhalt graphisch dargestellt. Aus der unsicheren a priori-Wahrscheinlichkeitsdichte
P (x) und der Messwahrscheinlichkeit P (z|x) wird durch die Bayes-Inferenz eine neue
17
2 Grundlagen
P (x|z)
P (z|x)
P (x)
x/z
2
2, 8
6
Abbildung 2.5: Das Kalman-Filter als optimale Lösung im Sinne von Bayes. Die gaußsche
a priori WDF P (x) wird durch eine Iteration des Kalman-Filters mit der ebenfalls gaußschen
Messwahrscheinlichkeit P (z|x) kombiniert. Ergebnis ist die sicherere (da kleinere Varianz)
Aussage P (x|z). (µ1 = 6, µ2 = 2, σ12 = 4, σ22 = 1, µ̂ = 2, 8, σ̂ 2 = 0, 8)
Aussage gebildet. Die Varianz von P (x|z) ist deutlich kleiner. Im Umkehrschluss bedeutet dies gleichzeitig, dass das Wissen über den Systemzustand aufgrund einer Messung aktualisiert und verbessert wurde.
2.5 Globales Navigationssatellitensystem
Ein globales Navigationssatellitensystem (GNSS) ermöglicht den Teilnehmern die Bestimmung ihrer Position. Dazu werden die von verschiedenen Satelliten abgestrahlten
Signale empfangen und kombiniert. Heute übliche GNS-Systeme sind u.a. das amerikanische GPS (ehemals Navstar), das russische GLONASS und das sich noch im
Aufbau befindende Galileo der Europäischen Union.
Die Positionsbestimmung erfolgt durch die Messung der Laufzeit des Signales von
Satellit zu Empfänger. Da sich elektromagnetische Signale mit Lichtgeschwindigkeit
ausbreiten, ergibt sich die Entfernung zwischen Sender um Empfänger aus:
Entfernung (Pseudorange) = Lichtgeschwindigkeit × (Signallaufzeit + Zeitfehler)
Die Signallaufzeit zwischen Sender (Satellit) und Empfänger lässt noch keine eindeutige Positionsaussage zu, deswegen muss ein GNSS-Empfänger immer mit mindestens
vier unabhängigen Satelliten in Kontakt stehen und die jeweiligen Laufzeiten messen.
18
2.5 Globales Navigationssatellitensystem
Satellit 1
Unsicherheit der
Abstandsmessung
Satellit 2
Satellit 3
Unsicherheit der Position
Abbildung 2.6: Positionsbestimmung eines GNSS mit Hilfe von drei unabhängigen Satelliten. Die Laufzeitmessung zwischen dem GNSS-Empfänger und einem Satelliten liefert einen
Pseudorange. Durch die Kombination von drei Pseusoranges kann bereits eine grobe Positionsbestimmung erfolgen. Die Messung eines zusätzlichen vierten Satelliten korrigiert den
Zeitfehler.
Aus den vier Werten kann dann ein Gleichungssystem aufgestellt werden, das sich
durch ein Optimierungsverfahren (meist eine gewichtete Lösung im Sinne der kleines
Quadrate) lösen lässt: Das Ergebnis ist die aktuelle Position und Uhrzeit. Stehen mehr
Satelliten zur Verfügung, wird das Ergebnis genauer.
Genauigkeit und typische Fehlerquellen
Nachfolgend wird die zu erwartende Genauigkeit sowie typische Fehlerquellen am Beispiel von GPS erläutert.
Handelsübliche GPS-Empfänger liefern typischerweise mit einer Frequenz von ca. 4 Hz
eine absolute Position (auch GPS-Fix genannt). Die Position setzt sich dabei, je nach
Sichtbarkeit der Satelliten, aus mindestens vier Laufzeitmessungen zusammen. Die
übliche Genauigkeit liegt bei ca. 10-20 Metern1 Standardabweichung. Der Fehler besteht zum einen aus einem Offset (z.B. durch Laufzeitunterschiede der Uhren, atmosund ionisphärische Störungen) zum anderen aber auch aus sich sprunghaft ändernden
Komponenten. Gerade in Ballungsgebieten kann es zu sogenannten Multipath-Effekten
1
Unter Zuhilfenahme von Diensten wie DGPS bzw. EGNOS lassen sich durchaus Standardabweichungen von ca. 1 m erreichen. Da diese Dienster allerdings nicht immer Verfügbar sind, werden
sie in der weiteren Betrachtung nicht berücksichtigt.
19
2 Grundlagen
kommen. Die Signale der einzelnen Satelliten werden u.U. an Hauswänden reflektiert
und haben somit unterschiedliche Laufzeiten.
Pseudorange Doppeldifferenzmessung
Das in [Graas und Braasch, 1991] vorgestellte Verfahren der Smoothed Pseudorange
Double Differences erlaubt die hochgenaue Bestimmung der Relativposition zwischen
zwei GPS-Empfängern. Benutzt man diese zusätzliche Information in einem kooperativen INS/GNSS-Filter, kann die absolute Positionsbestimmung der einzelnen Teilnehmer weiter verbessert werden. In [Sanguino, 2008] wurde diese Methode erfolgreich für
die Lokalisierung von Personen eingesetzt. Die dabei angenommene Standardabweichung betrug 5 Meter. Verglichen mit der Standardabweichung eines einfachen GPSEmpfängers hat sich der Fehler halbiert.
Unter einer Pseudorange Doppeldifferenzmessung versteht man die Linearkombination
der Pseudorangemessung zwischen jeweils zwei Satelliten und zwei Empfängern. Die
Pseudorange Doppeldifferenzen werden für die Empfänger k und m mit den Satelliten
j und l wie folgt berechnet:
(j,l)
(j)
(l)
∆pk,m = ∆pk,m − ∆pk,m
(2.39)
wobei
(j)
(j)
(2.40)
(l)
(l)
(2.41)
∆pk,m = pk − p(j)
m
∆pk,m = pk − p(l)
m
die Differenzen der Pseudoranges zwischen den beiden Empfängern zum jeweils gleichen Satelliten sind.
In einer praktischen Umsetzung ist darauf zu achten, dass die für die PseudorangeDifferenzmessung verwendeten Signallaufzeiten nur eingeschränkt für die Berechnung
des GPS-Fix benutzt werden sollten, da andernfalls unbekannte Korrelationen auftreten. Werden diese nicht berücksichtigt, kann es zum Unterschätzen der Fehler und
somit zum Divergieren des Filters kommen.
20
2.6 Zustandsraum und Bewegungsmodell
y
Fahrzeug i
θi
yi
xi
x
Abbildung 2.7: Darstellung der Komponenten Position und Ausrichtung des Zustandsvektors ~xi für das i-te Fahrzeug. Diese werden mit Hilfe eines vereinfachten CTRVBewegungsmodells geschätzt und vorhergesagt.
2.6 Zustandsraum und Bewegungsmodell
Für die Positionsschätzung eines Fahrzeuges durch ein Filter bietet es sich an, die
Zustandsgrößen Position und Ausrichtung zu schätzen und in einem dreidimensionalen
Zustandsvektor
T
~x = x y θ
(2.42)
zusammenzufassen. Hierbei bezeichnen x und y die Position und θ die Ausrichtung.
Abbildung 2.7 zeigt die Bedeutung der einzelnen Größen graphisch. Da sich die Freiheitsgrade einer Bewegung von Fahrzeugen aufgrund von technischen Gegebenheiten
(wie z.B. Lenkwinkel) gut einschränken lassen, kann ein Bewegungsmodell benutzt
werden um die Komponenten des Zustandsvektors aus Gleichung (2.42) zeitlich zu
prädizieren. Ein Überblick der gängigen Bewegungsmodelle findet sich in [Schubert
et al., 2008].
In dieser Arbeit wird auf das Constant Turn Rate and Velocity-Modell (CTRV) aus
[S. S. Blackman, 1999] zurückgegriffen. Das CTRV-Modell betrachtet die Eingangsgrößen Geschwindigkeit v und Drehrate2 ω als stückweise konstante Parameter und
2
Die Drehrate gibt die Rotation um die z-Achse eines Fahrzeuges an.
21
2 Grundlagen
berechnet für ein gegebenes T die vorhergesagte Position und Ausrichtung:


v
v
sin
(ωT
+
θ
(t))
−
sin
(θ
(t))
+
x(t)
ω
 ω

~x(t + T ) =  − ωv cos (ωT + θ (t)) + ωv sin (θ (t)) + y(t)  .
ωT
+
θ(t)
(2.43)
Für den Fall einer Geradeausfahrt (ω = 0) vereinfacht sich das Modell zu


v cos (θ (t)) T + x(t)


~x(t + T ) =  v sin (θ (t)) T + y(t)  .
θ(t)
(2.44)
Da der in dieser Arbeit verwendete Zustandsvektor ~x nur dreidimensional ist, ändert
sich auch die Gleichung (2.43) im Vergleich zum ursprünglichen CTRV-Modell aus
[S. S. Blackman, 1999], das zusätzlich die Größen v und ω angibt.
Obwohl das CTRV-Modell für die Modellierung einer Fahrzeugbewegung gut geeignet
ist, hat es in der hier angegebenen Form einen entscheidenden Nachteil: Das Modell
kann eine Drehung des Fahrzeuges im Stand vorhersagen, ohne dass ein reales Fahrzeug
(mit Ackermann-Lenkung, siehe [Choset et al., 2005]) dazu in der Lage wäre. Dies kann
nur durch eine zusätzliche Parameterprüfung außerhalb der CTRV-Gleichung oder ein
anderes Bewegungsmodell verhindert werden.
22
3 Partikel-Filter
Die Annahme, dass sowohl die Beobachtungen als auch der Zustandsübergang des
Systemmodells einer linearen Funktion unterliegen, sind zwingende Voraussetzungen
für den erfolgreichen Einsatz des Kalman-Filters. Das Kalman-Filter verdankt seine Effektivität der Tatsache, dass es möglich ist, die Parameter der resultierenden
Normalverteilung durch eine geschlossene analytische Lösung zu ermitteln. Unglücklicherweise werden sich die meisten realen Probleme komplexer gestalten: System- und
Messmodell sind nichtlinear. In Abbildung 3.1 ist exemplarisch dargestellt, was durch
die nichtlineare Transformation einer Normalverteilung geschieht: Im Beispiel ist das
Ergebnis P (y) eine multimodale Wahrscheinlichkeitsdichteverteilung, die nicht mehr
korrekt durch die Parameter einer Normalverteilung beschreibbar ist.
Um diesen Beschränkungen beizukommen, wurde eine Vielzahl unterschiedlicher nichtlinearer approximativer Filterverfahren entwickelt. Während einige dieser Methoden
sehr generell sind, beschränken sich andere auf spezielle Anwendungsgebiete. Nach [Ristic et al., 2004] lassen sich die nichtlinearen Filter in vier große Gruppen einteilen:
analytische Approximationen, nummerische Approximationen, gaußsche Summenfilter und Sampling-Verfahren. Einer der prominentesten Vertreter für die Zustandsschätzung nichtlinearer System ist das Erweiterte Kalman Filter (EKF) [Zarchan und
Musoff, 2005]. Das EKF beruht auf der Linearisierung des Systemmodells durch eine Taylor-Reihe erster Ordnung im Punkt des Mittelwertes der Verteilung. Dadurch
kann die Forderung der Linearität für kleine Abtastintervalle des Filters aufgeben werden. Während sich das EKF einer analytischen Approximation bedient, versucht das
Unscented Kalman Filter [Julier und Uhlmann, 1997a] die Nichtlinearität durch eine
statistische Näherung zu schätzen. Dazu wird die Verteilung anhand fest vorgegebener Stützstellen, die anschließend nichtlinear transformiert werden, beschrieben. Die
Integrale aus Gleichung (2.30) und (2.37) des Bayes-Algorithmus (siehe Abschnitt 2.4)
durch eine Summe zu ersetzen, ist ein Grundgedanke, der in Gitter-basierten Methoden
zum Einsatz kommt. Damit die nummerische Näherung im Bezug auf den kontinuierlichen Zustandsraum ausreichend gut ist, muss die Auflösung des Gitters entsprechend
fein gewählt werden. Im Zusammenhang mit mehrdimensionalen Problemen ist dies
eines der rechenintensivsten Verfahren überhaupt [Doucet und de Fretias, 2001], da die
23
3 Partikel-Filter
y
f (x)
x
P (x)
P (y)
Abbildung 3.1: Nichtlineare Transformation einer normalverteilten Zufallsvariable. Der
untere rechte Graph zeigt die ursprüngliche eindimensionale Gaußverteilung, die durch die
nichtlineare Funktion f (x) transformiert wird. Im oberen linken Graph ist das Ergebnis zu
sehen: Die wahre Wahrscheinlichkeitsdichte (blau) wird durch die Normalverteilung (rot)
nicht korrekt wiedergegeben.
Komplexität exponentiell mit der Anzahl der Dimensionen wächst. Die Schätzung einer
beliebigen Wahrscheinlichkeitsdichte durch eine Summe mehrerer Normalverteilungen
wird als gaußscher Summenfilter bezeichnet. Im Gegensatz zum EKF oder UKF lassen
sich damit sogar multimodale Probleme bearbeiten [Ristic et al., 2004]. In [Ristic et al.,
2004] wird ebenfalls darauf hingewiesen, dass die Wahl der Anzahl der Normalverteilungen und ihrer Parameter allerdings ein nicht zu unterschätzendes Problem darstellt.
Eine umfassende Vorstellung der verschiedenen Bayes-Implementierungen findet sich
in [Arulampalam et al., 2002].
Das EKF und das UKF als auch der gaußsche Summenfilter erzielen bei großen Nichtlinearitäten schlechte Ergebnisse (siehe [Thrun et al., 2005] und [Doucet und de Fretias,
2001]). Im Vergleich zu Gitter-basierten Verfahren sind sie jedoch weniger rechenintensiv. Das UKF kommt während der Linearisierung sogar ohne die Jakobimatrix aus
und wird daher auch als ableitungsfreies Filter bezeichnet.
24
Gitter-basierte
Verfahren
Topologische
Verfahren
Kalman-Filter
PartikelFilter
Extended /
Unscented
Kalman-Filter
Gaußscher
Summenfilter
diskret
kontinuierlich
Bayes-Filter
Abbildung 3.2: Einordnung des Partikel-Filters hinsichtlich der wichtigsten Bayes-Filter
Implementierungen nach [Fox, 2003]. Die gepunktet umrahmten Filter sind in der Lage optimale Lösung im Sinne von Bayes zu erzeugen.
Ein weiteres sehr vielseitiges Verfahren stellt das Partikel-Filter dar. Es handelt sich
beim Partikel-Filter um eine nichtparametrische Umsetzung des Bayes-Filters, der die
Wahrscheinlichkeitsdichteverteilung durch eine endliche Anzahl von gewichteten Stützstellen (Samples) beschreibt. Das Partikel-Filter gliedert sich in die große Gruppe der
Sequentiellen Monte-Carlo-Methoden (SMCM)1 ein und ist damit ein statistisches Verfahren. Wie der Namen bereits andeutet, liegen dabei Annahmen aus der Spieltheorie
zugrunde. Grob gesagt, versucht man durch Simulation vieler Systemzustände und
der anschließenden Bewertung in Abhängigkeit ihrer Entwicklung über der Zeit, eine
möglichst gute Schätzung des zu erwartenden Ergebnisses abzugeben. In Abbildung
3.2 ist die Einordnung des Partikel-Filters in die große Gruppe der Bayes-Filter zu
sehen ( [Fox, 2003]). Partikel-Filter zeichnen sich u.a. durch folgende Eigenschaften
aus (u.a. [Dellaert et al., 1999]):
ˆ Sie sind sehr vielseitig einsetzbar.
ˆ Sie sind einfach zu implementieren.
ˆ Im Vergleich zu Gitter-basierten Verfahren benötigen sie deutlich weniger Speicher.
ˆ Partikel-Filter können multimodale Verteilungen schätzen.
ˆ Eine Parallelisierung des Algorithmus ist ohne weiteres möglich.
1
Siehe [Hammersley und Morton, 1954] und [MacKay, 1998] als Einführung in die Monte-CarloTheorie.
25
3 Partikel-Filter
ˆ Sie erlauben das Berücksichtigen von weiteren Bedingungen: So ließe sich z.B.
für die Positionsschätzung im Zusammenhang mit einer Straßenkarte leicht eine
Plausibilitätsprüfung realisieren.
Im restlichen Teil dieses Kapitels werden die Grundlagen zum allgemeinen Verständnis
des Partikel-Filters vorgestellt. Anschließend werden spezifische Modifikationen des
Standard-Partikel-Filters, die in dieser Arbeit zum Einsatz kommen, erläutert.
3.1 Grundgedanke
Das Partikel-Filter [Gordon et al., 1993] repräsentiert die Schätzung einer Wahrscheinlichkeitsdichtefunktion durch eine endliche Anzahl N zufällig gewählter Stützstellen
der Dichtefunktion des Zustandes. Zusammen mit einem Gewicht ergibt jede Stützstelle ein sogenanntes Partikel. Die Menge aller Einzelpartikel wird im Partikel-Set S
zusammengefasst:
S = {hxi , ωi i |i = 1, . . . , N }
wobei xi jeweils einen konkreten Zustand und ωi einen nichtnegativen Wichtungsfaktor darstellt. Die Stützstellen beziehungsweise Partikel, werden dabei entsprechend der
kontinuierlichen Verteilung des Zustandes gewählt. Dies ist möglich, da eine Dualität
zwischen den Stichproben (Stützstellen) einer Verteilung und der Verteilung selbst,
aus der sie gezogen wurden, besteht [Smith und Gelfand, 1992]. Umgekehrt lässt sich
die ursprüngliche Verteilung jederzeit aus den Stichproben rekonstruieren (z.B. durch
Histogramm-Schätzung [Thrun et al., 2005] oder Kernel Density Estimation [Silverman, 1986]). Diese Annahme gilt allerdings nur für eine ausreichend große Anzahl von
Partikeln. Nach dem Gesetz der Großen Zahlen, ist die Beschreibung durch unendlich
viele Partikel identisch mit der kontinuierlichen Verteilungsfunktion. Sind es weniger
Partikel, handelt es sich dagegen um eine Approximation. In Abbildung 3.3 ist die Idee
der Darstellung durch Partikel für eine Normalverteilung zu sehen. Anstatt die Dichteverteilung durch die exponentielle Form der Normalverteilung darzustellen, werden
zufällig gezogene Partikel genutzt. Man erkennt, dass an den Stellen, an denen viele
Partikel dicht beieinander liegen (die räumliche Dichte ist hoch), die Wahrscheinlichkeit dafür, dass der wahre Systemzustand sich an diesem Ort befindet, sehr hoch ist.
Durch diese nichtparametrische Repräsentation ist es möglich, eine Vielzahl von Verteilungen zu beschreiben. Die korrekte nichtlineare Transformation der Partikel ist in
Abbildung 3.3 ebenfalls ersichtlich.
26
3.1 Grundgedanke
y
f (x)
x
P (x)
P (y)
Abbildung 3.3: Beschreibung und Transformation einer Wahrscheinlichkeitsdichteverteilung durch Partikel. Im unteren rechten Graph wurden 100 Stichproben (Partikel) aus der
Normalverteilung P (x) gezogen. Nachdem die Partikel durch die nichtlineare Funktion f (x)
transformiert wurden, sind sie entsprechend der Dichtefunktion P (y) verteilt (linker oberer
Graph) (Quelle: [Thrun et al., 2005]).
27
3 Partikel-Filter
Für die Herleitung des Partikel-Filters wird von einem nichtlinearen System- und Messmodell ausgegangen:
~xk = f~(~xk−1 ) + ω
~k
~zk = ~h(~xk ) + ~vk
(3.1)
(3.2)
Das Systemrauschen ω
~ k und das Messrauschen ~vk sind unkorreliert und werden durch
die Dichtefunktion Pω~ k und P~vk beschrieben. Es muss sich nicht zwangsläufig um eine
Normalverteilung handeln. Aus dem System- und Messmodell und den jeweiligen Dichtefunktionen des Rauschens folgt die wahrscheinlichkeitstheoretische Beschreibung des
Systemmodells, die Übergangswahrscheinlichkeit
P (xk |xk−1 ) = F(xk , f (xk−1 ), Pwk )
(3.3)
und die Beschreibung des Messmodells, die Messwahrscheinlichkeit
P (zk |xk ) = F(zk , h(xk ), Pvk ).
(3.4)
3.1.1 Repräsentation der Wahrscheinlichkeitsdichtefunktion
Die Idee des Partikel-Filters beruht auf der Darstellung einer beliebigen Wahrscheinlickeitsdichte durch eine bestimmte Anzahl von Partikeln. Durch N zufällig (aus der
Verteilung) gezogene Partikel
xi ∝ P (x|Z)
(3.5)
kann die Dichte P (x|Z) durch folgende Summe approximiert werden (Das Symbol ∝
aus Gleichung (3.5) repräsentiert hierbei das Ziehen einer Zufallszahl entsprechend
einer Wahrscheinlichkeitsdichte):
P (x|Z) ≈
N
X
ωi · δ(x − xi ).
(3.6)
i=1
P
Die Summe aller Gewichte ωi muss 1 ergeben: N
i=1 ωi = 1. Oft werden die einzelnen
Gewichte in der Initalisierungsphases des Partikel-Filters mit ωi = 1/N angenommen.
Das Kronecker-Delta δ(x), ist eine Funktion, die durch folgende Eigenschaft gekenn-
28
3.2 Sequentielle Monte-Carlo-Methode
zeichnet ist:
(
δ(x) =
0 wenn x 6= 0
1 wenn x = 0
Für N → ∞ verschwindet der Approximationsfehler und die Partikel-Darstellung
entspricht der kontinuierlichen Dichtefunktion. Gleichung (3.6) stellt dann nicht mehr
nur eine Näherung sondern ein äquivalente Beschreibung dar.
3.2 Sequentielle Monte-Carlo-Methode
Unter der Sequentiellen Monte-Carlo-Methode versteht man ein Verfahren, das einen
rekursiven (sequentiellen) Bayes-Filter durch Monte-Carlo-Simulation umsetzt2 . Wenn
eine ausreichend große Anzahl von Partikeln, die entsprechend einer Verteilung gezogen
wurden, zur Verfügung steht, können die im Allgemeinen nicht lösbaren Integrale aus
Gleichung (2.30) und (2.37) durch eine Approximation ersetzt werden. In der Praxis
ist es allerdings nur selten möglich, die Stichproben (Partikel) direkt aus der konkreten
Verteilung zu ziehen (genau diese Verteilung soll der Partikel-Filter schätzen!). Deswegen ist eine Modifikation der ursprünglichen Monte-Carlo-Methode notwendig: das
sogenannte Importance Sampling. Kurz gesagt, werden die Partikel jetzt nicht mehr
aus der ursprünglichen Verteilung gezogen, sondern aus einer dazu proportionalen, bekannten Verteilung generiert und anschließend mit einem Proportionalitätsfaktor, dem
Gewicht, versehen. Der nächste Schritt besteht darin, das Importance Sampling auf ein
rekursives Vorgehen zu erweitern. Der Sequentielle Importance Sampling-Algorithmus
(SIS) stellt somit die einfachste Form des Partikel-Filters dar. Ohne einen zusätzlichen Selektionsschritt divergiert dieser Algorithmus allerdings mit fortlaufender Zeit.
Erst die Einführung dieses als Resampling bezeichneten Zwischenschrittes in [Gordon
et al., 1993] führt zu einem robusten Verfahren. Eine umfassende Einführung in die
Monte-Carlo-Theorie findet sich in [Robert und Casella, 2004] und [Liu, 2001].
3.2.1 Monte-Carlo-Integration
Grundsätzlich lässt sich die Zielstellung der Monte-Carlo-Methode in zwei Teilprobleme zerlegen [MacKay, 1998]:
2
Partikel-Filter sind eine konkrete Umsetzung der Sequentiellen Monte-Carlo-Methode.
29
3 Partikel-Filter
ˆ Eine bestimmte Anzahl N von Stichproben aus einer gegebenen Verteilung P (x)
zu generieren.
ˆ Den Erwartungswert einer Funktion hinsichtlich der Verteilung P (x) zu schätzen.
Innerhalb des Partikel-Filters soll die a posteriori-Wahrscheinlichkeitsdichte rekursiv
geschätzt werden. Laut Bayes-Algorithmus aus Abschnitt 2.4 ist es dazu im Allgemeinen notwendig, dass mehrdimensionale Integral (2.37) zu lösen. Die Monte-CarloIntegration stellt nun ein prinzipielles Verfahren zur numerischen Bestimmung eines
mehrdimensionalen Integrals bereit. Zur Lösung des allgemeinen Integrals
Z
(3.7)
F = g(x) dx
mit x ∈ Rnx
wird die Funktion in g(x) = f (x) · P (x) zerlegt. Es wird angenommen, dass für eine
ausreichend große Anzahl N von unabhängigen Stichproben {xi |i = 1, . . . , N } aus der
Verteilung P (x), das Integral
Z
F = f (x) · P (x) dx
(3.8)
durch die Summe
N
1 X
FN =
f (xi )
N i=1
(3.9)
ersetzt werden kann. Für N → ∞ konvergiert die geschätzte Summe FN gegen den
wahren Wert des Integrals F . Der Fehler der Monte-Carlo-Integration, e = FN −
F , ist unabhängig von der Dimension nx , nur die Anzahl der Stichproben ist ausschlaggebend. Dies ist eine wertvolle Eigenschaft der Monte-Carlo-Integration, die
sie von der deterministischen, numerischen Integration der Gitter-basierten Verfahren unterscheidet. Innerhalb des Bayes-Algorithmus entspricht P (x) der a posterioriWahrscheinlichkeitsdichte des zu schätzenden Zustandes. Im Allgemeinen kann man
allerdings aus der a posteriori-Wahrscheinlichkeitsdichte keine Stichproben ziehen, da
diese meist multivariat, nichtgaußsch und nur hinsichtlich eines Proportionalitätsfaktors bekannt ist (siehe [Doucet und de Fretias, 2001] und [Ristic et al., 2004]). Der
nächste Schritt besteht in der Einführung des Importance Sampling-Algorithmus als
Alternative zur nummerischen Integration von Gleichung (3.8).
30
3.2 Sequentielle Monte-Carlo-Methode
3.2.2 Importance Sampling
Idealerweise werden die einzelnen Stichproben X direkt aus der Zielverteilung3 P gezogen. Anschließend konvergiert die Schätzung FN nach Gleichung (3.9) für N → ∞
gegen F . In der Praxis steht jedoch oft nur eine zu P proportionale Verteilung P ∗ zur
Verfügung [MacKay, 1998],
P (x) = P ∗ (x)/Z
(3.10)
die an den Stellen x ausgewertet werden kann. Das Ziehen von Stichproben ist allgemein aber weder aus P (x) noch aus P ∗ (x) möglich. Deswegen wird eine neue, einfachere, Verteilungsdichte Q(x), aus der nun Stichproben gezogen werden können, eingeführt. Auch Q(x) kann an allen Stellen x bezüglich einer multiplikativen Konstante
ZQ ausgerechnet werden.
Q(x) = Q∗ (x)/ZQ
(3.11)
Die neu eingeführte Verteilungsdichte Q wird in der Literatur mit dem Namen Sampler,
Proposal oder Importance Density bezeichnet.
Der Importance Sampling-Algorithmus schreibt vor, dass aus der Verteilung Q N
Stichproben beziehungsweise Samples {xi }N
i=1 zu ziehen sind. Hätte man die Stichproben direkt aus der Zielverteilung P gewonnen, währe die Schätzung nach Gleichung
(3.9) ausreichend. Durch das zufällige Generieren von Stichproben aus Q ist die Schätzung allerdings für Stützstellen deren Funktionswert Q(xi ) größer als P (xi ) ist, nicht
mehr korrekt. Der Schätzalgorithmus überschätzt diesen Bereich. Ist Q(xi ) hingegen
kleiner als P (xi ), wird die Wahrscheinlichkeit an dieser Stelle xi unterschätzt. Um diesen Fehler, der durch das Ziehen aus der falschen Verteilung Q zustande kommt, zu
kompensieren, wird das Gewicht ωi eingeführt:
ωi =
P ∗ (xi )
.
Q∗ (xi )
(3.12)
Mit dem Hintergedanken, dass eine Normierung der Gewichte ωi auf Eins erforderlich
3
In der englischen Literatur wird die Zielverteilung als Target Density bezeichnet.
31
3 Partikel-Filter
ist, verändert sich Gleichung (3.9) nun zu
N
P
FN =
ωi f (xi )
i=1
N
P
.
(3.13)
ωi
i=1
Damit die Gewichte in jedem möglichen Punkt berechnet werden können, ist es wichtig,
dass sowohl P als auch Q auf der selben Grundmenge definiert sind. Des weiteren
erkennt man leicht, dass das Importance Sampling nur funktionieren kann, wenn sich
P und Q ähnlich sind, d.h überdecken. In Abbildung 3.4 ist das Importance Sampling
graphisch dargestellt. Das eigentliche Ziel, Stichproben aus der Verteilung P zu ziehen
(Abbildung 3.4a) kann nicht direkt erreicht werden. Stattdessen werden Samples aus
der Verteilung Q generiert (Abbildung 3.4b). Damit die Samples aus Q überhaupt die
Verteilung P annähern, wird nun für jede Stichprobe ein Gewicht aus dem Quotienten
P (xi )/Q(xi ) gebildet. Das Ergebnis ist die Partikel-Darstellung in Abbildung 3.4c, die
eine äquivalente Beschreibung zu Abbildung 3.4a darstellt.
3.2.3 Sequentielles Importance Sampling
Das Importance Sampling ist eine allgemeine Methode zur Lösung eines mehrdimensionalen Integrals. Statt einer nummerischen Integration, wird bei der Monte-CarloSimulation eine statistische Integration vorgenommen. Dazu werden Stichproben, denen jeweils ein Gewicht zugeordnet ist, aufsummiert. Um das Importance Sampling
auf ein nichtlineares Filterproblem im Sinne des Bayes-Algorithmus anzuwenden, ist
ein weiterer Schritt notwendig: die Rekursion. Diese sequentielle oder auch rekursive
Verarbeitung stellt den grundlegenden rekursiven Monte-Carlo-Filter, das Sequential
Importance Sampling (SIS), dar. Das Partikel-Filter oder besser der SIS-Algorithmus
ist eine Umsetzung des rekursiven Bayes-Filters durch Monte-Carlo-Simulation, wobei
P ∗ der zu schätzenden a posteriori-Wahrscheinlichkeitsverteilung entspricht.
Für die Herleitung des SIS-Algorithmus ist die Betrachtung des Zustands über der Zeit
notwendig, deswegen werden nachfolgend Zeitindizes eingeführt. Es wird von folgenden
32
3.2 Sequentielle Monte-Carlo-Methode
0, 2
0, 2
0, 1
0, 1
0
−4
−2
0
2
4
6
8
(a) Die Zielverteilung P soll durch Partikel repräsentiert werden.
0
−4
−2
0
2
4
0, 1
−2
0
2
4
6
8
(c) Um die Verteilung P korrekt darzustellen werden die
Stichproben aus Q mit einem Korrekturfaktor, dem Gewicht
P (xi )/Q(xi ), versehen.
Abbildung 3.4: Darstellung und Bedeutung der Gewichte während des Importance Sampling
im Partikel-Filter-Algorithmus.
33
8
(b) Da das Generieren von Stichproben aus P nicht möglich
ist, werden Samples aus der Proposal Density Q gezogen.
0, 2
0
−4
6
3 Partikel-Filter
Festlegungen ausgegangen:
Xk = {xj | j = 0, . . . , k}
...
Folge aller Zustände bis zum Zeitpunkt k
P (Xk |Zk )
...
Verbundwahrscheinlichkeit zum Zeitpunkt k
P (xk |Zk )
< Xki , ωki > | i = 1, . . . , N
...
marginale Dichte
...
Partikel-Repräsentation von P (Xk |Zk )
P
i
wobei Xki die Stützstellen mit den zugehörigen Gewichten ωki (diese sind auf N
i=1 ωk =
1 normiert) darstellt. Die Verbundwahrscheinlichkeit kann nun durch die gewichtete
Summe in Form von Partikeln beschrieben werden:
P (Xk |Zk ) ≈
N
X
ωki δ(Xk − Xki ).
(3.14)
i=1
Die normalisierten Gewichte ωki werden nach dem Importance Sampling aus Abschnitt
3.2.2 gewonnen, wobei man davon ausgeht, dass die dazu benötigten Stichproben Xki
aus der Proposal Density Q(Xk |Zk ) gezogen wurden:
ωki ∝
P (Xki |Zk )
.
Q(Xki |Zk )
(3.15)
Nimmt man an, dass zum Zeitpunkt k − 1 die Schätzung P (Xk−1 |Zk−1 ) vorliegt und
sich die Proposal Density in
Q(Xk |Zk ) = Q(xk |Xk−1 , Zk )Q(Xk−1 |Zk−1 )
(3.16)
zerlegen lässt, ergibt sich die neue Schätzung P (Xk |Zk ) mit Eintreffen der Messung zk
aus:
P (Xk |Zk )
Bayes
=
=
Markov
=
P (zk |Xk , Zk−1 )P (Xk |Zk−1 )
P (zk |Zk−1 )
P (zk |Xk , Zk−1 )P (xk |Xk−1 , Zk−1 )P (Xk−1 , Zk−1 )
P (zk |Zk−1 )
P (zk |xk )P (xk |xk−1 )
P (Xk−1 |Zk−1 )
P (zk |Zk−1 )
34
(3.17)
(3.18)
(3.19)
3.2 Sequentielle Monte-Carlo-Methode
wobei der Nenner P (zk |Zk−1 ) lediglich eine Normierungskonstante darstellt und
P (Xk |Zk ) sich somit proportional angeben lässt:
P (Xk |Zk )
∝
P (zk |xk )P (xk |xk−1 ) P (Xk−1 |Zk−1 ).
(3.20)
Durch Einsetzten von Gleichung (3.20) und (3.16) in (3.15) ergibt sich die Gleichung
zur Aktualisierung der Gewichte zu
i
ωk−1
z
}|
{
i
i
i
i
)
|Z
)
|x
P
(X
)P
(x
P
(z
|x
k−1
k
k−1
k k−1
k
·
.
ωki ∝
i
i
, Zk )
|Zk−1 )
Q(xik |Xk−1
Q(Xk−1
(3.21)
Offensichtlich wird das vorhergehende Gewicht eines jeden Partikels rekursiv beim
Eintreffen einer neuen Messung aktualisiert. Vereinfacht man die Proposal Density
zu Q(xk |Xk−1 , Zk ) = Q(xk |xk−1 , zk ), d.h. einem Markov-Prozess erster Ordnung, ist
es ausreichend den jeweils aktuellen Zustand eins Partikels xik zu speichern, da die
i
i
gesamte Historie von Xk−1
und Zk−1
darin enthalten ist:
ωki
∝
i
ωk−1
P (zk |xik )P (xik |xik−1 )
.
Q(xik |xik−1 , zk )
(3.22)
Mit den neuen Gewichten wird die geschätzte a posteriori-Wahrscheinlichkeitsdichte
zum Zeitpunkt k durch
P (xk |Zk ) ≈
N
X
ωki δ(xk − xik )
(3.23)
i=1
approximiert.
3.2.4 Wahl der Proposal Density
Neben der Anzahl der Partikel, ist die Wahl der Proposal Density ein weiterer Freiheitsgrad beim Entwurf eines konkreten Partikel-Filters. In [Ristic et al., 2004] wird
35
3 Partikel-Filter
gezeigt, dass die optimale4 Dichtefunktion Q sich zu
Q(xik |xik−1 , zk )opt = P (xik |xik−1 , zk )
=
P (zk |xik , xik−1 )P (xik |xik−1 )
P (zk |xik−1 )
(3.24)
ergibt. Setzt man Qopt in (3.22) ein, ergibt sich die Gleichung zur Aktualisierung der
Gewichte unter der Bedingung einer neuen Messung zu
i
P (zk |xik−1 ).
ωki ∝ ωk−1
(3.25)
Das Problem der optimalen Wahl der Proposal Density besteht darin, dass die aktualisierten Werte der Gewichte zum Zeitpunkt k berechnet werden, bevor die Partikel von
k − 1 nach k propagiert werden. Dies ist aber nur für den Spezialfall von gaußschen
Dichtefunktionen (siehe Kalman-Filter) möglich ( [Ristic et al., 2004]).
Suboptimale Wahl
Statt der Wahl von Qopt als Proposal Density, wird in der Literatur oft die Übergangswahrscheinlichkeit (siehe Gleichung (3.3) aus Abschnitt 3.1 ) vorgeschlagen:
Q(xik |xik−1 , zk ) = P (xik |xik−1 ).
(3.26)
Durch Einsetzen von Gleichung (3.26) in (3.22) ergibt sich die vereinfachte Aktualisierungsgleichung der Gewichte zu
i
ωki ∝ ωk−1
P (zk |xik ).
(3.27)
Die Wahl der Übergangswahrscheinlichkeit erlaubt eine sehr einfache Berechnung der
neuen Gewichte. Dazu werden die Werte der alten Gewichte mit der Messwahrscheinlichkeit der Messung zk unter der Bedingung des vorhergesagten Zustandes xk multipliziert. Eine ausführliche Herleitung für einen Partikel-Filter mit Proposal Density =
Übergangswahrscheinlichkeit findet sich in [Wendel, 2007].
4
Optimierungskriterium ist die Minimierung der Varianz der Gewichte.
36
3.3 Resampling
ω
ω
ω
xk+1
xk
→
x
ω
xk+2
→
x
xk+3
→
x
x
Abbildung 3.5: Graphische Darstellung der Partikel-Degeneration. Aufgrund der sequentiellen Multiplikation des vorhergehenden Gewichtes mit der Messwahrscheinlichkeit, werden
die Gewichte einiger Partikel immer kleiner, während andere gegen Eins gehen. Nach nur
drei Zeitschritten hat bereits nur noch ein Partikel ein signifikantes Gewicht, alle anderen
Partikel tragen nicht mehr zur Darstellung der Schätzung bei.
3.3 Resampling
Ein Problem des SIS-Algorithmus ist die zunehmende Verzerrung der geschätzten
Dichtefunktion (die Varianz der Gewichte steigt) mit voranschreitender Zeit. In der
Praxis gehen die Gewichte vieler Partikel schon nach wenigen Iterationsschritten des
Filters gegen Null. Folglich tragen nur noch wenige Partikel mit einem signifikanten
Gewicht zur Approximation der Dichtefunktion bei. Im Endeffekt kann die a posterioriWahrscheinlichkeit nicht mehr richtig geschätzt werden, das Filter schlägt fehl. Dieses
als Degeneration (siehe [Doucet und de Fretias, 2001] und [Ristic et al., 2004]) bekannte Phänomen resultiert direkt aus der sequentiellen Berechnung der Gewichte nach
Gleichung (3.22). Abbildung 3.5 verdeutlicht die Partikel-Degeneration graphisch. Die
Multiplikation eines ohnehin schon kleinen Gewichtes (ω 1) mit einer geringen
Messwahrscheinlichkeit führt auf ein noch viel kleineres Gewicht. Nach nur wenigen
Zeitschritten tragen einige Partikel bereits nicht mehr nummerisch zur Beschreibung
der Dichtefunktion bei, sie sind ineffektiv. Eine weitere Konsequenz aus nummerisch
unbedeutenden Gewichten ist die Verschwendung von Rechenzeit. Für jedes Partikel,
auch wenn das Gewicht noch so klein ist, wird der Vorhersageschritt und die Normierung durchgeführt, ohne das dabei ein direkter Nutzen entstehen würde: Partikel mit
winzigen Gewichten tragen nicht zur Beschreibung der Dichtefunktion bei.
Durch einen zusätzlichen Selektionsschritt innerhalb des Partikel-Filter-Algorithmus,
dem Resampling, kann dieses Problem gelöst werden. Man möchte dabei gleichgewichtete (ωki = 1/N ) Partikel5 erzeugen, die effektiv zur Beschreibung der Dichtefunktion beitragen. Der Grundgedanke besteht darin, Partikel mit einem unbedeutenden
5
Die neuen Partikel werden aus der diskreten Dichtefunktion generiert.
37
3 Partikel-Filter
Gewicht auszulöschen, während Partikel mit einem signifikanten Gewicht dupliziert
werden. Dazu transformiert man alle Partikel aus dem Partikel-Set S1 in das neue
gleichgewichtete Partikel-Set S2 :
S1 =
Resampling
xik , ωki | i = 1, . . . , N −−−−−−→ S2 = xi∗
k , 1/N | i = 1, . . . , N .
Dieser Schritt ändert an der Approximation der a posteriori-Wahrscheinlichkeitsverteilung nichts, da Partikel sowohl durch ihr Gewicht als auch durch ihre räumliche Dichte (Punktmasse) zur Beschreibung beitragen. Beide Mechanismen sind äquivalent,
das Resampling ist die dazu notwendige Transformation6 . In Abbildung 3.6 ist der
Resampling-Schritt schematisch dargestellt. Vergleicht man das eben beschriebene
Resampling mit dem Importance Sampling aus Abschnitt 3.2.2 , erkennt man sehr
gut die Gemeinsamkeit beider Verfahren. Während das Importance Sampling zum Ziehen von gewichteten Stichproben aus einer kontinuierlichen Verteilung genutzt wird,
dient das Resampling zum Generieren von Stichproben aus einer diskreten Verteilung.
Die gezogenen Samples können dann jeweils für die Lösung eines Integrals nach der
Monte-Carlo-Integration (siehe Abschnitt 3.2.1) genutzt werden.
Das Resampling muss nicht unbedingt nach jedem Aktualisierungschritt des Filters
ausgeführt werden. Es kann entweder deterministisch nach jedem k-ten Zeitschritt
oder dynamisch in Abhängigkeit eines Schwellwertes geschehen. Die Anzahl der effektiven Samples Neff liefert dazu ein Maß, das die Degeneration einer Partikel-Verteilung
angibt:
Neff =
1
N
P
i=1
mit 1 ≤ Neff ≤ N.
(3.28)
2
(ωki )
Tragen alle Partikel im gleichen Maße zur Darstellung der Dichtefunktion bei, ist
Neff = N . Hat nur ein einziges Partikel ein Gewicht mit ω = 1 ist Neff = 1. Für
alle anderen Fälle bewegt es sich dazwischen. Mit dem Quotienten NNeff kann somit ein
Schwellwert für die Durchführung des Resampling-Schrittes definiert werden.
Die Motivation und prinzipielle Funktionsweise des Resampling ist ausreichend dargestellt und wird nun durch die Beschreibung drei konkreter Resampling-Algorithmen,
die in dieser Arbeit zum Einsatz kommen, ergänzt.
6
An dieser Stelle sei auf das Importance Sampling aus Abschnitt 3.2.2 verwiesen. In Abbildung 3.4
erkennt man ebenfalls die Analogie zwischen Partikel-Gewicht und räumlicher Dichte.
38
3.3 Resampling
WDF
0
1
ω1
ω2
ω3
ω4
ω5
ω6
ω7
WDF
Abbildung 3.6: Schematische Darstellung des Resampling-Schrittes.Die Größe der Kreise
repräsentiert das Gewicht. Im oberen Teil der Abbildung sind insgesamt sieben Partikel mit
unterschiedlichen Gewichten zu sehen. In Abhängigkeit ihres jeweiligen Gewichtes werden
einige Partikel nun dupliziert andere hingegen verworfen. Das Ergebnis, die sieben gleichgewichteten Partikel, ist im unteren Teil zu sehen. Die gleiche Dichtefunktion wird nun nicht
mehr anhand des Gewichtes sondern durch die räumliche Dichte beschrieben.
39
3 Partikel-Filter
3.3.1 Resampling-Verfahren
Die Hauptaufgabe eines Resampling-Algorithmus besteht in der Transformation der
gewichteten Schätzung PN in die ungewichtete Schätzung P̂N :
PN (x) =
N
X
ωi δ(x − xi )
→
i=1
N
X
1
δ(x − x∗i ).
P̂N (x) =
N
i=1
(3.29)
Dies wird durch die Elimination von Partikeln mit niedrigem Gewicht und Vervielfältigung von Partikeln mit einem hohen Gewicht erreicht. Wobei die Gesamtzahl N der
Partikel vor und nach dem Resampling im Allgemeinen konstant bleibt. Ein günstiges
Kriterium für die Konvergenz eines Algorithmus ist die Varianz beziehungsweise Abweichung zwischen der ursprünglichen Dichtefunktion PN und der durch Resampling
entstandenen Dichtefunktion P̂N :
"Z
#
Z
2
V ar FN = E
f (x)PN (x) dx −
f (x)P̂N (x) dx
,
(3.30)
die für N → ∞ gegen Null gehen sollte.
Eine ausführliche Untersuchung der nachfolgend vorgestellten Resampling-Algorithmen
findet sich in [Hol et al., 2006] und [Douc und Cappe, 2005]. Eine Analyse hinsichtlich
der Komplexität wurde in [Bolić et al., 2004] vorgestellt.
Multinomiales Resampling
Der grundlegende Algorithmus ist das erstmals in [Gordon et al., 1993] vorgestellte
Multinomiale Resampling. Es betrachte die Vorkommen der verschiedenen Gewichte
ωi selbst als Verteilung7 , die in Form der kumulativen Dichtefunktion (CDF) aufgetragen werden (siehe Abbildung 3.7). Im eigentlichen Selektionsschritt werden N
gleichverteilte Zufallsvariablen ui aus dem Intervall [0, 1) generiert. Für jedes ui wird
anschließend aus der CDF der dazugehörige Partikel-Index bestimmt und das entsprechende Partikel gewählt. Die Gewichte der neuen Partikel werden alle auf 1/N gesetzt.
7
Genauer gesagt als multinomiale Verteilung, d.h. eine Verteilung die genau so viel Elementarereignisse besitzt, wie es Partikel gibt (also hier N Stück).
40
3.3 Resampling
CDF
1
1
ωi
0
ui
0
i
Partikel-Index
Abbildung 3.7: Selektionsschritt des Multinomialen Resampling. Es werden N gleichverteilte Zufallszahlen ui aus dem Intervall [0, 1) gezogen. Anschließend wird jeweils der Index
des zu duplizierenden Partikels aus der kumulativen Dichtefunktion (CDF) der Gewichte
bestimmt.
Der Multinomiale Resampling-Algorithmus generiert N neue Partikel, die voneinander
unabhängig sind. Jedes Partikel wird dabei in Abhängigkeit der CDF n mal dupliziert
(wobei n auch gleich Null sein kann).
Systematisches Resampling
Obwohl das gerade vorgestellte Multinomiale Resampling sehr leicht zu implementieren ist und den SIS-Algorithmus stabilisiert, wird in der Literatur (z.B. [Arulampalam
et al., 2002] und [Thrun et al., 2005]) das Systematische Resampling (SR) bevorzugt.
Dies hat u.a. zwei wichtige Gründe: Zum einen verändert das Systematische Resampling aufgrund des deterministischen Algorithmus das Partikel-Set nicht, wenn alle Partikel das gleiche Gewicht haben. Dadurch geht keine Information verloren, wenn keine
neue Messung eingearbeitet wurde. Zum anderen lässt es sich mit einer Komplexität
von O(n) implementieren, während die meisten anderen Algorithmen eine Komplexität
von O(n log n) aufweisen. Da der SR-Algorithmus das Konvergenzkriterium nach Gleichung (3.30) minimiert, wird er auch mit dem Namen Low Variance oder Minimum
Variance Sampler bezeichnet.
Statt wie beim Multinomialen Resampling N gleichgewichtete Zufallszahlen zu ziehen
und aus diesen die neuen Partikel-Indizes abzuleiten, benutzt das SR nur eine gleichverteilte Zufallszahl uSR aus dem Intervall [0, N1 ] und errechnet daraus über einen
41
3 Partikel-Filter
CDF
uSR ∼ U(0, 15 ) = 0, 13
1
1
u5 = uSR + 15 · 4 = 0, 93
u4 = uSR + 15 · 3 = 0, 73
u3 = uSR + 15 · 2 = 0, 53
u2 = uSR +
1
5
= 0, 33
u1 = uSR = 0, 13
0
0
1 2 3 4 5
Partikel-Index
Abbildung 3.8: Systematisches Resampling anhand einer diskreten Verteilung mit fünf
gleichgewichteten Partikeln. Für diesen Spezialfall liefert der SR-Algorithmus das gleiche
Ergebnis wie die Ausgangsverteilung, jedes Partikel wird genau einmal dupliziert.
sequentiellen stochastischen Prozess die neue Partikel-Verteilung. Dazu wird erneut
die kumulative Dichtefunktion aus Abbildung 3.7 benötigt. Innerhalb einer Schleife,
die N mal durchlaufen wird, wird jeweils eine deterministische Zufallszahl ui über
ui = uSR +
1
(i − 10)
N
(3.31)
berechnet und das zu duplizierende Partikel anhand des Index aus der CDF ermittelt.
In Abbildung 3.8 ist das Prinzip des Systematischen Resampling anhand einer gleichgewichteten Partikel-Verteilung zu sehen. Aufgrund des deterministischen Algorithmus
wird unabhängig von der Wahl von uSR jedes Partikel genau einmal dupliziert.
Eine beispielhafte Umsetzung des Systematischen Resamplings in Pseudocode findet
sich z.B. in [Ristic et al., 2004], [Thrun et al., 2005] und [Arulampalam et al., 2002].
Adaptives KLD-Resampling
Sowohl das Multinomiale als auch das Systematische Resampling arbeiten mit einer
konstanten Anzahl von Partikeln. Je nach Dimension des Filterproblems, kann die
Anzahl dabei sehr hoch sein. Für den Einsatz innerhalb eines dezentralen kooperativen Lokalisierungsnetzes wie in dieser Arbeit, in dem die Partikel-Verteilung ständig
zwischen den einzelnen Teilnehmern ausgetauscht werden muss, kann dies leicht zu
42
3.3 Resampling
Engpässen bezüglich der Bandbreite8 führen. Mit dem in [Fox, 2003] vorgestellten
KLD-Resampling steht ein effektiver Resampling-Algorithmus zur Verfügung, der die
Anzahl der Partikel adaptiv verändern kann.
Der Kerngedanke des adaptiven Resamplings nach KLD kann wie folgt beschrieben
werden: In jedem Iterationsschritt des Filters wird die Anzahl der Partikel so variiert, dass der Fehler zwischen der wahren a posteriori-Wahrscheinlichkeit und der
geschätzten Approximation kleiner als ist. Als Maß für die Abweichung wird dabei
auf den Kulback-Leibler-Abstand (engl. Kullback-Leibler distance, kurz KLD) zurückgegriffen:
K(PN , PM ) =
X
PN (x) log
x
PN (x)
PM (x)
(3.32)
Da die tatsächliche a posteriori Verteilung aber gar nicht bekannt ist (genau darin
besteht ja die Aufgabe des Partikel-Filters), muss ein Ersatz gefunden werden. In [Fox,
2003] wird dies durch die Einführung eines Gitters im Zustandsraum gelöst. Anhand
der Häufigkeit, wie oft ein Gitterpunkt bereits belegt ist, wird dann die Gesamtanzahl
der notwendigen Partikel berechnet. Idealerweise wird der KLD-Algorithmus dafür
sorgen, dass die Anzahl der Partikel während der Initialisierung des Filters hoch ist,
der Zustandsraum dementsprechend gut abgedeckt wird. Nachdem sich das Filter im
eingeschwungenen Zustand befindet, kann die Anzahl der Partikel verringert werden,
ohne dass sich dadurch die Qualität der Schätzung verschlechtert. Anschließend ist
eine effektive Übertragung der Partikel-Verteilung im Netzwerk möglich.
Die konkrete Implementierung des KLD-Resamplings für diese Arbeit wird in Abschnitt 5 beschrieben.
3.3.2 Weitere Verfahren und Probleme
Der Vollständigkeit halber sei auf die zwei weiteren Algorithmen, das Residual Resampling und das Stratified Resampling, hingewiesen. Besonders im Zusammenhang mit
Multi-Hypothesen-Filtern (multimodale Verteilungen) entfaltet das aus der Umfragetheorie (siehe [Bolstad, 2007]) bekannte Stratified Resampling seine Leistungsfähigkeit
( [Chen, 2003]). Grundsätzlich wird dabei der Ereignisraum in Subintervalle mit bekannten/geschätzten Wahrscheinlichkeiten zerlegt. Die Anzahl der Stichproben wird
8
Eine Untersuchung bezüglich der benötigten Bandbreite für dezentrale Partikel-Filter findet sich
in [Ong et al., 2005b].
43
3 Partikel-Filter
entsprechend dieser Intervalle aufgeteilt, wobei innerhalb eines jeden Intervalls die
weiter oben vorgestellten Resampling-Verfahren zum Einsatz kommen können.
Auch wenn der Resampling-Schritt das Problem der Partikel-Degeneration löst, führt
es unter Umständen dennoch zur sogenannten Partikel-Verarmung9 . Dies ist hauptsächlich in der Natur der diskreten Verteilung begründet. Während der Selektion im
Resampling-Algorithmus kann immer nur ein konkretes Partikel (also Ausprägung des
Zustandes) dupliziert werden. Eine Streuung ähnlich dem Ziehen aus einer kontinuierlichen Verteilung ist nicht ohne weiteres möglich. Dieses Problem verschärft sich
um so mehr, je kleiner das modellierte Prozessrauschen des Bewegungsmodells ist. Im
ungünstigsten Fall können alle Partikel auf einen einzigen Punkt im Zustandsraum
zusammenfallen, das Filter versagt. Mögliche Lösungsansätze sind zum Beispiel das
zufällige Einstreuen von Partikeln auf Kosten der Genauigkeit ( [Gustafsson et al.,
2002], [Gordon et al., 1993]) oder das Umwandeln der diskreten Verteilung in eine
kontinuierliche Darstellung vor jedem Resampling-Schritt (siehe Regularized Particle
Filter in [Ristic et al., 2004]).
3.4 Bestimmung des Erwartungswertes
In der Praxis stellt sich zu einem bestimmten Zeitpunkt innerhalb des Filteralgorithmus zwangsläufig die Frage, wo genau sich das geschätzte Fahrzeug befindet. Aus
der geschätzten Dichtefunktion muss ein konkreter Zustand extrahiert und ausgegeben werden. Die formale Definition dafür ist der Erwartungswert. Für die Berechnung
des Erwartungswertes aus einer diskreten Partikel-Verteilung gibt es prinzipiell zwei
Möglichkeiten, die auch im Rahmen dieser Arbeit implementiert wurden.
1. Aufsummieren der Partikel:
x̂k =
N
X
ωki xik
(3.33)
i=1
2. Zustand des Partikels mit maximalem Gewicht auswählen:
x̂k = xikmax
9
mit imax = max(ωki |i = 1, . . . , N )
Im Englischen mit Sample Depletion oder Sample Impoverishment bezeichnet.
44
(3.34)
3.5 Varianten des Partikel-Filters
3.5 Varianten des Partikel-Filters
Nachdem alle notwendigen Schritte für den robusten Einsatz eines Partikel-Filters
vorgestellt worden, ist in Abbildung 3.9 der vollständige Ablauf des Algorithmus zu
sehen. In Tabelle 3.1 ist ein Bruchteil der möglichen Partikel-Filter-Implementierungen
aufgelistet. Die meisten Varianten sind Erweiterungen des SIS-Algorithmus, die sich
durch einen modifizierten Resampling-Schritt voneinander unterscheiden.
Varianten
Beschreibung
SIS-Algorithmus
Sequential Importance Sampling: Die einfachste und zugleich
grundlegende Form des Partikel-Filters. Sie basiert auf der
Monte-Carlo-Integration in Verbindung mit dem Importance
Sampling. Die Gewichte der Partikel werden in jedem Iterationsschritt mit dem Wert ihres zeitlichen Vorgängers multipliziert.
Oft kommt es aufgrund der steigenden Varianz der Gewichte
nach wenigen Schritten zur Degeneration der Verteilung.
SIR-Algorithmus/
Bootstrap-Filter
Sequential Importance Resampling: Erweiterung des SISAlgorithmus um den Resampling-Schritt, wodurch das Problem
der Partikel-Degeneration vermieden wird. Das Resampling muss
hierbei nicht zwangsläufig nach jedem Iterationsschritt geschehen. Zusätzlich wird während des Importance Sampling als
Proposal Density die Übergangswahrscheinlichkeit angekommen.
Dadurch vereinfacht sich die Berechnung der Gewichte.
Auxiliary
SIR-Filter
Eine Erweiterung/Modifikation des SIR-Algorithmus bezüglich
des Resampling. Statt das Resampling nach dem Vorhersageschritt durchzuführen, wird es schon zum Zeitpunkt k − 1 unter
der Bedingung der neuen Messung zk durchgeführt. Im Endeffekt
haben die Partikel bezüglich der aktuellen Messungen eine höhere Messwahrscheinlichkeit, so dass die Degeneration der Partikel
vermieden wird und somit ein Großteil der Partikel zur Schätzung beiträgt (siehe [Ristic et al., 2004]).
Regularized
Partikel-Filter
Während der SIR-Algorithmus das Resampling aus einer diskreten Verteilung durchführt, werden die neuen Partikel beim RPF
aus einer kontinuierlichen Verteilung generiert. Da die Partikel
im Falle einer kontinuierlichen Verteilung verrauschter sind, wird
das Problem der Partikel-Verarmung somit automatisch vermieden (siehe [Ristic et al., 2004] und [Doucet und de Fretias, 2001]).
Tabelle 3.1: Gegenüberstellung der wichtigsten Partikel-Filter-Varianten.
45
3 Partikel-Filter
Partikel
initialisieren
neue Beobachtung
Partikel
generieren
Gewichte
berechnen
Gewichte
normalisieren
Erwartungswert
Schätzung
nein
Gewichte
degeneriert?
ausgeben
ja
Resampling
ja
weitere
Beobachtungen?
nein
Ende
Abbildung 3.9: Ablaufdiagramm des allgemeinen Partikel-Filter-Algorithmus.
46
4 Dezentralisierte Datenfusion
Um Informationen in einem Netzwerk miteinander zu kombinieren, gibt es prinzipiell
zwei unterschiedliche Verfahren. Es drängt sich unweigerlich die Entscheidung auf, ob
man einen zentralen oder einen dezentralen Ansatz zur Fusion der Messungen und
der Schätzungen von verschiedenen Teilnehmern im Netzwerk verwenden möchte. Auf
den ersten Blick scheint der zentrale Ansatz die meisten Vorteile mit sich zu bringen,
Kreuzkorrelationen werden implizit beachtet, die einzelnen Teilnehmer im Netzwerk
benötigen nur eine geringe Rechenleistung, da ein Großteil der Berechnungen auf der
zentralen Instanz durchgeführt wird. Andererseits stellen dezentrale Ansätze deutliche
geringere Anforderungen an die Infrastruktur, skalieren leichter und besitzen keinen
Single Point of Failure, d.h. der Ausfall der zentralen Instanz führt nicht unmittelbar
zum Versagen des ganzen Netzwerks.
Sanguino präsentiert in seiner Arbeit [Sanguino, 2008] einen zentralen Ansatz zur
Lokalisation von Personen. Die Korrelationen werden dabei implizit in einem großen
Zustandsraum mitgeschätzt und sind somit auch nach der Datenfusion der einzelnen
Teilnehmer weiterhin konsistent.
In [Karam et al., 2006b] und [Karam et al., 2006a] wird zur kooperativen Lokalisierung von Fahrzeugen ein dezentraler Algorithmus vorgestellt. Jeder Teilnehmer hat
einen großen Zustandsraum in dem zusätzlich zur eigenen Schätzung auch die Position der anderen Fahrzeuge mitgeschätzt wird. Dieser Zustandsvektor wird lediglich
durch die Messwerte der eigenen Sensoren aktualisiert und anschließend an die restlichen Teilnehmer versandt. Zusätzlich hält sich jedes Fahrzeug einen weiteren internen
Zustandsvektor, in den die empfangenen Informationen der übrigen Teilnehmer eingearbeitet werden. Um inkonsistente Schätzungen im Netzwerk zu verhindern, darf
dieser Zustandsvektor niemals nach außen gelangen.
Die größte Herausforderung für den erfolgreichen Einsatz dezentraler Methoden besteht darin, ständig eine konsistente Schätzung zu gewährleisten. Die Korrelation der
einzelnen Messungen von den unterschiedlichen Teilnehmern sind meistens nicht explizit bekannt, existieren aber. Es darf niemals vorkommen, dass die gleiche Information
47
4 Dezentralisierte Datenfusion
mehrmals mit der eigenen Schätzung fusioniert wird, ohne dass dabei besondere Vorkehrungen getroffen wurden. Wird diese Bedingung nicht beachtet, kommt es zum
sogenannten Dateninzest, d.h. das Ergebnis der Schätzung wird inkonsistent und der
Filter divergiert.
Erste Ansätze für eine konsistente dezentrale Datenfusion werden in [Julier und Uhlmann, 1997b], [Franken und Hupper, 2005] und [Niehsen, 2002] unter dem Namen Covariance Intersection vorgestellt. Die Kovarianzen zweier Schätzungen werden dabei
ohne das Wissen um die konkreten Kreuzkorrelationen kombiniert und ermöglichen
somit konsistente Schätzungen. Da diese Methode mit Kovarianzen arbeitet, ist sie
allerdings auf gaußsche Probleme beschränkt.
Eine allgemeinere Lösung, die im Partikel-Raum arbeitet, ist in [Ong et al., 2006b]
und [Ong et al., 2008] zu finden. Durch den Einsatz eines diskreten Partikel-Sets
zur Repräsentation der geschätzten Verteilungsfunktion sind nahezu alle Verteilungen, auch nichtlineare, darstellbar.
4.1 Problembeschreibung
Während der dezentralen Lokalisierung kommunizieren mehrere Teilnehmer miteinander und tauschen Informationen aus. Werden diese Informationen naiv miteinander
fusioniert, kann es zum Dateninzest kommen. Dies liegt daran, dass lokale Schätzungen unter Umständen über das Netzwerk zum ursprünglichen Sender zurückgelangen
und, wenn nicht richtig verarbeitet, zu einer inkonsistenten Schätzung führen.
Um dieses Problem zu lösen, muss eine Möglichkeit gefunden werden, die gemeinsame
Information zwischen zwei Teilnehmern aus der kommunizierten Schätzung zu entfernen, um anschließend die eigentliche Datenfusion durchzuführen. Gleichung (4.1)
ist eine allgemeine Beschreibung der Fusion zweier korrelierter Informationen i und j
(siehe [Bar-Shalom und Li, 1995]):
lokal
P x|Zi
[
Zj
entfernt
z }| { z }| {
P (x|Zi ) P (x|Zj )
∝
\ .
P x|Zi Zj
|
{z
}
(4.1)
gemeinsam
Der linke Teil der Gleichung stellt das gesucht Ergebnis, die neue fusionierte Information dar. Auf de rechten Seite tauchen sowohl die eigenen Schätzungen der jeweiligen
48
4.2 Covariance Intersection
Teilnehmer (im Zähler) als auch die gemeinsame Information (Nenner) zwischen beiden Knoten auf. Die konsistente dezentrale Datenfusion läuft somit prinzipiell auf zwei
Rechenoperationen, eine Multiplikation und eine Division, hinaus. Die Division lässt
sich allerdings nur für gaußsche Zufallsvariablen (mit bekannter Korrelation) analytisch lösen (siehe [Upcroft et al., 2005]).
4.2 Covariance Intersection
Ein weiteres Verfahren1 zur Fusion von korrelierten gaußschen Informationen ist der
Covariance Intersection-Algorithmus (CI-Algorithmus). Er wurde ursprünglich von
[Julier und Uhlmann, 1997b] vorgeschlagen und in [Niehsen, 2002] und [Franken und
Hupper, 2005] auf das Problem der dezentralen Datenfusion angewedet. Der CI-Algorithmus stellt eine konvexe Kombination Pc der Mittelwerte und Kovarianzen zweier gaußscher Schätzungen A und B mit unbekannter Korrelation Pab im Informationsraum (Invertierung der Kovarianzmatrix) dar. Der Grundgedanke kann leicht
graphisch veranschaulicht werden (siehe Abbildung 4.1a). Trägt man die KovarianzEllipsen von Pa , Pb und Pc auf, befindet sich Pc für verschiedenen Werte von Pab immer
zwischen den Schnittpunkten von Pa und Pb . Die aktualisierte Kovarianz Pc sollte so
gewählt werden, dass sie so nahe wie möglich an den Schnittpunkten der Ellipsen
von Pa und Pb liegt, damit die vorhandene Information weitestgehend effektiv genutzt
werden kann. Die konvexe Kombination2 wird durch folgende Gleichung für die neue
Kovarianz Pc und den Mittelwerte µc ausgedrückt:
Pc−1 =ωPa−1 + (1 − ω)Pb−1
µc =Pc ωPa−1 µa + (1 − ω)Pb−1 µb
(4.2)
(4.3)
wobei 0 ≤ ω ≤ 1. Der Optimierungsparameter ω beeinflusst somit die Wichtung
der einzelnen Informationsanteile von A und B. Abbildung 4.1 zeigt den Einfluss von
verschiedenen Werten für ω auf die aktualisierte Kovarianz-Ellipse von Pc .
Der CI-Algorithmus stellte eine konservative aber dennoch konsistente Methode zur
Fusion von Informationen mit unbekannter Korrelation bereit. Da er mit Kovarianzen
arbeitet, ist er allerdings auf kontinuierliche gaußsche Probleme beschränkt.
1
2
Das Ergebnis des Covariance Intersection-Algorithmus ist eine nicht-optimale Lösung.
Bei einer konvexen Kombination ist ω + (1 − ω) = 1 und 0 ≤ ω ≤ 1.
49
4 Dezentralisierte Datenfusion
B
A
(a) Kovarianz-Ellipsen an der Stelle σ
von A und B. Zusätzlich sind mögliche
Kovarianz-Ellipsen Pc (gestrichelt) für verschiedenen Kreuzkorrelationen Pac eingezeichnet
(c) ω = 0, 5
(b) Ergebnis des CI-Algorithmus für
ω = 0, 1
(d) ω = 0, 9
Abbildung 4.1: Einfluss des Parameters ω auf den Covariance Intersection-Algorithmus
zum Fusionieren zweier Informationen A und B.
50
4.3 DDF im Partikel-Raum
1
0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
Partikel-Set 1
×
0
2
4
6
1
0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
Partikel-Set 2
6=
0
2
4
6
1
erwartetes Ergebnis
0.9
Partikel-Set 3
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
2
4
6
Abbildung 4.2: Die intuitive Multiplikation zweier Partikel-Verteilungen führt auf ein
falsches Ergebnis.
4.3 DDF im Partikel-Raum
Wird eine allgemeingültige Verteilung durch Partikel repräsentiert, ist eine analytische
Lösung der in Gleichung (4.1) benötigten Division nicht möglich. Das heißt, selbst wenn
die gemeinsame Information zwischen den beiden Teilnehmern bekannt ist, kann sie
vor dem Fusionsschritt nicht entfernt werden.
Aufgrund der Tatsache, dass Partikel diskrete Darstellungen einer Verteilung sind,
haben einzelne Partikel der jeweiligen Verteilungen keine gemeinsamen Stützstellen3 .
In Abbildung 4.2 ist das vermeintlich korrekte Ergebnis einer Multiplikation von zwei
Partikel-Verteilungen zu sehen. Das richtige Ergebnis dieser Multiplikation ist in Abbildung 4.3 dargestellt.
Da ein Partikel immer nur einen infinitesimal kleinen Intervall im Zustandsraum belegt und ansonsten den Wert Null besitzt, gibt es keinerlei Überschneidung zwischen
den einzelnen Stichproben. Solange zwei zu multiplizierende Partikel nicht exakt den
gleichen Zustand beschreiben, führt das Ergebnis immer auf den Wert Null (siehe Abbildung 4.3). Beispielsweise ergibt sich die Multiplikation der ersten Stichproben von
Partikel-Set 1 mit der ersten Stichprobe von Partikel-Set 2 zu: δ(x−1)·δ(x−1, 8) = 0.
Um Partikel-Verteilungen dennoch miteinander nach Gleichung (4.1) zu fusionieren,
müssen die einzelnen Verteilungen vor der Multiplikation bzw. Division in eine kon3
Im Englischen wird dies als Support bezeichnet.
51
4 Dezentralisierte Datenfusion
1
0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
Partikel-Set 1
×
0
2
4
6
1
0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
Partikel-Set 2
=
0
2
4
6
1
0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
Ergebnis
Partikel-Set 3
0
2
4
6
Abbildung 4.3: Multiplikation zweier Partikel-Verteilungen. Aufgrund der unterschiedlichen Stützstellen der jeweiligen Verteilungen ist das Ergebnis Null. Der Partikel an der
Stelle 1 (Werte 0,6) von Partikel-Set 1 wird mit dem Wert 0 an der Stelle 1 von Partikel-Set
2 multipliziert.
tinuierliche Darstellung umgewandelt werden. In den Arbeiten von [Upcroft et al.,
2005] und [Ridley et al., 2004] wird jeweils ein Verfahren zur konsistenten Fusion von
Partikel-Verteilungen vorgestellt. Die dort beschriebenen Methoden wandeln allerdings
immer beide Partikel-Verteilungen in kontinuierliche Darstellungen um und führen anschließend eine allgemeine Form der Covariance Intersection bzw. eine nummerische
Approximation durch. In dieser Arbeit soll ein Verfahren eingesetzt werden, bei dem
nur eine der beteiligten Verteilungen in eine kontinuierliche Repräsentation umgewandelt werden muss, während die andere Verteilung als Partikel-Set beibehalten werden
kann.
4.3.1 Umwandlung in eine kontinuierliche Verteilungsfunktion
Das Umwandeln einer Partikel-Verteilung in eine kontinuierliche Darstellung kann
grundsätzlich mit zwei verschiedenen Methoden durchgeführt werden: durch eine Summe von Gaußverteilungen (Gaussian Mixture Model, GMM) oder durch eine Summe
von Parzen-Kernen (Parzen Density Estimatoin).
52
4.3 DDF im Partikel-Raum
Gaussian Mixture Model
Im Falle der Approximation durch eine Summe von Gaußverteilungen ergibt sich die
kontinuierliche Verteilung zu:
P (x) =
N
X
γi Gi (x, µi , Σi )
(4.4)
i=1
wobei γi die Gewichte der einzelnen Komponenten und Gi einen Gauß-Kern mit dem
Mittelwertsvektor µi und der Kovarianz Σi darstellt. Innerhalb einer Summe von Gaußverteilungen kann also für jede Komponente eine eigene Kovarianz angenommen werden. Dieses Verfahren ist sehr flexibel und erlaubt die genaue kontinuierliche Darstellung fast jeder Verteilung. Allerdings sollte der Aufwand zur Berechnung (z.B. mit
Expectation Maximation) der einzelnen Komponenten nicht unterschätzt werden.
Parzen Density Estimation
Das Verfahren der Parzen Density Estimation ist prinzipiell ähnlich zur Darstellung
mit Hilfe einer Summe von Gaußverteilungen. Obwohl theoretisch jeder beliebige Kern
für die Parnzen Density Estimation angenommen werden kann, eignen sich GaußKerne aufgrund ihrer einfachen mathematischen Handhabbarkeit besonders gut. Im
Gegensatz zum GMM-Algorithmus wird für jede Komponente der Summe die gleiche
Kovarianz angenommen, so dass sich die Gleichung dann zu
P (x) =
N
X
γi G(x, µi , Σ)
(4.5)
i=1
ergibt. Aufgrund der einfacheren Berechnung wird für die Umwandlung in eine kontinuierliche Verteilung im Weiteren auf die Parzen Density Estimation zurückgegriffen.
Für die konkrete Anwendung auf eine Partikel-Verteilung wurde Gleichung (4.5) für
den eindimensionalen Fall4 wie folgt angepasst (siehe [Musso et al., 2001] und [Silverman, 1986]):
N
1X
wi K
fc (x) =
h i=1
4
x − xi
h
Die Gleichungen für den mehrdimensionalen Fall sind in Anhang A.1 zu finden.
53
(4.6)
4 Dezentralisierte Datenfusion
Die kontinuierliche Repräsentation fc (x) der Partikel-Verteilung ergibt sich als eine
gewichtete Summe5 von Gauß-Kernen
1 2
1
K(x) = √ e− 2 x
2π
(4.7)
die, mit einem Skalierungsparameter versehen sind. Der Skalierungsparameter h, auch
Bandbreite genannt, legt die Breite der einzelnen Kerne fest. Er ist zugleich auch ein
Einstellparameter, um die Form der resultierenden kontinuierlichen Verteilung festzulegen. Für h → 0 entsprechen die Kerne einem Dirac-Impuls und die Parzen-Summe
somit einer Partikel-Darstellung. Umso größer h gewählt wird, um so weniger Details
sind in entstehenden Verteilung zu sehen. Abbildung 4.4 verdeutlicht den Einfluss von
h auf das Ergebnis.
Ein günstiger Wert für h ist laut [Ong et al., 2005b] durch
1
hopt
1
4 d+4 − d+4
=
N
(d + 2)
(4.8)
gegeben, wobei N die Anzahl der Partikel und d die Dimension des Zustandsraums
darstellt.
4.3.2 Fusion durch Multiplikation und Division
Nachdem das Verfahren zum Umwandeln einer Partikel-Verteilung in eine kontinuierliche Darstellung vorgestellt wurde, kann nun die Multiplikation bzw. Division aus
Gleichung (4.1) durchgeführt werden. Dabei ist zu beachten, dass in der praktischen
Umsetzung von CoVelLoc die Verteilung P (x|Zj ) bereits als kontinuierliche Verteilung
vorliegt (siehe Abschnitt 5.3.2: die Einarbeitung des Differenzvektors in die PartikelVerteilung liefert als Ergebnis eine kontinuierliche Darstellung). Um eine Verteilung
in kontinuierlicher Form von einer Partikel-Verteilung abzugrenzen, wird nachfolgend
der hochgestellte Index c benutzt:
P x|Zi
[
Zj ∝
P (x|Zi )P c (x|Zj )
T
.
P (x|Zi Zj )
(4.9)
Folgender Algorithmus wird für die Fusion der Schätzungen unter Berücksichtigung
der gemeinsamen Information angewendet:
5
Die Gewichte γi werden durch die Gewichte ωi der Partikel ersetzt.
54
4.3 DDF im Partikel-Raum
0, 4
0, 2
0
−4
−3
−2
−1
0
1
2
3
1
2
3
1
2
3
(a) Bandbreite h = 0,4
0, 8
0, 6
0, 4
0, 2
0
−4
−3
−2
−1
0
(b) Bandbreite h = 0,2
0, 4
0, 3
0, 2
0, 1
0
−4
−3
−2
−1
0
(c) Bandbreite h = 0,8
Abbildung 4.4: Einfluss der Bandbreite h auf das Ergebnis der Parzen Density Estimation.
Jedes Partikel wurde durch einen Gauß-Kern ersetzt.
55
4 Dezentralisierte Datenfusion
1. Division der kontinuierlichen Verteilung P c (x|Zj ) durch die gemeinsamen InforT
mation P (x|Zi Zj ): Dazu wird die kontinuierliche Verteilung P c (x|Zj ) mit den
T
Partikeln der Verteilung P (x|Zi Zj ) abgetastet. Die sich daraus ergebenden
Wahrscheinlichkeiten l(i) werden in Gleichung
ω −(i) =
l(i)
ω (i)
(4.10)
eingesetzt und ergeben so die neuen Gewichte ω −(i) .
Die so entstandene Partikel-Verteilung P − (x|Zj ) stellt anschließend die um die
gemeinsame Information bereinigte, Schätzung des entfernten Teilnehmers dar.
2. Fusion von P (x|Zi ) mit P − (x|Zj ): Da beide Verteilungen noch in der PartikelRepräsentation vorliegen und somit keine gemeinsamen Stützstellen besitzen,
wird P − (x|Zj ) durch eine Parzen Density Estimation in die kontinuierliche Verteilung P c− (x|Zj ) umgewandelt. Anschließend kann die Multiplikation durch Abtasten von P c− (x|Zj ) mit den Partikeln aus P (x|Zi ) erfolgen. Die resultierenden
Wahrscheinlichkeiten l−(i) führen dann durch Gleichung
ω +(i) = ω (i) · l−(i)
auf die neuen Gewichte der gesuchten aktualisierten Schätzung P (x|Zi
56
(4.11)
S
Zj ).
5 Umsetzung – CoVelLoc
Das in dieser Arbeit unter dem Namen CoVelLoc (Cooperative Vehicle Localization)
vorgestellte System besteht aus einem dezentralen Netz mit unabhängigen Knoten.
Jeder einzelne Knoten schätzt ausschließlich seinen eigenen Zustand und hat kein
zusätzliches Wissen über das gesamte Netz. In Abbildung 5.1 ist die Systemarchitektur von CoVelLoc für einen Teilnehmer dargestellt. Mehrere lokale Sensoren wie
GNSS, Geschwindigkeit und Drehrate werden mit Hilfe eines Bayes-Filters fusioniert
und anhand eines nichtlinearen Bewegungsmodells (CTRV) vorhergesagt. Die Vorgehensweise funktioniert prinzipiell, führt aber wie in [Schubert et al., 2008] gezeigt
zu Lokalisationsfehlern. Deshalb erfolgt eine Erweiterung dieses klassischen Ansatzes:
Zusätzlich werden die empfangenen Positionsschätzungen der anderen Teilnehmer unter Zuhilfenahme der Pseudorange Doppeldifferenzen in die jeweils eigene Schätzung
eingearbeitet, der Lokalisierungsfehler wird kleiner. Diese verbesserte Schätzung wird
anschließend im Netzwerk an die anderen Knoten verteilt, so dass auch diese wiederum
davon profitieren können.
Das Hauptproblem besteht allerdings darin, dass die im Netzwerk ausgetauschten Informationen nicht unabhängig voneinander, d.h. korreliert sind. Wird diese Korrelation nicht berücksichtigt, kommt es zum Dateninzest, die Schätzung wird inkonsistent.
In dieser Arbeit besteht die Lösung im Einsatz eines dezentralisierten DatenfusionsAlgorithmus nach Abschnitt 4.
Die Umsetzung von CoVelLoc wurde in zwei Teilschritten durchgeführt:
1. Klassische Positionsschätzung: Als erstes wurde ein Partikel-Filter samt den in
Abschnitt 3.3 vorgestellten Resampling-Verfahren implementiert. Zusammen mit
dem CTRV-Bewegungsmodell konnte bereits eine Positionsschätzung unter Zuhilfenahme von GPS-Messungen durchgeführt werden. Eine der grundlegenden
Aufgaben bestand hierbei im Lösen des Initialisierungsproblems.
2. Dezentralisierte Positionsschätzung: Nachdem das Partikel-Filter prinzipiell funktioniert, wurde die nächste Ausbaustufe, die dezentrale Datenfusion, umgesetzt.
Der Austausch der Schätzungen der einzelnen Teilnehmer im Zusammenhang
57
5 Umsetzung – CoVelLoc
Drehrate,
Geschwindigkeit
Datenfusion
Versenden der
Schätzung an verbundene Knoten
GNSS
Pseudorange Double
Difference von
verbundenen Knoten
Transformation
in Messraum
Entfernte
Information von
verbundenen Knoten
Entfernen der
gemeinsamen
Information
Abbildung 5.1: Systemarchitektur der kooperativen Fahrzeuglokalisierung (CoVelLoc).
mit dem Differenzvektor (Pseudorange Doppeldifferenz) kam als evolutionärer
Schritt hinzu. Unter Berücksichtigung von korrelierten Informationen konnte eine
konsistente Schätzung gewährleistet werden. Diese führte letztendlich auf einen
kleineren Lokalisierungsfehler.
5.1 Ground Truth-Simulation
Diese Arbeit stellt einen ersten Schritt auf dem Weg zur dezentralen kooperativen
Fahrzeuglokalisierung in der Praxis dar. Die vorgestellten Verfahren und entwickelten
Algorithmen wurden in einer Simulation angewendet und überprüft. Da keine Fahrzeugdaten (Bewegungsprofile und dazugehörige GPS-Messungen) in Form von aufgezeichneten Datensätzen zur Verfügung standen, musste eine Möglichkeit zur Vorgabe
der Trajektorie geschaffen werden. Das als Ground Truth bezeichnete Bewegungsprofil
stellt die Referenz für die geschätzten Filterergebnisse dar. Die Abweichung zwischen
Filterergebnis und Ground Truth ist ein Maß für die Qualität des Filters und sollte
möglichst klein werden.
Im Zusammenhang mit dieser Arbeit wurde eine Anwendung zur Simulation der
Ground Truth entwickelt. Diese basiert im Grunde auf zufällig aufeinander folgen-
58
5.1 Ground Truth-Simulation
den Kreissegmenten, die innerhalb eines vorgegebenen Terrains angeordnet werden.
Die simulierten Fahrzeuge bewegen sich dann mit einem einstellbaren Geschwindigkeitsprofil auf diesen Kreissegmenten. Zusätzlich werden die Daten des ABS-Sensors
(Geschwindigkeit und Beschleunigung) sowie die Messung des fahrzeugeigenen GPSEmpfängers simuliert. Um die Simulation realistisch zu gestalten, können u.a. folgende
Parameter variiert werden:
ˆ Genauigkeit von Geschwindigkeits-, Beschleunigungs- und GPS-Messung
ˆ Frequenz der simulierten Messwerte
ˆ Abmaße des simulierten Terrains
ˆ minimaler und maximaler Radius der Kreissegmenten
ˆ Geschwindigkeitsprofil
ˆ Anzahl der simulierten Fahrzeuge
ˆ Entfernung der Fahrzeuge zueinander, damit eine Kommunikation stattfinden
kann (dadurch wird die Reichweite eines praktisch vorhandenen Übertragungsmediums simuliert)
5.1.1 Parameter der Ground Truth-Simulation
Die entscheidenden Parameter der Ground Truth-Simulation wurden für die nachfolgenden Untersuchungen, soweit nicht abweichend angegeben, wie folgt gewählt:
Parameter
Wert
Frequenz ABS-Sensor
Frequenz GPS-Messung
Standardabweichung GPS
Standardabweichung Geschwindigkeit
Standardabweichung Drehrate
Reichweite für Kommunikation
50 Hz
4 Hz
10 m
0,1 m/s
0,1 °/s
Maximalwert
Tabelle 5.1: Standardwerte für die Simulation der Ground Truth.
59
5 Umsetzung – CoVelLoc
5.2 Filter und Modelle
Für die konkrete Umsetzung des Filters in dieser Arbeit wurde ein Partikel-Filter
nach dem SIR-Algorithmus (siehe Abschnitt 3.5) implementiert, wobei die Übergangswahrscheinlickeit als Proposal Density angenommen wurde. Als Systemmodell (siehe
Gleichung (3.1)) kommt das in Abschnitt 2.6 vorgestellten CTRV-Bewegungsmodell
zum Einsatz.
Es wurden die in Abschnitt 3.3.1 vorgestellten Resampling-Verfahren implementiert
und das Systematische Resampling als Standardverfahren gewählt.
5.2.1 Zeitsynchronität
Ein Problem bei der Datenfusion mehrerer Sensoren ist die zeitliche Abfolge der einzelnen Messwerte. Sowohl ABS-Sensor als auch GPS-Empfänger liefern mit unterschiedlicher Frequenz Messwerte, die jeweils einen absoluten Zeitstempel TM ESS tragen. Das
Partikel-Filter selbst befindet sich am absoluten Zeitpunkt TP F . Praktisch können nun
folgende drei Fälle auftreten:
1. Der Zeitstempel einer Messung TM ESS ist jünger als TP F , liegt also im Vergleich
zum Partikel-Filter in der Zukunft. Um die Messung einzuarbeiten muss das
Partikel-Filter um die Zeitdifferenz ∆T = TM ESS −TP F in die Zukunft prädiziert
werden. Dies ist in der Regel der Normalfall.
2. Der Zeitstempel der Messung ist älter als TP F . Es soll ein Messwert, der bezüglich des Filters in der Vergangenheit liegt, verarbeitet werden. Prinzipiell kann
man genau wie im Fall 1 verfahren und wieder ein ∆T (diesmal mit negativem
Vorzeichen) berechnen und damit das Filter in die Vergangenheit prädizieren.
Praktisch hat man dann allerdings das Systemrauschen doppelt beaufschlagt,
so dass sich letztendlich die Frage stellt, ob das Schätzergebnis dadurch nicht
eventuell sogar ungenauer wird. In dieser Arbeit wird ein Messwert aus der Vergangenheit einfach verworfen.
3. TM ESS und TP F sind identisch. Es ist keine Prädiktion notwendig, der Messwert
kann direkt eingearbeitet werden.
Die Verarbeitung der lokalen Messwerte ist gelöst. Schwieriger gestaltet sich allerdings
das Einarbeiten einer empfangenen Schätzung von einem anderen Teilnehmer. Selbst
60
5.2 Filter und Modelle
wenn man davon ausgeht, dass alle Teilnehmer im Netzwerk eine exakt synchronisierte Uhrzeit verwenden, werden die Schätzungen praktisch immer im Bezug zur lokalen
Filterzeit TP F in der Vergangenheit liegen (dies liegt u.a. an der notwendigen Verarbeitungszeit sowie der Latenz des Übertragungsmediums). Die Schätzung nun einfach
zu verwerfen, wie in 2. vorgeschlagen, wäre ungünstig und würde eine dezentrale Datenfusion komplett verhindern. Die einfachste Möglichkeit besteht also in der Rückwärtsprädiktion des Filters. Das doppelt eingerechnete Systemrauschen nimmt man in
Kauf und hofft, dass die Informationen aus der Schätzung so wertvoll sind, dass sie das
Ergebnis verbessern. Alternativ könnten man auch eine Historie (Cache) der letzten
Systemzustände vorhalten und statt einer Rückwärtsprädiktion den zeitlich dichtesten
Zustand wählen.
In dieser Arbeit konnte die zeitliche Synchronität aufgrund der Simulation sichergestellt werden, das oben beschriebene Problem trat somit nicht auf.
5.2.2 Initialisierungsproblem
Damit ein Inertiales Navigationssystem zusammen mit einem Bewegungsmodell funktionieren kann, muss anfänglich die Initialposition bekannt sein. Dies wird im CoVelLocSzenario durch die erste empfangene GPS-Messung realisiert, vorher liefert das PartikelFilter keine Schätzwerte.
Die initiale Partikel-Verteilung P (x0 ) des dreidimensionalen Zustandsraums (x-, yPosition und Ausrichtung) wird nach dem Empfang der ersten GPS-Messung wie folgt
aufgebaut:
1. Festlegung der Partikel-Anzahl N
2. Die GPS-Messung wird als Normalverteilung mit der konkreten Messung als
Mittelwert und der Standardabweichung aus Tabelle 5.1 interpretiert. Für jedes
Partikel wird nun eine Stichprobe aus der Verteilung gezogen und damit die xbzw. y-Position des Zustandes belegt.
3. Über die Ausrichtung des Fahrzeuges ist anfangs nichts bekannt, dies wird durch
eine Gleichverteilung modelliert. Dazu wird der Winkelraum der Ausrichtung
(0-360 °) in N Bereiche aufgeteilt. Jedes Partikel bekommt also eine mögliche
Ausrichtung hinsichtlich der gewählten Auflösung zugeordnet.
Damit ist die Lösung des Initialisierungsproblems vorbereitet. Die initiale Position
wird aufgrund der ersten GPS-Messung unter Berücksichtigung der Messunsicherheit
61
5 Umsetzung – CoVelLoc
festgelegt. Da zu diesem Zeitpunkt noch keine Aussage über die Ausrichtung getroffen
werden kann, wird dieser Teil des Zustandsraums gleichverteilt (Unwissen!) angenommen.
5.2.3 Ablauf der klassischen Positionsschätzung
Der Ablauf der klassischen Positionsschätzung, wie er grundsätzlich auch in CoVelLoc
zum Einsatz kommt ist in Abbildung 5.2 zu sehen. Das Initialisierungsproblem wird
durch den Empfang der ersten GPS-Messung gelöst.
5.2.4 Ablauf der dezentralisierten Positionsschätzung
Im Vergleich zum Ablauf aus Abbildung 5.2 ändert sich im dezentralen kooperativen
Lokalisierungsszenario folgendes:
1. Zusätzlich zu den lokal vorhandenen Sensoren (ABS und GPS) wird in unregelmäßigen zeitlichen Abständen (das ist u.a. von der Sichtbarkeit der einzelnen Teilnehmer untereinander abhängig) die komplette Schätzung (im PartikelRaum) eines anderen Teilnehmers empfangen.
2. Mit jeder Schätzung wird zusätzlich ein Pseudorange Doppeldifferenz Messwertepaar übertragen. Der lokale Filter muss ebenfalls Doppeldifferenzmessungen
zu den gleichen Satelliten vorhalten.
3. Bevor die entfernte Schätzung durch einen DDF-Algorithmus eingearbeitet werden kann, müssen die Zustandsräume angeglichen werden. Dies geschieht durch
eine Verschiebung um den tatsächlichen Differenzvektor ∆d~12 ∼ N (µd , σd2 ).
4. Der Differenzvektor zwischen dem lokalen Fahrzeug und dem Sender der empfangenen Schätzung wird aus den zugehörigen Pseudorange Doppeldifferenzmessungen errechnet.
5. Die empfangene Schätzung wird um den Differenzvektor verschoben.
6. Anschließend kann die transformierte Schätzung mit einem DDF-Algorithmus
eingearbeitet werden. Dabei wird durch eine geschickte Fusion für eine konsistente Schätzung gesorgt.
7. Schlussendlich wird die neue lokale Schätzung selbst im Netzwerk zusammen mit
den aktuellen Pseudorange Doppeldifferenzen übertragen.
62
5.2 Filter und Modelle
erste GPSMessung
Partikel-Filter
initialisieren
und starten
Neue
ABS/GPSMessung
nein
ja
Partikel-Filter
zum Zeitpunkt
TM ESS
prädizieren
Messung
einarbeiten
Schätzung
ausgeben
Abbildung 5.2: Ablauf der klassischen Positionsschätzung. Innerhalb von CoVelLoc wird
das Initialisierungsproblem durch die erste GPS-Messung gelöst.
63
5 Umsetzung – CoVelLoc
5.3 Einarbeiten des Differenzvektors
In den Arbeiten [Coates, 2004], [Ong et al., 2005b] und [Ong et al., 2008] versuchen
mehrere Teilnehmer mit verschiedenen Sensorinformationen jeweils den Zustand eines
Systems zu schätzen und ihre Ergebnisse zu bestimmten Zeitpunkten zu fusionieren.
Da der Zustandsraum der zu kombinierenden Schätzungen immer identisch ist, lassen sich sofort die Algorithmen aus Abschnitt 4.3 zur dezentralisierten Datenfusion
anwenden. Wird die Datenfusion wie in [Sanguino, 2008] auf einer zentralen Instanz
durchgeführt sind ebenfalls keine weiteren Vorbereitungen notwendig.
Die in dieser Arbeit zu Grunde liegende Annahme, dass mehrere Fahrzeuge, die jeweils
nur ihre eigene Position und Ausrichtung schätzen, die Ergebnisse untereinander austauschen, führt unmittelbar zu dem Problem verschiedenartiger Zustandsräume. Eine
naive Datenfusion zweier Verteilungen aus unterschiedlichen Zustandsräumen würde
unweigerliche zu einer inkonsistenten Schätzung führen.
In Abbildung 5.3 ist die Transformation einer Schätzung um einen Differenzvektor für
zwei Fahrzeuge dargestellt. Beide Fahrzeuge bewegen sich auf einer Kreisbahn und
aktualisieren ständig ihre eigene Positions- und Ausrichtungsschätzung. Fahrzeug V2
empfängt schließlich die Schätzung x~1 von V1 . Da die beiden Zustandsräume noch nicht
zueinander passen (V1 ist am Ort (x1 ,y1 ) und V2 an (x2 ,y2 )), muss vor der Datenfusion
eine Transformation des empfangenen Zustandsraums (hier x~1 ) um den Differenzvektor ∆d~12 erfolgen. Der zweidimensionale Differenzvektor liefert allerdings nur Informationen über die relative Lage der beiden Fahrzeuge zueinander, nicht über deren
Ausrichtung. Deswegen ist vor der Transformation von x~1 eine Marginalisierung notwendig. Dabei wird die Dimension des Zustandsraums auf n = 2 reduziert. In x~1 m sind
anschließend nur noch x1 und y1 enthalten. Erst jetzt ist eine eindeutige Verschiebung
des marginalisierten Zustandsraums x~1 m um den Differenzvektor möglich. Schließlich
kann die verschobene, marginalisierte Verteilung durch einen DDF-Algorithmus mit
der eigenen Schätzung kombiniert werden. Der gesamte Ablauf ist in Algorithmus 5.1
dargestellt.
Wäre die Größe des Differenzvektor ∆d~12 genau bekannt, würde zur Verschiebung
des marginalisierten Zustandes x~1 m eine einfach Addition ausreichen. Da die Differenzvektormessung aber selbst mit einer gaußschen Unsicherheit behaftet ist, ist ein
komplexere Form der Addition beziehungsweise Verschiebung notwendig. In den folgenden Abschnitten sollen drei verschiedene Verfahren zur Verschiebung einer Wahrscheinlichkeitsverteilung unter Unsicherheit vorgestellt und verglichen werden. Um die
64
5.3 Einarbeiten des Differenzvektors
Algorithmus 5.1 : Marginalisierung und Verschiebung um Differenzvektor
ˆ Empfangen der Schätzung x~1 von V1
ˆ Differenzvektor ∆d~12 aus empfangenen Daten und Sensorinformationen
generieren
ˆ Marginalisierung von x~1 . Zustandsraum wird auf zwei Dimensionen reduziert:
x~1 = [x1 , y1 , θ1 ]
Marginalisierung
−−−−−−−−−→
x~1 m = [x1 , y1 ]
ˆ Verschieben des marginalisierten Zustandes x~1 m um ∆d~12 :
x~1 ∗ = x~1 m + ∆d~12
ˆ Mit DDF-Algorithmus fortfahren und x~1 ∗ mit x~2 fusionieren.
notwendingen Ableitungen und Vergleiche so übersichtlich wie möglich zu gestalten,
wird jeweils nur der eindimensionale Fall betrachet. Der Quellcode der Matlab-Skripte
findet sich im Anhang B.
5.3.1 Verschiebung einer Gaußverteilung unter Unsicherheit
Die Verschiebung einer Wahrscheinlichkeitsverteilung unter Unsicherheit soll zuerst für
den Fall einer kontinuierlichen Gaußverteilung gezeigt werden. Diese dient anschließend
als Grundlage und Referenz für alle weiteren Betrachtungen.
Folgende Notation wird anschließend verwendet:
P (x) . . . Wahrscheinlichkeitsverteilung der Schätzung
P (∆d) . . . Wahrscheinlichkeitsverteilung des Differenzvektors
X und ∆D sind jeweils normalverteilte Zufallsvariablen mit
X ∼ N (0, 12 )
∆D ∼ N (3, 22 ).
Da eine normalverteilte Zufallsvariable invariant gegenüber der Faltung ist, ergibt die
65
5 Umsetzung – CoVelLoc
y
P (x1 )
x1
θ2
y2
V2
x~2 = [x2 , y2 , θ2 ] P (∆x12 )
∆d~12 = [∆x12 , ∆y12 ]
+
∆x12
y1
V1
P (x∗1 )
θ1
=
x~1 = [x1 , y1 , θ1 ]
x1
x2
x
x∗1
Abbildung 5.3: Zwei Fahrzeuge V1 und V2 bewegen sich auf einer Kreisbahn. V2 empfängt
die marginalisierte Positionsschätzung von V1 und verschiebt diese um den gaußschen Differenzvektor ∆d~12 (hier nur für die x-Komponente des Zustandsraumes gezeigt). Die transformierte Schätzung x∗1 ist nun kompatibel zum Zustandsraum von V2 und kann mit einem
DDF-Algorithmus fusioniert werden. (Das bedeutet andererseits auch, dass V1 die Position
von V2 über x∗1 implizit mitschätzt.)
66
5.3 Einarbeiten des Differenzvektors
0, 4
Schätzung
Differenzvektor
Ergebnis
0, 3
0, 2
0, 1
0
−5
0
5
10
Abbildung 5.4: Verschiebung der kontinuierlichen gaußschen Schätzung um den gaußschen
Differenzvektor. Das Ergebnis ist eine flachere und breitere ( unsicherere“) Kurve. Diese
”
Kurven dienen als Referenz für die nachfolgenden Untersuchungen.
Addition zweier unabhängiger Normalverteilungen ebenfalls eine Normalverteilung.
A ∼ N (µa , σa2 )
B ∼ N (µb , σb2 )
A + B ∼ N (µa + µb , σa2 + σb2 )
(5.1)
Der Mittelwert einer Summe ist somit gleich der Summe der Mittelwerte (siehe [Bolstad, 2007]). Gleiches gilt für die Varianz, die Summe der einzelnen Varianzen entspricht der Varianz der Summe (von unabhängigen Variablen). Wird die Gleichung
(5.1) auf X und ∆D angewendet ergibt sich:
X + ∆D ∼ N (µx + µd , σx2 + σd2 ) = N (0 + 3, 12 + 22 ) = N (3, 5)
In Abbildung 5.4 ist das Ergebnis der Addition graphisch dargestellt. Die resultierende
Normalverteilung (rote Kurve) hat jetzt einen Mittelwert von 3 und eine Varianz von
5. Das entspricht dem intuitiv erwarteten Resultat, dass die Addition zweier unsicherer
Größen insgesamt unsicherer sein sollte, als die jeweiligen Summanden.
Aufbauend auf diesen Überlegungen werden nachfolgend drei verschiedenen Verfahren vorgestellt, mit denen sich eine beliebige, durch Partikel dargestellte,Verteilung
um einen gaußschen Differenzvektor verschieben lässt. Um nachzuweisen, dass die Ergebnisse mit der analytisch geschlossenen Form aus diesem Abschnitt identisch sind,
67
5 Umsetzung – CoVelLoc
werden die gleichen Ausgangsverteilungen angenommen. Die Ausgangsschätzung wird
jeweils durch Partikel dargestellt, transformiert und anschließend wieder in eine kontinuierliche Form umgewandelt, um sie mit dem hier vorliegenden Ergebnis vergleichen
zu können.
5.3.2 Verschiebung der Parzenverteilung
Die in Abschnitt 4.3.1 vorgestellte Umwandlung einer diskreten Partikel-Verteilung in
eine kontinuierliche Verteilungsfunktion (Parzen Density Estimation) dient als Grundlage für das nachfolgend mit dem Namen Uncertain Parzen Density Esitmation bezeichnete Verfahren.
Der Grundgedanke besteht darin, die Unsicherheit (Varianz) des Differenzvektors bereits in die einzelnen Gauß-Kerne, die für die Parzen Density Estimation benötigt
werden, einfließen zu lassen. Die übrigen Schritte bleiben unverändert. Die kontinuierliche Verteilungsfunktion fc (x) ergibt sich für das Partikel-Set
S = {hxi , wi i | i = 1, . . . , N }
aus
N
1X
wi K
fc (x) =
h i=1
x − xi
h
1 2
1
K(x) = √ e− 2 x
2π
h = hopt .
Die optimale Bandbreite hopt wird über Gleichung (4.8) berechnet.
Soll die kontinuierliche Verteilungsfunktion zusätzlich den Erwartungswert und die
Unsicherheit der Differenzvektors ∆d enthalten ändert sich die Gleichung wie folgt:
N
1 X
= ∗
wi K
h i−1
q
h∗ = h2opt + σd2
fc∗ (x)
x∗i = xi + µd
68
x − x∗i
h∗
(5.2)
(5.3)
(5.4)
5.3 Einarbeiten des Differenzvektors
0, 4
Schätzung (P)
Differenzvektor
0, 3
Ergebnis (UP)
0, 2
0, 1
0
−5
0
5
10
Abbildung 5.5: Die durch N=100 Partikel repräsentierte Schätzung wird um den gaußschen Differenzvektor verschoben und mit der Uncertain Parzen Density Estimation in eine
kontinuierliche Verteilung umgewandelt. (P) bedeutet, die kont. Verteilungsfunktion wurde
durch eine Parzen Density Estimation gewonnen, (UP) entspricht Uncertain Parzen Density Estimation.
Das Ergebnis dieses Algorithmus ist in Abbildung 5.5 zu sehen. Es gelten exakt die
gleichen Voraussetzungen wie in Abschnitt 5.1.1. Die Ausgangsschätzung wird jetzt
durch Partikel repräsentiert, der Differenzvektor ist weiterhin mit einer kontinuierlichen Normalverteilung beschrieben. Zusätzlich ist die kontinuierliche Verteilung fc (x),
die durch eine normale Parzen Density Estimation gewonnen wurde, zu sehen. Die
verschobene kontinuierliche Schätzung fc∗ (x) hat qualitativ die gleichen Eigenschaften
wie das Ergebnis aus Abbildung 5.4. Die entstandene Kurve ist flache und zugleich
breiter.
5.3.3 Verschiebung im Partikel-Raum
Wenn sich die Verteilung der Ausgangsschätzung durch Partikel darstellen lässt, kann
auch der gaußsche Differenzvektor durch zufällig gezogenen Partikel beschrieben werden. Die Verschiebung entspricht dann einer Addition zweier Partikel-Verteilungen.
Da jedes Partikel einen möglichen Zustand beschreibt, muss die Addition zweier PartikelVerteilungen eine Kombination jeden Partikels der einen Verteilung mit jedem Partikel
der anderen Verteilung bedeuten. Nur so können alle den Verteilungen innewohnenden
Informationen ohne Verlust transportiert werden. Abbildung 5.6 zeigt schematisch die
Addition im Partikel-Raum für einen Partikel xi der Ausgangsschätzung. Dieser wird
69
5 Umsetzung – CoVelLoc
Algorithmus 5.2 : Uncertain Parzen Density Estimation
Eingabe : {hxi , wi i}N
i=1 , Partikel-Set der Ausgangsschätzung
Eingabe : ∆D ∼ N (µd , σd2 ), Parameter des gaußschen Differenzvektors
Ergebnis : fc∗ (x), verschobenen kontinuierliche Verteilungsfunktion
ˆ Optimale Bandbreite berechnen: hopt =
1
1
4 1+4
N − 1+4
(1+2)
ˆ Modifizierte Bandbreite berechnen: h∗ =
q
h2opt + σd2
ˆ Kontinuierliche verschobenen Schätzung mit x∗i = xi + µd berechnen:
fc∗ (x)
N
1 X
x − x∗i
= ∗
wi K
h i−1
h∗
dabei n mal dupliziert und jeweils um einen zufällig gezogenen Wert aus N (µd , σd2 ),
der Verteilung des Differenzvektors, verschoben. Dieser Schritt ist für alle Partikel
i = 1, . . . , N zu wiederholen, so dass die resultierende Partikel-Verteilung dann durch
N + (N · n) Partikel beschrieben wird. Die Gewichte der duplizierten Partikel werden
von den Originalpartikeln der Ausgangsschätzung übernommen. Abschließend erfolgt
PN +N n ∗
ωi = 1 gilt.
eine Normierung, damit i=1
In Abbildung 5.7 ist das Ergebnis der Verschiebung im Partikel-Raum unter den Annahmen von Abschnitt 5.1.1 graphisch zu sehen. Obwohl die Verschiebung ausschließlich im diskreten Partikel-Raum vorgenommen wurde, sind die entsprechenden kontinuierlichen Verteilungsfunktionen (durch Parzen Density Estimation) für einen besseren Vergleich eingezeichnet. In den ersten drei Abbildungen wurde jeweils die Anzahl
der zu duplizierenden Partikel variiert. Wie aus der Monte-Carlo-Theorie vorgeben,
ist das Ergebnis nur für eine große Anzahl von Partikeln ausreichend gut. Werden nur
jeweils fünf Partikel dupliziert, ist das Ergebnis nicht befriedigend. Die resultierende
Verteilung ähnelt vielmehr der Verteilung des Differenzvektors. Wird die Unsicherheit
des Differenzvektors durch n = 50 Partikel beschrieben, ergibt sich bereits eine gute Näherung. Verglichen mit Abbildung 5.7a ist die resultierende Kurve flacher und
breiter und gibt somit die Ergebnisse aus Abbildung 5.4 wieder. Für n = 100 verbessert sich die Qualität weiter, die Kurve wird glatter. In Abbildung 5.7d wurde
n = 50 beibehalten, allerdings die Anzahl der Partikel in der Ausgangsschätzung von
100 auf N = 200 erhöht. Im Vergleich zu Abbildung 5.7b (N = 100, n = 50) ändert
sich das Ergebnis nur geringfügig. Die Qualität der verschobenen Schätzung ist somit
70
5.3 Einarbeiten des Differenzvektors
Jedes Partikel n
mal duplizieren
und jeweils um
dk=1...n ∝ N (µd , σd2 )
verschieben.
f (x)
xi
f ∗ (x)
x
x
n×
x∗i
Abbildung 5.6: Schematische Darstellung der Addition zweier Partikel-Verteilungen. Jedes Partikel wird jeweils n mal dupliziert und um eine zufällig gezogene Ausprägung aus
der Wahrscheinlichkeitsverteilung des Differenzvektors verschoben. Die verschobene PartikelVerteilung wird anschließend durch N + (N · n) Partikel repräsentiert.
hauptsächlich von der Anzahl n der duplizierten Partikel abhängig.
5.3.4 Verschiebung im Partikel-Raum mit Sigma-Punkten
Das im letzten Abschnitt vorgestellte Verfahren erzielt für eine ausreichend große Anzahl (n ≥ 50) der duplizierten Partikel gute Ergebnisse. Allerdings sollte der dabei
entstehende Rechenaufwand nicht unterschätzt werden. Wird die Ausgangsschätzung
beispielsweise durch 100 Partikel und der Differenzvektor durch 50 Partikel beschrieben, benötigt die resultierende Verteilung 5100 Partikel. Wollte man die Anzahl der
Partikel wieder reduzieren, müsste man Approximationsverfahren (z.B. auf Basis von
KLD) anwenden. Nachfolgen soll eine modifizierte Form des in Abschnitt 5.1.3 vorgestellten Algorithmus untersucht werden. Hauptkriterium ist dabei, neben der Güte,
die Anzahl der notwendigen Partikel.
Aufbauend auf dem letzten Abschnitt und den Arbeiten von [Julier und Uhlmann,
1997b] und [van der Merwe, 2004] soll die Beschreibung des gaußschen Differenzvektors jetzt nicht mehr auf zufällig gezogenen Partikeln basieren, sondern ein deterministischer Ansatz angenommen werden. Die von Julier vorgeschlagenen Transformation
(Unscented Transformation), eine Gaußverteilung durch eine feste Menge von Punkten
(Sigma-Punkte) zu nähern, dient eigentlich als Grundlagen für das Unscented Kalman
Filter, ein Form des Kalman Filters für nichtlineare Probleme. Obwohl diese Methode
in gewisser Weise Ähnlichkeit mit den Monte-Carlo-Verfahren besitzt, unterscheidet
71
5 Umsetzung – CoVelLoc
0, 4
0, 4
Schätzung (P)
Schätzung (P)
Differenzvektor
0, 3
Differenzvektor
0, 3
Ergebnis (P)
Ergebnis (P)
0, 2
0, 2
0, 1
0, 1
0
−5
0
5
10
0
−5
(a) N=100, n=5
0
10
(b) N=100, n=50
0, 4
0, 4
Schätzung (P)
Schätzung (P)
Differenzvektor
0, 3
Differenzvektor
0, 3
Ergebnis (P)
Ergebnis (P)
0, 2
0, 2
0, 1
0, 1
0
−5
5
0
5
10
0
−5
(c) N=100, n=100
0
5
(d) N=200, n=50
Abbildung 5.7: Addition zweier Partikel-Verteilungen für eine unterschiedliche Anzahl n
von duplizierten Partikeln. Wird die Anzahl der duplizierten Partikel weiter vergrößert, verbessert sich auch die Güte der Schätzung. Eine Erhöhung von N , der Anzahl der Partikel
der Ausgangsschätzung hingegen, zeigt keine Veränderung.
72
10
5.3 Einarbeiten des Differenzvektors
Algorithmus 5.3 : Verschiebung im Partikel-Raum
Eingabe : S = {hxi , wi i}N
i=1 , Partikel-Set der Ausgangschätzung
Eingabe : ∆D ∼ N (µd , σd2 ), Parameter des gaußschen Differenzvektors
Eingabe : n, Anzahl der duplizierten Partikel für jedes xi
N +N n
, verschobenen Ausgangsschätzung
Ausgabe : S ∗ = {hx∗i , ωi∗ i}i=1
ˆ Initialisierung: S ∗ = 0
ˆ FOR i = 1 : N
– i-ten Partikel aus S übernehmen und verschieben: S ∗ = S ∗ + hxi + µd , ωi i
– FOR j = 1 : n
* Ziehen von r ∼ N (µd , σd2 )
* i-ten Partikel n mal duplizieren und verrauschen:
S ∗ = S ∗ + hxi + r, ωi i
– END FOR
ˆ END FOR
ˆ Gewichte Normalisieren:
PN +N n
i=1
ωi∗ = 1
P (x)
x
xi+n
x0
q
xi
(n + λ)Σx
i
Abbildung 5.8: Beschreibung einer eindimensionalen (n = 1) Normalverteilung durch drei
deterministisch gezogene Sigma-Punkte.
73
5 Umsetzung – CoVelLoc
0, 4
0, 4
Schätzung (P)
Schätzung (P)
Differenzvektor
0, 3
Differenzvektor
0, 3
Ergebnis (P)
Ergebnis (P)
0, 2
0, 2
0, 1
0, 1
0
−5
0
5
0
−5
10
(a) α = 0, 9 und κ = 1.
0
5
10
(b) α = 1 und κ = 0, 5.
Abbildung 5.9: Addition zweier Partikel-Verteilungen mit Hilfe von deterministisch gezogenen Sigma-Punkten. Zwei verschiedene Kombinationen der Skalierungsparameter α und
κ.
sie sich doch in einem zentralen Punkt. Die Stützstellen der Verteilung werden nicht
zufällig, sondern nach einem vorgegebenen Algorithmus gezogen und mit einem entsprechenden Gewicht versehen. Im Endeffekt ist es möglich, die Informationen einer
Wahrscheinlichkeitsverteilung mit nur wenigen Punkten zu beschreiben (siehe Abbildung 5.8).
Eine n-dimensionale Zufallsvariable x mit dem Mittelwert ~x wird durch 2n + 1 gewichtete Punkte approximiert:
x0 = ~x
w0 = λ/(n + λ)
p
xi = ~x +
(n + λ)Σx
i
p
(n + λ)Σx
xi+n = ~x −
wi = 1/2(n + λ)
(5.5)
wi+n = 1/2(n + λ)
i
wobei λ = α2 (n+κ)−n einen Skalierungsparameter darstellt. α bestimmt den Abstand
der Sigma-Punkte vom Mittelwert und wird üblicherweise auf einen kleinen positiven
Wert (0, 01 ≤ α ≤ 1) gesetzt ( [van der Merwe, 2004]). Der zweite Parameter κ ist ein
weiterer Freiheitsgrad, der für gaußsche Verteilung
p meist auf κ = 3 − n gesetzt wird
( [Julier und Uhlmann, 1997b]). Der Term
n + λ)Σx entspricht der i-ten Zeile
i
(bzw. Spalte) der Wurzel der gewichteten Kovarianz Σx . Die einzelnen Sigma-Punkte
bekommen jeweils das Gewicht ωi zugeordnet.
74
5.3 Einarbeiten des Differenzvektors
Abbildung 5.9 zeigt die Ergebnisse für zwei verschiedene Kombinationen der Skalierungsparameter α und κ. Tendenziell nähern sich die Kurven dem Ergebnis aus
Abbildung 5.4 an. Allerdings bildet die Verteilungsfunktion in der Mitte eine Art Plateau aus beziehungsweise ist multimodal. Die Anzahl der Partikel in der verschobenen
Schätzung beträgt N · (2n + 1), im Beispiel also 300 Partikel. Die durch Sigma-Punkte
genäherte Verteilungsfunktion ist sehr empfindlich, schon geringfügige Änderungen der
Skalierungsparameter wirken sich merklich auf die Qualität der Approximation aus.
Möglicherweise ist eine Beschreibung des Differenzvektors in Kombination mit einer
Partikel-Verteilung durch 2n + 1 Sigma-Punkte nicht ausreichend. In einer weiterführenden Arbeit könnte zum Beispiel untersucht werden, inwieweit die Erhöhung der
Sigma-Punkte zu einer Verbesserung der Approximationsgüte beiträgt. Der Higher
Order Unscented Kalman Filter [Tenne und Singh, 2003] oder das in [Julier, 1998]
vorgestellte Verfahren könnten dabei zum Einsatz kommen. Eine Übersicht der mit
dem Namen Linear Regression Kalman Filter bezeichneten Algorithmen findet sich
in [Lefebvre et al., 2005].
Algorithmus 5.4 : Verschiebung im Partikel-Raum mit Sigma-Punkten
Eingabe : S = {hxi , wi i}N
i=1 , Partikel-Set der Ausgangschätzung
Eingabe : ∆D ∼ N (µd , σd2 ), Parameter des gaußschen Differenzvektors
Daten : n . . . Anzahl der Dimensionen
+(N ·2n)
, verschobenen Ausgangsschätzung
Ausgabe : S ∗ = {hx∗i , ωi∗ i}N
i=1
ˆ Initialisierung: S ∗ = 0
ˆ FOR i = 1 : N
– Gewicht für Sigma-Punkte berechnen: ω0∗ = λ/(n + λ)
– i-ten Partikel aus S übernehmen und verschieben: S ∗ = S ∗ + hxi + µd , ω0∗ i
– Gewicht für Sigma-Punkte berechnen: ωi∗ = 1/2(n + λ)
p
– deterministische Position des Sigma-Punktes: r =
(n + λ)Σx
m=1
– i-ten Partikel 2n mal duplizieren und deterministisch verschieben:
S ∗ = S ∗ + hxi + r, ωi∗ i
– S ∗ = S ∗ + hxi − r, ωi∗ i
– . . . abhängig von n für jede Dimension wiederholen
ˆ END FOR
ˆ Gewichte Normalisieren:
PN +(N ·2n)
i=1
ωi∗ = 1
75
5 Umsetzung – CoVelLoc
Komplexität in O-Notation
Verfahren
Uncertain Parzen Density Esitmation O(N )
Verschiebung im Partikel-Raum
O(N · n)
Verschiebung mit Sigma-Punkten
O(N )
Tabelle 5.2: Gegenüberstellung der Komplexität der vorgestellten Algorithmen zur Verschiebung einer diskreten Partikel-Verteilung unter Unsicherheit.
5.3.5 Vergleich und Auswahl
Die drei vorgestellten Verfahren liefern unter dem Gesichtspunkt der Approximationsgüte für eine günstige Wahl der entsprechenden Parameter verwertbare Ergebnisse.
Die Verteilungsfunktion der um den gaußschen Differenzvektor verschobenen Ausgangsschätzung wird in allen drei Fällen qualitativ korrekt genähert, sie ist sowohl
flacher, als auch breiter. Während das Ergebnis des ersten Verfahrens, der Uncertain
Parzen Density Estimation, eine kontinuierliche Wahrscheinlichkeitsverteilung ist, arbeiten Algorithmus 5.3 und 5.4 ausschließlich im Partikel-Raum und liefern eine diskrete Verteilung, bei der sich allerdings die Anzahl der Partikel erhöht. Die Beurteilung
der Algorithmen hinsichtlich ihrer Komplexität (bezogen auf die variable Anzahl der
Partikel) ist in Tabelle 5.2 zu sehen. Die Verschiebung im Partikel-Raum mit einem
durch Partikel beschriebenen Differenzvektor ist das aufwändigste der hier vorgestelleten Verfahren, es erhöht die Anzahl der Partikel um ein Vielfaches. Die Stärke dieser
Methode besteht aber in ihrer Flexibilität, eine Verschiebung um einen nichtgaußschen
Differenzvektor wäre damit ebenfalls möglich. Das Näherungsverfahren nach Algorithmus 5.4 ist von der Komplexität zwischen der Uncertain Parzen Density Estimation
und der Verschiebung im Partikel-Raum angesiedelt. Mit nur wenigen zusätzlichen
Partikeln (ausschließlich abhänig von der Dimension des Zustandsraumes) lässt sich
die verschobenen Verteilung beschreiben. Insgesamt liefert diese Methode allerdings
die schlechteste Approximation und reagiert sehr empfindlich auf eine Veränderung
der Skalierungsparameter.
Unter den vorgestellten Verfahren soll nun eine Auswahl getroffen werden. Da im
CoVelLoc-Szenario nach der Einarbeitung des Differenzvektors ohnehin eine DDF nach
Abschnitt 4.3 folgt, bietet es sich an, die Uncertain Parzen Density Estimation zu
wählen. Diese nimmt die Verschiebung um den Differenzvektor und die Umwandlung
in eine kontinuierliche Verteilung in einem Schritt vor. Ihr verhältnismäßig geringer
Rechenauffwand und die gute Approximationsgüten sind zwei weitere Kriterien, die
sie für eine dezentrale Datenfusion qualifizieren.
76
5.4 Vermeiden von Dateninzest
5.4 Vermeiden von Dateninzest
Dass es bei einer unbedarften Datenfusion zu einer inkonsistenten Schätzung kommt,
wurde in der Einführung zu Kapitel 4 gezeigt. Im Rahmen einer kooperativen dezentralen Lokalisierung gibt es prinzipiell mehrere Möglichkeiten. Die für CoVelLoc
möglichen Ansätze werden nachfolgend hinsichtlich ihrer Eigenschaften und Leistungsfähigkeit diskutiert.
5.4.1 Zwei unabhängige Filter pro Fahrzeug
Jedes teilnehmende Fahrzeug wird mit jeweils zwei voneinander unabhängigen PartikelFiltern zur Schätzung des eigenen Zustands ausgestattet. Dieser Ansatz geht davon
aus, dass die einfachste Form zur Vermeidung von Dateninzest darin besteht, korrelierte Information nicht weiter im Netzwerk zu verteilen. Während ein Filter (remote
PF ) ausschließlich zum Einarbeiten der empfangenen Schätzungen dient und für lokale
Zwecke verwendet werden kann, besteht die Aufgabe des anderen Filters (local PF ) im
Versenden der eigenen Schätzung (diese wurde niemals durch Informationen anderer
Teilnehmer verbessert).
Dieser einfache dezentrale Ansatz wurde im Anfangsstadium von CoVelLoc implementiert, anschließend aber verworfen. Obwohl er das Entstehen von Dateninzest effektiv
verhindert, hat er dennoch einige entscheidende Nachteile. Zum einen muss in jedem
Fahrzeug die Rechenkapazität für zwei unabhängige Partikel-Filter aufgebracht werden
und zum anderen kann es passieren, dass die beiden Filter hinsichtlich der Schätzung
divergieren. In diesem Falle beleibt nur noch das Verwerfen der Schätzung des remote
PF übrig: es handelt sich effektiv um eine klassische lokale Positionsschätzung.
Im Endeffekt ist festzustellen, dass zwei Filter pro Fahrzeug die ursprünglich gesetzte
Anforderung der Robustheit an eine dezentrale Lokalisierung nicht erfüllen.
5.4.2 Teilweise kooperative Lokalisierung
Wenn man die ursprüngliche Forderung, dass alle Teilnehmer im Netzwerk sowohl
ihre eigene Schätzung senden als auch fremde Schätzungen empfangen dürfen, etwas
einschränkt, kann eine kooperative Lokalisierung relativ einfach umgesetzt werden.
Nachfolgend soll dies am Beispiel von zwei Fahrzeugen erklärt werden.
77
5 Umsetzung – CoVelLoc
Die Rollen zwischen Fahrzeug V1 und Fahrzeug V2 sind eindeutig geklärt: Während V1
seine Schätzung im Netzwerk überträgt, ist V2 passiv und sendet keine Informationen
aus. Andererseits ist es V2 im Gegensatz zu V1 erlaubt, die empfangenen Informationen einzuarbeiten. Man könnte dieses Szenario auch mit ein Teilnehmer sendet, der
”
andere empfängt“ umschreiben. Im Endeffekt profitiert bei dieser Umsetzung nur ein
Teilnehmer, V2 , von einer genaueren Positionsschätzung. Dennoch ist hervorzuheben,
dass die Schätzungen aller Teilnehmer im Netzwerk weiterhin konsistent ist.
Der Wert dieser Methode ist vor allem theoretischer Natur und eignet sich gut zur
Beurteilung der Effektivität der kooperativen Lokalisierung. In dieser Arbeit wurde
somit beispielsweise die Verbesserung der Genauigkeit im Vergleich zur klassischen
Positionsschätzung ermittelt.
5.4.3 DDF im Partikel-Raum
Die leistungsfähigste Methode zur dezentrale kooperativen Fahrzeuglokalisierung besteht im Einsatz der in Abschnitt 4.3 vorgestellten Methode. Jeder Teilnehmer im
Netzwerk kann damit Schätzungen entfernter Fahrzeuge einarbeiten. Es muss lediglich
darauf geachtet werden, dass bekannt ist, wer bereits mit wem welche Informationen
ausgetauscht hat. Die gemeinsame Information muss dann vor jedem Fusionsschritt
herausgerechnet werden. Diese Einschränkung stellt eine Anforderung an die verwendete Netztopologie: Es muss sichergestellt werden, dass keine Schätzung auf einem
Umweg (z.B. über ein drittes Fahrzeug) zurück zum ursprünglichen Sender gelangt.
Andernfalls kann dieser nicht ohne weiteres herausfinden, welche Informationen bereits
ausgetauscht wurden. Im Gegensatz zu einem Broadcast-Netzwerk kommt deshalb in
CoVelLoc ein selbstorganisierendes baumartig strukturiertes Netzwerk1 wie in Abbildung 5.10 zu sehen, zum Einsatz.
5.5 Adaptive Anzahl der Partikel
Hinsichtlich einer späteren praktischen Umsetzung von CoVelLoc im Fahrzeug, stellt
sich die Frage nach der verfügbaren Bandbreite eines physikalischen Übertragungsmediums. Wenn man davon ausgeht, dass mehrere Teilnehmer mit einer entsprechend
hohen Frequenz ihre Schätzungen im Netzwerk verteilen, kann sehr schnell eine große
Datenmenge auflaufen. Dieses Problem lässt sich nur durch eine Verringerung der
1
In [Ong et al., 2008] wird solch ein Netzwerk auch als tree-connected network bezeichnet.
78
5.5 Adaptive Anzahl der Partikel
A
A
B
C
D
B
D
C
E
(a) Baumartige Netztopologie mit fünf
Teilnehmern.
(b) Spezialfall einer baumartigen Topologie
mit vier Teilnehmern.
Abbildung 5.10: Baumartige Netztopologien. Jeder Teilnehmer im Netzwerk kann immer
nur eine Verbindung aktiv (gerader Pfeil) initiieren. Er darf allerdings auf beliebig viele
Anfragen anderer Teilnehmer antworten (geschwungener Pfeil). Durch diese Festlegung wird
gesichert, dass zu jedem Zeitpunkt bekannt ist, welche Informationen die Teilnehmer bereits
untereinander ausgetauscht haben.
Übertragungsfrequenz oder eine Reduzierung der Partikel-Anzahl pro Schätzung entschärfen. In [Ong et al., 2005b] ist die mögliche Partikel-Anzahl pro Teilnehmer für
ein Übertragungsmedium mit gegebener Bandbreite angegeben.
Eine weitere Möglichkeit besteht in der Umwandlung der Partikel-Verteilung in eine
kompaktere Darstellung. In [Ong et al., 2005b] wird dies z.B. durch einen Summe von
Gaußverteilungen realisiert. Das Übertragen einer kontinuierlichen Verteilung ist aber
für den in dieser Arbeit betrachten Anwendungsfall ungünstig.
Eine Alternative besteht im Einsatz eines Resampling-Verfahrens mit adaptiver PartikelAnzahl. Das KLD-Resampling aus Abschnitt 3.3.1 leistet diese Anforderung. Während
der Initialisierungsphase startet das Partikel-Filter mit einer großen Anzahl (> 1000)
von Partikeln. Mit dem Eintreffen neuer GPS-Messungen und Schätzungen anderer
Netzwerkteilnehmer sinkt die Varianz der Schätzung, das Filter schwingt sich ein und
die Partikel-Anzahl kann reduziert werden.
79
5 Umsetzung – CoVelLoc
Die Bestimmung der Partikel-Anzahl innerhalb des KLD-Algorithmus läuft wie folgt
ab:
1. Diskretisieren des Zustandsraums durch ein dreidimensionales Gitter. Die Auflösung des Gitters ist ein Einstellparameter des KLD-Algorithmus. Je feiner das
Gitter, desto höher wird die resultierende Partikel-Anzahl werden.
2. Im Resampling-Schritt wird nun jeweils ein Partikel aus der bisherigen PartikelVerteilung generiert. Dazu kommt das Multinomiale Resampling zum Einsatz.
Für jedes generierte Partikel wird anschließend im Gitter nachgeschaut, ob die
entsprechende Zelle bereits belegt ist oder nicht.
3. Die Summe der belegten Zellen innerhalb des Gitter liefert indirekt die Anzahl
der notwendigen Partikel. Über eine Gleichung (siehe [Fox, 2003]) wird die neue
notwendige Anzahl nx ausgerechnet.
4. Solange die aktuelle Partikel-Anzahl kleiner als nx ist wird der Algorithmus mit
Schritt 2 fortgesetzt.
Hinsichtlich des ursprünglich in [Fox, 2003] vorgestellten KLD-Algorithmus unterscheidet sich der in dieser Arbeit implementierte Algorithmus durch folgende Eigenschaften:
1. Eine Anforderung bestand darin, das Ersetzen der Resampling-Algorithmen so
einfach wie möglich zu gestalten. Dazu war es notwendig, den Prädiktionsschritt
aus dem KLD-Algorithmus herauszulösen.
2. Um die Anzahl der belegten Zellen innerhalb des Gitters effektiv und ressourcenschonend zu ermitteln, wurde eine baumartige Datenstruktur definiert. Da es nur
darauf ankommt, zu zählen, welche Zelle wie oft belegt ist, reicht es, die Indizes
der belegten Zellen zu speichern. Für den eindimensionalen Fall soll dies an einem
Beispiel erläutert werden: Das zu betrachtende Partikel hat als x-Position einen
Wert von 200 m. Die gewählte Gitterauflösung in x-Richtung beträgt 2 m. Der Index der belegten Gitterzelle ergibt sich dann zu gi = x/xres = 200 m/2 m = 100.
Durch die gewählte Implementierung kann ein beliebig ausgedehntes Gitter umgesetzt
werden. Eine andere Möglichkeit bestünde beispielsweise in einer Art mitfahrendem
endlichen Gitter. Dazu würde der aktuelle Erwartungswert der Schätzung als Mittelpunkt dienen und das Gitter rundherum aufgebaut werden.
80
6 Ergebnisse
In diesem Kapitel erfolgt die Vorstellung und Diskussion der durch Simulation gewonnenen Resultate. Soweit nicht abweichend angegeben, wurden die in Abschnitt 5
vorgestellten Simulationsparameter verwendet. Die Ergebnisse werden in Abhängigkeit
ihrer Komplexität präsentiert. Zuerst wird die Initialisierungsphase des Partikel-Filters
gezeigt, anschließend werden die Schätzergebnisse des Filters für ein Fahrzeug in Kombination mit einem Bewegungsmodell diskutiert. Abschließend folgt ein Vergleich mit
der in dieser Arbeit entwickelten kooperativen Lokalisierungsmethode.
6.1 Initialisierungsphase
Die erfolgreiche Initialisierung des Anfangszustandes ist der erste Schritt im Ablauf des
vorgestellten Lokalisierungsalgorithmus. Erst nachdem dieser ermittelt wurde, kann die
eigentlich Schätzung der Momentanposition in Abhängigkeit von Geschwindigkeit und
Drehrate stattfinden. Aufgrund der Verfügbarkeit einer Absolutposition durch GPS
können die Komponenten x- und y-Position des Zustandsraums unter Berücksichtigung der jeweiligen Unsicherheit bereits nach dem Empfang der ersten GPS-Messung
geschätzt werden. Da eine einzelne GPS-Messung allerdings noch keine Aussage über
die mögliche Ausrichtung des Empfängers zulässt, muss diese während der Initialisierung als unbekannt angekommen werden.
In Abbildung 6.1 ist der Zustandsraum des Partikel-Filters mit 100 Partikeln kurz nach
der Initialisierung zu sehen. Die erste simulierte GPS-Messung1 lieferte für x = 10 m
und y = 10 m. Um die mit der GPS-Messung verbundene Unsicherheit zu berücksichtigen, wurden die Messwerte als Mittelwertsvektor einer zweidimensionalen Gaußverteilung interpretiert. Aus der Gaußverteilung wurden 100 Stichproben gezogen, die anschließend jeweils einem Partikel zugeordnet wurden. Die Ausrichtung θ der einzelnen
Partikel wurde entsprechend einer Gleichverteilung aus dem Intervall 0-360 °generiert.
1
Die Koordinaten innerhalb der Simulation sind jeweils auf einen fiktiven Nullpunkt bezogen.
81
6 Ergebnisse
x-y plane
Vehicle 0 particles
Vehicle 0 estimated
Vehicle 0 ground truth
20
10
0
-10
-10
0
10
20
30
x position
Abbildung 6.1: Darstellung der Partikel-Verteilung unmittelbar nach der Initialisierung.
Die grünen Kreise repräsentieren jeweils einen Partikel. Die an jedem Kreis angebrachte
Linie gibt die Ausrichtung des Partikels an.
82
6.2 Klassische Positionsschätzung
Nachdem die Partikel im Zustandsraum verteilt wurden folgt die Vorhersage der neuen
Position durch die Odometriedaten im Zusammenhang mit dem Bewegungsmodell.
Abbildung 6.2 zeigt die zeitliche Entwicklung der Partikel im Zustandsraum. Jeder
Partikel bewegt sich anhand der durch den ABS-Sensor vorgegebenen Werte von Geschwindigkeit und Drehrate (zusätzlich wird noch ein Prozessrauschen beaufschlagt).
Einkommende GPS-Messungen dienen nun zur Beurteilung der Partikel. Je näher ein
Partikel im Bereich einer GPS-Messung liegt, desto höher ist sein Gewicht. Die orange
ausgefüllten Partikel wurden als gut befunden, die weiß ausgefüllten Partikel tragen
nicht zur Schätzung bei. Zur Erzeugung dieses Bildes wurde der Resampling-Schritt
des Filters übersprungen, nur so kann man die kreisförmige Ausbreitung der Partikel
um den Mittelwert der ersten GPS-Messung während der Initialisierung zeigen. In
der realen Anwendung sogt das Resampling dafür, dass die Partikel mit einem hohen
Gewicht vervielfacht und Partikel mit einem kleinen Gewicht ausgelöscht werden. Für
die konkrete Abbildung würden sich die Partikel nach dem Resampling in Form einer
Sichel um den Punkt x = 20 und y = 10 anordnen.
Der vorgestellte Algorithmus zur Initialisierung der Anfangsposition sowie der Ausrichtung funktioniert innerhalb der Simulation gut. Die x- und y- Position werden aus
der ersten GPS-Messung abgeleitet, die Ausrichtung θ anfangs durch eine Gleichverteilung (Unwissen) repräsentiert. Über die zeitliche Entwicklung der Partikel und die
ständige Beurteilung durch weitere GPS-Messungen wird die Ausrichtung somit implizit mitgeschätzt. Denkbar wäre auch eine explizite Schätzung der Ausrichtung aus der
zeitlichen Ableitung der GPS-Messwerte. Dies war allerdings im Rahmen der Simulation nicht notwendig, könnte aber bei Problemen in der Praxis durchaus zusätzlich
eingesetzt werden.
6.2 Klassische Positionsschätzung
Die in Abschnitt 5.2.3 vorgestellte klassische Positionsschätzung ist der Ausgangspunkt für den anschließenden Vergleich mit dem dezentralen kooperativen Lösungsansatz dieser Arbeit. Die Partikel werden nach Abschnitt 6.1 initialisiert und dann anhand des Bewegungsmodells zeitlich vorhergesagt. Abbildung 6.3 zeigt die durch den
Partikel-Filter geschätzte Position eines Fahrzeuges im Vergleich zur Ground Truth.
Man erkennt, dass das Filter prinzipiell in der Lage ist, die Position des Fahrzeuges
vorherzusagen.
83
6 Ergebnisse
x-y plane
Vehicle 0 particles
Vehicle 0 estimated
Vehicle 0 ground truth
60
40
y position
20
0
-20
-40
-60
-50
0
50
100
x position
Abbildung 6.2: Zeitliche Entwicklung der Partikel im Zustandsraum kurz nach der Initialisierung. Die farbig aufgefüllten Partikel wurden mit einem hohen Gewicht beurteilt.
(Resampling ausgeschaltet)
84
6.2 Klassische Positionsschätzung
x-y plane
Vehicle 0 ground truth
Vehicle 0 estimated
0
-20
y position
-40
-60
-80
-100
-120
40
60
80
100
120
140
160
x position
Abbildung 6.3: Geschätzte Position und Ground Truth eines Fahrzeuges auf Basis der
klassischen Positionsbestimmung.
85
6 Ergebnisse
6.3 Dezentralisierte Positionsschätzung
Innerhalb der dezentralisierten kooperativen Positionsschätzung tauschen die Fahrzeuge ihre Schätzungen permanent untereinander aus. In Abbildung 6.4 ist die durch DDF
geschätzte Position eines Fahrzeuges zu sehen (die Trajektorie des zweiten Fahrzeuges
wurde aufgrund der besseren Übersicht ausgeblendet). Ein qualitativer Unterschied zu
Abbildung 6.3 , der klassischen Positionsschätzung, zeigt sich in der glatteren Schätzung. Desweiteren ist zu sehen, dass die kooperative Schätzung im Mittel näher an der
Ground Truth liegt, als beim klassischen Verfahren. Neben der Form der Trajektorie ist
die Abweichung der Schätzung von der Ground Truth ein weiteres wichtiges Kriterium
zur Beurteilung der Güte des Filters. In Abbildung 6.5 ist der mittlere quadratische
Fehler (RMSE) von drei Fahrzeugen aufgetragen. Zum Vergleich ist der Fehler der reinen GPS-Messung (grün) ebenfalls eingezeichnet. Es ist zu sehen, dass die kooperative
Lokalisierung für die gewählte Konfiguration ungefähr einen mittleren quadratischen
Fehler von 2m erreicht, während die Messungen des simulierten GPS-Empfängers lediglich Werte mit einer Abweichung von 7m liefert.
Dass sich der Einschwingvorhang der einzelnen Fahrzeuge augenscheinlich unterscheidet, ist bereits in groben Zügen in Abbildung 6.5 zu erkennen. Abbildung 6.6 zeigt eine
detaillierte Darstellung des Einschwingvorgangs anhand zweier Fahrzeuge. Die unterschiedlichen Abweichungen zu Beginn der Schätzung resultieren aus den verschiedenen Startzeitpunkten der Filter. Aufgrund der Tatsache, dass Fahrzeug V0 zeitlich vor
Fahrzeug V1 startet, kann dies anfänglich nicht von der kooperativen Lokalisierung profitieren (es ist kein weiterer Kommunikationspartner vorhanden). Im Gegenzug kann
V1 aber sofort nach dem Start die Ergebnisse der Schätzung von V0 einarbeiten. Daraus
resultiert die Einsicht, dass bei der kooperative Lokalisierung neben einer geringeren
Abweichung auch ein schnellerer Einschwingvorgang möglich ist. Wenn anfänglich keine Kommunikationspartner vorhanden sind, verhält sich das Filter wie im klassischen
Fall. Beobachtet man die Filter der verschiedenen Fahrzeuge weiter, stellt man fest,
dass sie sich aufgrund der gegenseitigen Kommunikation beide nahezu auf den selben
statischen Endwert (hier ca. 3 m) einschwingen.
6.3.1 Einfluss der Anzahl von Fahrzeugen
In Tabelle 6.1 ist der Mittelwert des mittleren quadratischen Fehlers aller teilnehmenden Fahrzeuge nach 300 Zeitschritten aufgetragen. Es ist zu sehen, dass dieser mit einer
86
6.3 Dezentralisierte Positionsschätzung
x-y plane
Vehicle 0 ground truth
Vehicle 0 estimated
0
-20
y position
-40
-60
-80
-100
-120
60
80
100
120
140
160
x position
Abbildung 6.4: Geschätzte Position und Ground Truth eines Fahrzeuges auf Basis der
dezentralisierten Positionsschätzung. Die Trajektorie des zweiten, an der Kommunikation
beteiligten, Fahrzeuges wurde ausgeblendet.
87
6 Ergebnisse
root mean squared error of position
Vehicle 0 error
Vehicle 2 error
GPS
Vehicle 3 error
Vehicle 1 error
10
9
8
RMSE in m
7
6
5
4
3
2
1
0
0
100
200
300
400
500
600
700
800
t
Abbildung 6.5: Mittlerer quadratischer Fehler des kooperativen Filters für drei Fahrzeuge.
Desweiteren ist der Fehler der GPS-Messung (grün) eingetragen (Standardabweichung σ =
7m)
root mean squared error of position
Vehicle 0 error
Vehicle 1 error
20
RMSE in m
15
10
5
0
0
100
200
300
400
500
600
700
800
t
Abbildung 6.6: Einschwingvorgang des Filters am Beispiel zweier Fahrzeuge. Fahrzeug V0
(magenta) startet zeitlich vor Fahrzeug V1 (grün). In dem Moment, in dem V1 startet kann
das Filter dieses Fahrzeuges bereits von den Schätzergebnissen von V0 profitieren.
88
6.4 Einfluss der simulierten GPS-Messung
steigenden Anzahl von Fahrzeugen abnimmt. Mehr aktive Teilnehmer verbessern also
das Schätzergebis der einzelnen Filter.
Anzahl der Fahrzeuge
1
2
3
Fahrzeug Vi
0
0
1
0
1
2
0
1
2
3
Fehler e
4,1
3,5
2,8
3,3
2,6
2,4
3,2
2,6
2,4
2,5
mittlerer Fehler ē
4,1
3,15
4
2,76
2,67
Tabelle 6.1: Mittlerer quadratischer Fehler für eine verschiedene Anzahl von kommunizierenden Fahrzeugen nach 300 Zeitschritten
6.3.2 Einfluss der Genauigkeit des Differnzvektors
Der Einfluss der Kovarianz des Differenzvektors wurde in Tabelle 6.2 überprüft. Für die
drei simulierten Fahrzeuge wurde der mittlere quadratische Fehler zu vier verschiednen Zeitpunkten ermittelt und verglichen. Ein verrauschterer Differzenzvektor führt
gleichzeitig zu einem geringeren Informationsgewinn durch die kooperative dezentrale
Schätzung und somit auch zu einer geringeren Güte des Partikel-Filters.
XXX
XXX
Zeitschritte
XXX
σ [m]
XXX
10
50
100
300
1
6,6
4
3,4
2,5
2
6,7
4,3
3,6
2,5
5
7,3
4,7
4,3
3,9
Tabelle 6.2: Mittelwert des mittleren quadratischen Fehlers von drei Fahrzeugen zu verschiedenen Zeitpunkten für unterschiedliche Werte von σ.
6.4 Einfluss der simulierten GPS-Messung
Die Messungen des simulierten GPS-Empfängers bilden die Grundlage für den Test
des in dieser Arbeit vorgestellten kooperativen dezentralen Partikel-Filters. In Abbildung 6.7 sind die Rohdaten der GPS-Messung im Vergleich zur Ground Truth eingezeichnet. Weiterhin ist die aus den GPS-Daten und dem Bewegungsmodell durch
das Filter geschätzte Position zu sehen. Man erkennt deutlich, dass die Messungen des
GPS-Empfänger sehr stark mit der eingestellten Standardabweichung um die Ground
89
6 Ergebnisse
x-y plane
Vehicle 0 ground truth
Vehicle 0 GPS measurment
Vehicle 0 estimated
y position
20
15
10
5
10
20
30
40
50
60
x position
Abbildung 6.7: Darstellung der GPS-Rohdaten im Vergleich zur daraus ermittelten Schätzung und der wahren Ground Truth.
Truth springen. Dies ist aufgrund der gewählten Implementierung durch eine gaußsche
Zufallsvariable (vereinfachte Annahme!) auch nicht anders zu erwarten. Dennoch sind
die Schätzergebnisse des Filters für diese Form der Rohdaten akzeptabel.
90
7 Zusammenfassung
In der vorliegenden Diplomarbeit wurden spezifische Methoden zur kooperativen Fahrzeuglokalisierung in dezentralen Netzwerken erarbeitet und anschließend simulativ untersucht. Durch die Kombination von lokalen Fahrzeugsensoren wie Odometrie und
GPS mit Informationen anderer Fahrzeuge konnte die geschätzte Fahrzeugposition im
Vergleich zur reinen GPS-Messung verbessert werden. Die verwendete Filtertechnologie basiert auf dem Konzept des Partikel-Filters in Verbindung mit einem nichtlinearen Bewegungsmodell für das Fahrzeug. Das Partikel-Filter wurde innerhalb eines
bestehenden Softwareframeworks in C++ zusammen mit drei üblichen ResamplingVerfahren implementiert und anschließend in einer Simulationsumgebung genutzt.
Ein Hauptprobleme währende der dezentralen Datenfusion besteht in der Vermeidung
des Dateninzest. Es konnte gezeigt werden, dass es möglich ist, die Informationen
von mehreren Fahrzeugen so miteinander zu fusionieren, dass auch eine langfristige
konsistente Schätzung möglich ist. Ausgehend vom Konzept der Covariance Intersection wurde der dazu notwendige Algorithmus für die dezentrale Fusion von PartikelVerteilungen vorgestellt und getestet.
Da die Partikel-Darstellung des Zustandsraums zwischen den einzelnen Teilnehmern
des Netzwerkes ständig ausgetauscht werden muss, bestand die Anforderung, diese
Darstellung kompakt zu halten. Dies konnte durch den Einsatz eines auf der KullbackLeibler-Divergenz beruhenden Resampling-Verfahrens gewährleistet werden. Die Anzahl der für eine Schätzung notwendigen Partikel kann so adaptiv, je nach Güte des
Schätzergebnisses, angepasst werden.
Um den Einsatz der dezentralen Lokalisierung unter Echtzeitbedingungen zu gewährleisten, wurden die vorgestellten Algorithmen soweit wie möglich auf Effizienz optimiert, ohne dabei allerdings an Allgemeinheit zu verlieren.
Die vorgeschlagene Netztopologie basiert auf einem selbstorganisierenden baumartigen
Netzwerk, dass kaum Anforderungen an die notwendige Infrastruktur stellt und somit
leicht implementiert werden kann.
91
8 Schlussbemerkung und Ausblick
Unter den gegebenen Bedingungen konnte ein erster Schritt zur Durchführung einer konsistenten dezentralen Datenfusion mit dem Schwerpunkt Fahrzeuglokalisierung
gemacht werden. Innerhalb der Simulation liefert der vorgestellte Algorithmus eine
verbesserte Positionsschätzung. Somit steht eine Basis für zukünftige Untersuchungen
und Arbeiten zur Verfügung. Im Folgenden sollen mögliche weiterführende Themen
aufgezeigt werden:
ˆ Die simulierten Rohdaten des GPS-Empfänger (siehe Abbildung 6.7) basieren auf
dem sehr einfachen Modelle einer zweidimensionalen gaußschen Zufallsvariable.
Da GPS-Fehler in der Praxis meist zum Großteil aus einem Bias sowie Sprüngen
durch Multipath-Effekt bestehen, könnte man über den Einsatz eines besseren
GPS-Fehler-Modells nachdenken. Erste Ansätze dafür finden sich beispielsweise
in [Balaguer und Carpin, 2008].
ˆ Die praktische Umsetzung des CoVelLoc-Algorithmus könnte sich im ersten
Schritt durch die Ausrüstung zweier Fahrzeuge mit GPS-Empfänger sowie Zugriff auf die Odometriedaten durchführen lassen. Der vorhandene Algorithmus
lässt sich leicht für den Online-Betrieb anpassen. Anschließend müsste die Echtzeitfähigkeit überprüft, sowie ein mögliches Kommunikationsmedium evaluiert
werden.
ˆ Der in der vorliegenden Arbeit entwickelt Algorithmus basiert auf dem allgemeinen Konzept einer Partikel-Verteilung und ist nicht auf das Problem der Fahrzeuglokalisierung fixiert. Beispielsweise könnte er sich leicht auf die Problematik
des Fußgängerschutzes in belebten Kreuzungsbereichen anwenden lassen. Auch
dort ist es notwendig, Informationen von mehreren Sensoren zu kombinieren und
konsistent zu schätzen.
92
Anhang A
Mathematische Gleichungen
A.1 Mehrdimensionale Parzen Density Estimation
Im Falle einer mehrdimensionalen Verteilung ergibt sich die Gleichung für die Parzen
Density Estimation einer Partikel-Verteilung zu (siehe [Silverman, 1986]):
N
x − Xi
1 X
fc (x) = d
wi K
,
h i=1
h
(A.1)
wobei d die Anzahl der Dimensionen angibt und nun ein multivariater Gauß-Kern
angenommen wird:
K(x) =
1
− 12 xT x
e
.
(2π)d/2
(A.2)
Die optimale Bandbreite sollte mit
hopt =
4 1/(d+4)
d+2
(A.3)
gewählt werden. Da die Bandbreite h nur als Skalar angenommen wird, besitzt der
Gauß-Kern in jeder Dimension die gleiche Ausdehnung. Unter Umständen ist es von
Vorteil, einen Bandbreitenvektor oder sogar eine Matrix einzuführen. Dadurch können
unterschiedliche Skalierungen zwischen den Dimensionen berücksichtigt werden. Sind
die Standardabweichungen innerhalb der einzelnen Dimensionen bekannt, kann der
Bandbreitenvektor z.B. durch
H = [h1 , h2 , . . . , hd ] = σi
93
4
(d + 2)
1/(d+4)
(A.4)
Anhang A Mathematische Gleichungen
gebildet werden.
Eine andere Möglichkeit besteht darin, die Daten der einzelnen Dimensionen vor der
Verarbeitung zu skalieren, um so große Unterschiede zwischen den Achsen zu kompensieren. Anschließend ist es ausreichend, die eindimensionale Form des Bandbreitenparameters h aus Gleichung (A.3) zu wählen und die Daten im Anschluß daran zurückzutransformieren.
94
Anhang B
Matlab-Skripte
Alle innerhalb dieser Diplomarbeit angefertigten Matlab-Skripte finden sich auf der
beiliegenden CD-ROM.
95
Literaturverzeichnis
[Arulampalam et al., 2002] Arulampalam, M., Maskell, S., Gordon, N., und Clapp,
T. (2002). A tutorial on particle filters for online nonlinear/non-gaussian bayesian
tracking. 50(2):174–188.
[Balaguer und Carpin, 2008] Balaguer, B. und Carpin, S. (2008). Lecture notes in
Computer Since, chapter Where Am I? A Simulated GPS Sensor for Outdoor Robotic Applications, Seiten 222–233. Springer Berlin.
[Bar-Shalom und Li, 1995] Bar-Shalom, Y. und Li, X.-R. (1995).
MultitargetMultisensor tracking: Principles and Techniques. YBS Publishing, Storrs, CT.
[Bolic et al., 2005] Bolic, M., Djuric, P., und Hong, S. (2005). Resampling algorithms
and architectures for distributed particle filters. 53(7):2442–2450.
[Bolić et al., 2004] Bolić, M., Djurić, P., und Hong, S. (2004). Resampling Algorithms
for Particle Filters: A Computational Complexity Perspective. EURASIP Journal
on Applied Signal Processing, 15:2267–2277.
[Bolstad, 2007] Bolstad, W. (2007).
Interscience, 2. Auflage.
Introduction to Bayesian Statistics.
Wiley-
[Chen, 2003] Chen, Z. (2003). Bayesian filtering: From kalman filters to particle filters,
and beyond.
[Choset et al., 2005] Choset, M. H., Hutchinson, S., Lync, K., und Kantor, G. (2005).
Principles of Robot Motion: Theory, Algorithms, and Implementation. MIT Press.
[Coates, 2004] Coates, M. (2004). Distributed particle filters for sensor networks. In
Proc. Third International Symposium on Information Processing in Sensor Networks
IPSN 2004, Seiten 99–107.
[Dellaert et al., 1999] Dellaert, F., Fox, D., Burgard, W., und Thrun, S. (1999). Monte
Carlo localization for mobile robots. In Proc. IEEE International Conference on
Robotics and Automation, Band 2, Seiten 1322–1328.
[Devroye, 1987] Devroye, L. (1987). A course in density estimation. Birkhauser Boston
Inc. Cambridge, MA, USA.
97
Literaturverzeichnis
[Douc und Cappe, 2005] Douc, R. und Cappe, O. (2005). Comparison of resampling
schemes for particle filtering. In Proc. 4th International Symposium on Image and
Signal Processing and Analysis ISPA 2005, Seiten 64–69.
[Doucet und de Fretias, 2001] Doucet und de Fretias (2001). Sequential Monte Carlo
methods in practice. Springer.
[Edelmayer et al., 2008] Edelmayer, A., Miranda, M., und Nebahaj, V. (2008). A Cooperative Filtering Approach For Enhanced Position Estimation In Ad-Hoc Vehicle
Networks. Technical report, Systems and Control Laboratory, Computer and Automation Research Insitute Hungarian Academy of Sciences.
[Fox, 2003] Fox, D. (2003). Adapting the sample size in particle filters through kldsampling. International Journal of Robotics Research, 22:2003.
[Franken und Hupper, 2005] Franken, D. und Hupper, A. (2005). Improved fast covariance intersection for distributed data fusion. In Proc. 8th International Conference
on Information Fusion, Band 1, page 7pp.
[Gordon et al., 1993] Gordon, N. J., Salmond, D. J., und Smith, A. F. M. (1993). Novel
approach to nonlinear/non-Gaussian Bayesian state estimation. IEE Proceedings F
Radar and Signal Processing, 140(2):107–113.
[Graas und Braasch, 1991] Graas, F. und Braasch, M. (1991). GPS Interferometric
Attitude and Heading Determination: Initial Flight Test Results. Navigation: Journal of the Institute of Navigation, 38, No. 4, Winter 1991-92.
[Grewal et al., 2001] Grewal, M., Weill, L., Andrews, A., und Wiley, J. (2001). Global
Positioning Systems, Inertial Navigation, and Integration. Wiley.
[Gu, 2007] Gu, D. (2007). Distributed particle filter for target tracking. In Proc. IEEE
International Conference on Robotics and Automation, Seiten 3856–3861.
[Gustafsson et al., 2002] Gustafsson, F., Gunnarsson, F., Bergman, N., Forssell, U.,
Jansson, J., Karlsson, R., und Nordlund, P.-J. (2002). Particle filters for positioning,
navigation, and tracking. 50(2):425–437.
[Hammersley und Morton, 1954] Hammersley, J. und Morton, K. (1954). Poor man’s
Monte Carlo. Journal of the Royal Statistical Society.
[Härdle, 2004] Härdle, W. (2004). Nonparametric and Semiparametric Models. Springer.
[Hol et al., 2006] Hol, J., Schön, T., und Gustafsson, F. (2006). On resampling algorithms for particle filters. In Nonlinear Statistical Signal Processing Workshop,
Cambridge, United Kingdom.
98
Literaturverzeichnis
[Julier, 1998] Julier, S. (1998). A skewed approach to filtering. In The Proceedings
of AeroSense: The 12th International Symposium on Aerospace/Defense Sensing,
Simulation and Controls., Band 3373, Seiten 54–65, Orlando FL, USA. SPIE, Signal
and Data Processing of Small Targets.
[Julier und Uhlmann, 2001] Julier, S. und Uhlmann, J. (2001). General decentralized
data fusion with Covariance Intersection. CRC Press.
[Julier und Uhlmann, 1996] Julier, S. und Uhlmann, J. K. (1996). A general method
for approximating nonlinear transformations of probability distributions. technical
report, RRG, Dept. of Engineering Science, University of Oxford.
[Julier und Uhlmann, 1997a] Julier, S. und Uhlmann, J. K. (1997a). A new extension
of the kalman filter to nonlinear systems. In In Int. Symp. Aerospace/Defense
Sensing, Simul. and Controls, Seiten 182–193.
[Julier und Uhlmann, 1997b] Julier, S. und Uhlmann, J. K. (1997b). A non-divergent
estimation algorithm in the presence of unknown correlations. In Proc. American
Control Conference the 1997, Band 4, Seiten 2369–2373.
[Julier und Uhlmann, 2004] Julier, S. und Uhlmann, J. K. (2004). Unscented filtering
and nonlinear estimation. Proceedings of the IEEE, 92(3):401–422.
[Karam et al., 2006a] Karam, N., Chausse, F., Aufrere, R., und Chapuis, R. (2006a).
Collective Localization of Communicant Vehicles Applied to Collision Avoidance. In
Intelligent Transportation Systems, 2006. Proceedings. 2006 IEEE, Seiten 442–449.
[Karam et al., 2006b] Karam, N., Chausse, F., Aufrere, R., und Chapuis, R. (2006b).
Cooperative multi-vehicle localization. In Intelligent Vehicles Symposium, 2006 IEEE, Seiten 564–570.
[Köhler, 2005] Köhler, B.-U. (2005). Konzepte der statistischen Signalverarbeitung.
Springer.
[Kotecha und Djuric, 2003] Kotecha, J. und Djuric, P. (2003). Gaussian particle filtering. 51(10):2592–2601.
[Lefebvre et al., 2005] Lefebvre, T., Bruyninckx, H., und De Schutter, J. (2005). Nonlinear Kalman filtering for force-controlled robot tasks. Springer.
[Liu, 2001] Liu, J. (2001). Monte Carlo Strategies in Scientific Computing. Springer.
[MacKay, 1998] MacKay, D. J. C. (1998). Introduction to Monte Carlo methods. In
Jordan, M. I., Herausgeber, Learning in Graphical Models, NATO Science Series,
Seiten 175–204. Kluwer Academic Press.
99
Literaturverzeichnis
[Musso et al., 2001] Musso, C., Oudjane, N., und Gland, F. L. (2001). Sequential
Monte Carlo Methods in Practice, chapter Improving Regularised Particle Filters,
Seiten 247–253. Springer.
[Niehsen, 2002] Niehsen, W. (2002). Information fusion based on fast covariance intersection filtering. In Proc. Fifth International Conference on Information Fusion,
Band 2, Seiten 901–904.
[Ong et al., 2008] Ong, L.-L., Bailey, T., Durrant-Whyte, H., und Upcroft, B. (2008).
Decentralised particle filtering for multiple target tracking in wireless sensor networks. In Proc. 11th International Conference on Information Fusion, Seiten 1–8.
[Ong et al., 2005a] Ong, L.-L., Ridley, M., Upcroft, B., Kumar, S., Bailey, T., Sukkarieh, S., und Durrant-Whyte, H. (2005a). A comparison of probabilistic representations for decentralised data fusion. In Proc. International Conference on Intelligent
Sensors, Sensor Networks and Information Processing Conference, Seiten 187–192.
[Ong et al., 2006a] Ong, L.-L., Upcroft, B., Bailey, T., Ridley, M., Sukkarieh, S., und
Durrant-Whyte, H. (2006a). A decentralised particle filtering algorithm for multitarget tracking across multiple flight vehicles. In Proc. IEEE/RSJ International
Conference on Intelligent Robots and Systems, Seiten 4539–4544.
[Ong et al., 2005b] Ong, L.-L., Upcroft, B., Ridley, M., Bailey, T., Sukkarieh, S., und
Durrant-Whyte, H. (2005b). Decentralised Data Fusion with Particles. Technical
report, ARA Centre of Excellence in Autonomous Systems (CAS).
[Ong et al., 2006b] Ong, L.-L., Upcroft, B., Ridley, M., Bailey, T., Sukkarieh, S., und
Durrant-Whyte, H. (2006b). Consistent methods for decentralised data fusion using
particle filters. In Proc. IEEE International Conference on Multisensor Fusion and
Integration for Intelligent Systems, Seiten 85–91.
[Plessis, 1997] Plessis, R. D. (1997). Poor man’s explanation of Kalman Filters or
How I stopped worrying and learned to love matrix inversion. Taygeta Scientific,
Incorporated.
[Prasad et al., 2005] Prasad, R., Ruggieri, M., und NetLibrary, I. (2005). Applied
Satellite Navigation Using GPS, GALILEO, and Augmentation Systems. Artech
House.
[Richter et al., 2008] Richter, E., Obst, M., und Schubert, R. (2008). Cooperative
Localization Algorithms for Improved Road Navigation.
[Ridley et al., 2004] Ridley, M., Upcroft, B., Ong, L. L., Kumar, S., und Sukkarieh, S.
(2004). Decentralised data fusion with parzen density estimates. In Proc. Intelligent
100
Literaturverzeichnis
Sensors, Sensor Networks and Information Processing Conference the 2004, Seiten
161–166.
[Ristic et al., 2004] Ristic, B., Arulampalam, S., und Gordon, N. (2004). Beyond the
Kalman filter. Artech House.
[Robert und Casella, 2004] Robert, C. und Casella, G. (2004). Monte Carlo Statistical
Methods. Springer.
[Rosencrantz et al., 2003] Rosencrantz, M., Gordon, G., und Thrun, S. (2003). Decentralized sensor fusion with distributed particle filters. In In Proc. of UAI.
[S. S. Blackman, 1999] S. S. Blackman, R. P. (1999). Design and Analysis of Modern
Tracking Systems. Artech House.
[Sanguino, 2008] Sanguino, J. E. (2008). Enhanced waw location-based service positioning. In The 11th International Symposium on Wireless Personal Multimedia
Communications (WPMC 2008).
[Schubert et al., 2008] Schubert, R., Richter, E., und Wanielik, G. (2008). Comparison and evaluation of advanced motion models for vehicle tracking. In Proc. 11th
International Conference on Information Fusion, Seiten 1–6.
[Scott und Sain, 2004] Scott, D. W. und Sain, S. R. (2004). Multi-Dimensional Density Estimation, Seiten 229–263. Elsevier, Amsterdam.
[Shah und Malaney, 2006] Shah, Z. und Malaney, R. (2006). Particle filters and position tracking in wi-fi networks. In Proc. VTC 2006-Spring Vehicular Technology
Conference IEEE 63rd, Band 2, Seiten 613–617.
[Silverman, 1986] Silverman, B. (1986). Density Estimation for Statistics and Data
Analysis. Chapman & Hall/CRC.
[Smith und Gelfand, 1992] Smith, A. F. M. und Gelfand, A. E. (1992). Bayesian Statistics without Tears: A Sampling-Resampling Perspective. The American Statistician, 46(2):84–88.
[Tenne und Singh, 2003] Tenne, D. und Singh, T. (2003). The higher order unscented
filter. In Proc. American Control Conference the 2003, Band 3, Seiten 2441–2446.
[Thrun et al., 2005] Thrun, S., Burgard, W., und Fox, D. (2005). Probabilistic robotics.
MIT Press.
[Upcroft et al., 2005] Upcroft, B., Ong, L., Kumar, S., Ridley, M., Bailey, T., Sukkarieh, S., und Durrant-Whyte, H. (2005). Rich probabilistic representations for
bearing only decentralised data fusion. In Proc. 8th International Conference on
Information Fusion, Band 2, Seiten 8 pp.–.
101
Literaturverzeichnis
[van der Merwe, 2004] van der Merwe, R. (2004). Sigma-Point Kalman Filters for
Probabilistic Inference in Dynamic State-Space Models. Phd thesis, OGI School of
Science & Engineering, Oregon Health & Science University.
[Wendel, 2007] Wendel, J. (2007). Integrierte Navigationssysteme. Oldenbourg, 1.
Auflage.
[Zarchan und Musoff, 2005] Zarchan, P. und Musoff, H. (c2005). Fundamentals of
Kalman filtering. American Institute of Aeronautics and Astronautics, 2. Auflage.
102
Selbstständigkeitserklärung
Hiermit erkläre ich, daß ich die vorliegende Arbeit selbstständig angefertigt, nicht anderweitig zu Prüfungszwecken vorgelegt und keine anderen als die angegebenen Hilfsmittel verwendet habe. Sämtliche wissentlich verwendete Textausschnitte, Zitate oder
Inhalte anderer Verfasser wurden ausdrücklich als solche gekennzeichnet.
Chemnitz, den 5. Februar 2009
Marcus Obst
103