Der diskrete Kalman Filter

Transkript

1 Der diskrete Kalman Filter Fachbereich: Informatik Betreuer: Marc Drassler Patrick Winkler Dezember 2004 Technische Universität Darmstadt Simulation und Systemoptimierung Darmstadt Dribbling Dackels

2 Inhalt Theoretischer Hintergrund Problemstellung Lösungsansatz Herleitung eindimensionaler Fall Erweiterung zum mehrdimensionalen Fall Einsatz im Robocup Ziel Anpassung des Kalman Filters Beispiele 1

3 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 2

4 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! 3

5 Funktionsweise Kalman Filter Zustandsvorhersage Korrektur Initialisierung Black Box wird noch gefüllt 4

6 Herleitung Zum Zeitpunkt t 1 erfolgt x-achse: Qualität der Messung y-achse: Wahrscheinlichkeit des Wertes Messung z 1 Messung kann nur Bereich angeben, in der sich der exakte Wert mit einer gewissen Wahrscheinlichkeit befindet Kann durch Gauß- Glockenkurve dargestellt werden (grün) 5

7 Herleitung Zweite Messung z 2 zum Zeitpunkt t 2 kommt hinzu (blau) Kombination der beiden Messwerte Gewichtete Linearkombination: x-achse: Qualität der Messung y-achse: Wahrscheinlichkeit des Wertes Geschätzter Wert zum Zeitpunkt t 2 : x t2 =k 1 z 1 k 2 z 2 6

8 Herleitung Der Schätzwert (rot) hat eine geringere Varianz als die Einzelmessungen Die Faktoren k 1 und k 2 werden durch Methoden der Statistik bestimmt x-achse: Qualität der Messung y-achse: Wahrscheinlichkeit des Wertes x t2 =k 1 z 1 k 2 z 2 7

9 Herleitung Sei x t1 =z 1 K t 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 x t2 = x t1 K t z 2 x t1 Zusätzlich Varianz für geschätzten Wert 8

10 Übersicht Zustandsvorhersage Korrektur (1) Berechnung Kalman Gain (2) Aktualisierung Schätzung (3) Aktualisierung Fehlerkovarianz (Varianz des geschätzten Wertes) Initialisierung 9

11 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 x t n x t n = x t n 1 u dt Dazu kommt die Varianz dieser Schätzung 10

12 Übersicht Zustandsvorhersage (1) Schätzung Zustand (2) Vermuteter Schätzfehler Korrektur (1) Berechnung Kalman Gain (2) Aktualisierung Schätzung (3) Aktualisierung Fehlerkovarianz (Varianz des geschätzten Wertes) Initialisierung Eindimensionaler Fall abgedeckt 11

13 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 12

14 Schätzung des Systemzustands x k =A x k 1 B u k 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 13

15 Vermuteter Schätzfehler P k =A P k 1 A T Q 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 14

16 Übersicht Korrektur Zustandsvorhersage (1) Schätzung Systemzustand x k =A x k 1 B u k (1) Berechnung Kalman Gain (2) Aktualisierung Schätzung (2) Vermuteter Schätzfehler P k =A P k 1 A T Q (3) Aktualisierung Fehlerkovarianz Initialisierung 15

17 Berechnung Kalman Gain K k =P k H T H P k H T R 1 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 16

18 Aktualisieren der Schätzung x k = x k K k z k H x k Auch a posteriori Zustand genannt K ist das Kalman Gain H beschreibt wie die aktuelle Messung in das System eingeht x k Vorher geschätzter Systemzustand z k Aktuelle Messung 17

19 Fehler-Kovarianzmatrix P k = E K k H P k 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 18

20 Übersicht Korrektur Zustandsvorhersage (1) Schätzung Systemzustand x k =A x k 1 B u k (2) Vermuteter Schätzfehler P k =A P k 1 A T Q (1) Berechnung Kalman Gain K k =P k H T H P k H T R 1 (2) Aktualisierung Schätzung x k = x k K k z k H x k (3) Aktualisierung Fehlerkovarianz P k = E K k H P k Initialisierung 19

21 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

22 Einsatz im Robocup 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 21

23 Einsatz im Robocup 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 22

24 Einsatz im Robocup Kovarianzmatrizen / Zustand Sehr schwer passende Kovarianzmatrizen zu finden Zustand x i Position x Position y Geschwindigkeit y Richtung = x i Geschwindigkeit x Richtung 23

25 Einsatz im Robocup 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 24

26 Beispiele 25

27 Ende Vortrag Danke für Ihre Aufmerksamkeit!