Der diskrete Kalman Filter
Fachbereich: InformatikBetreuer: Marc Drassler
Patrick Winkler1168954
6. Dezember 2004
TechnischeUniversitätDarmstadt
Simulationund Systemoptimierung
DarmstadtDribblingDackels
Inhalt● Theoretischer Hintergrund
– Problemstellung– Lösungsansatz– Herleitung eindimensionaler Fall– Erweiterung zum mehrdimensionalen Fall
● Einsatz im Robocup
– Ziel– Anpassung des Kalman Filters
● Beispiele
1
Problemstellung● Das Ziel ist die Simulation von physikalischen Prozessen
● Basis bei der Modellierung eines physikalischen Prozesses ist eine ideale Modellwelt
● Wendet man dieses Modell in der Realität an, treten Probleme auf:
– Messfehler: gemessener Wert ungleich dem tatsächlichen– Effekte, die das Modell nicht berücksichtigt, z.B. Reibung
● Möglicher Ansatz: Kalman Filter
Theoretischer Hintergrund
2
Lösungsansatz● Definition Kalman Filter:
Der diskrete Kalman Filter ist eine statistische, rekursive Methode, welche vergangene, aktuelle und zukünftige Systemzustände vorhersagen kann. Dabei sind keine genauen Kenntnisse des modellierten Systems notwendig.
➔ Kann das Problem lösen!
Theoretischer Hintergrund
3
Funktionsweise Kalman FilterTheoretischer Hintergrund
ZustandsvorhersageKorrektur
Initialisierung
● Black Box wird noch gefüllt
4
Herleitung
● Zum Zeitpunkt t1 erfolgt
Messung z1
● Messung kann nur Bereich angeben, in der sich der exakte Wert mit einer gewissen Wahrscheinlichkeit befindet
● Kann durch Gauß-Glockenkurve dargestellt werden (grün)
Theoretischer Hintergrund
x-Achse: Qualität der Messungy-Achse: Wahrscheinlichkeit des Wertes
5
Herleitung
● Zweite Messung z2 zum
Zeitpunkt t2 kommt hinzu
(blau)● Kombination der beiden
Messwerte➔ Gewichtete
Linearkombination:
Geschätzter Wert zum Zeitpunkt t
2:
Theoretischer Hintergrund
x t2=k 1∗z1k 2∗z2
6
x-Achse: Qualität der Messungy-Achse: Wahrscheinlichkeit des Wertes
Herleitung● Der Schätzwert (rot) hat
eine geringere Varianz als die Einzelmessungen
● Die Faktoren k1 und k
2
werden durch Methoden der Statistik bestimmt
Theoretischer Hintergrund
x t2=k 1∗z1k 2∗z2
7
x-Achse: Qualität der Messungy-Achse: Wahrscheinlichkeit des Wertes
Herleitung● Sei
● Definition Kalman Gain :
Legt fest, wie stark die Differenz zwischen dem vorherigen Schätzwert und der aktuellen Messung in eine weitere Schätzung eingeht.
➔ Neue Gleichung für den geschätzten Wert
● Zusätzlich Varianz für geschätzten Wert
Theoretischer Hintergrund
xt1=z1
x t2= x t1K t∗ z2− x t1
K t
8
Übersicht
ZustandsvorhersageKorrektur
Initialisierung
(1) Berechnung Kalman Gain
(2) Aktualisierung Schätzung
(3) Aktualisierung Fehlerkovari-anz (Varianz des geschätzten Wertes)
Theoretischer Hintergrund
9
Herleitung● Änderung des Signals durch einen messbaren Faktor u
● Erhalten zusätzliche Gleichung
● Mit wird der Schätzwert bezeichnet, in dem die aktuelle Messung zum Zeitpunkt t
n noch nicht berücksichtigt
wurde
● Dazu kommt die Varianz dieser Schätzung
Theoretischer Hintergrund
x t n−= x t n−1u∗dt
x t n−
10
Übersicht
ZustandsvorhersageKorrektur
Initialisierung
(1) Berechnung Kalman Gain
(2) Aktualisierung Schätzung
(3) Aktualisierung Fehlerkovari-anz (Varianz des geschätzten Wertes)
Theoretischer Hintergrund
(1) Schätzung Zustand
(2) Vermuteter Schätzfehler
● Eindimensionaler Fall abgedeckt
11
Mehrdimensionaler Fall● Seien die zu untersuchenden Messwerte mehrdimensional,
z.B. die Position eines Objektes im Raum
● Sei n = Dimension des Messvektors
● Aus Koeffizienten werden Vektoren und n x n Matrizen
● Sämtliche Matrizen auf der rechten Seite der folgenden Gleichungen können sich von Zustand zu Zustand ändern
Theoretischer Hintergrund
12
Schätzung des Systemzustands●
● Auch „a priori“ Zustand genannt
● A beschreibt das Verhalten des Systems, d.h. wie schließt man vom vorherigen zum nächsten Zustand
– Zum Beispiel Rotationsmatrix wenn x die Position im Raum angibt
● B dient einer optionalen Kontrolleingabe
Theoretischer Hintergrund
xk−=A xk−1Buk
13
Vermuteter Schätzfehler●
● Gibt die Unsicherheit bei der Projektion des Zustandes an
● Je kleiner P, desto genauer
● Q ist die Process Noise Covariance Matrix
– Hängt davon ab, wie genau der Prozess ist, z.B. beschränkte Dezimalstellen nach dem Komma
– Große Werte in Q stehen für große Ungenauigkeit
Theoretischer Hintergrund
P k−=AP k−1 A
TQ
14
ÜbersichtTheoretischer Hintergrund
P k−=AP k−1 A
TQ
(1) Schätzung Systemzustand
(2) Vermuteter Schätzfehler
xk−=A xk−1Buk
Zustandsvorhersage
Korrektur
Initialisierung
(1) Berechnung Kalman Gain
(2) Aktualisierung Schätzung
(3) Aktualisierung Fehlerkovarianz
15
Berechnung Kalman Gain●
● K gibt an, wie stark die neue Messung gegenüber dem „a priori“ Zustand gewichtet wird
● Hohes K steht für eine sichere Messung des aktuellen Zustands
● R ist die Measurement Noise Covarianz Matrix
– Hängt davon ab, wie genau die Messung ist, z.B. Toleranzbereich der Sensoren
– Große Werte stehen für große Ungenauigkeit
Theoretischer Hintergrund
K k=P k− H T H P k
− H TR−1
16
Aktualisieren der Schätzung●
● Auch „a posteriori“ Zustand genannt
● K ist das Kalman Gain
● H beschreibt wie die aktuelle Messung in das System eingeht
● Vorher geschätzter Systemzustand
● Aktuelle Messung
Theoretischer Hintergrund
xk= xk−K k z k−H xk
−
xk−
17
z k
Fehler-Kovarianzmatrix●
● E ist die Einheitsmatrix entsprechender Dimension
● Gibt an, wie genau der momentane Zustand ist
● Je kleiner die Werte sind, desto weniger trägt die neue Messung zum „a posteriori“ Zustand bei
Theoretischer Hintergrund
P k=E−K k H P k−
18
ÜbersichtTheoretischer Hintergrund
P k−=AP k−1 A
TQ
(1) Schätzung Systemzustand
(2) Vermuteter Schätzfehler
xk−=A xk−1Buk
Zustandsvorhersage
Korrektur
Initialisierung
(1) Berechnung Kalman Gain
(2) Aktualisierung Schätzung
(3) Aktualisierung Fehlerkovarianz
K k=P k− H T H P k
− H TR−1
xk= xk−K k z k−H xk
−
P k=E−K k H P k−
19
Einsatz im Robocup● Ziel ist die Positionsbestimmung des Balles
● Roboter versucht durch die Kamera die Position relativ zu sich zu bestimmen
● Messungenauigkeiten:
✗ Sich ändernde Lichtverhältnisse✗ Schnell bewegende Bälle✗ Entfernte Bälle✗ Rauschende Kamerabilder
● Durch Kalman Filter diese Ungenauigkeiten beseitigen
20
Realisierung● Bewegungsmodell mit gleichförmiger Bewegung ohne
Verzögerung angenommen
● Ein Körper, auf den keine Kraft einwirkt, behält seine Geschwindigkeit bei, z.B. Reibung, Luftwiderstand
● Vereinfachung, da
➔ Körper sehr wohl Kräften ausgesetzt ist
➔ Es sich mit linearen Bewegungsmodell modellieren lässt
Einsatz im Robocup
21
Kovarianzmatrizen● Q hängt weniger von der Position des Balles als vielmehr
von der Geschwindigkeit ab
● Bestimmung von Q über Tests, also empirisch
● Bei R wurden verschiedene Messungen in Abhängigkeit von
– Entfernung zum Ball– Roboterbewegung, insbesondere Kopfbewegung
durchgeführt
● Aus diesen ergaben sich verschiede Kovarianzmatrizen die in einer Datei gespeichert wurden
Einsatz im Robocup
22
Kovarianzmatrizen / Zustand● Sehr schwer passende Kovarianzmatrizen zu finden
● Zustand xi
xi= Position xPosition y
Geschwindigkeit x−RichtungGeschwindigkeit y−Richtung
Einsatz im Robocup
23
Fazit● Tatsächliche Verbesserung der Balllokalisation
● Angewiesen auf die Qualität der Eingangsdaten
● Nächster Schritt:
✔ Optimierung der Kovarianzmatrizen
✔ Verbesserung des Bewegungsmodells, z.B. Reibung
Einsatz im Robocup
24