Der diskrete Kalman Filter - sim.informatik.tu-darmstadt.de · Problemstellung Das Ziel ist die Simulation von physikalischen Prozessen Basis bei der Modellierung eines physikalischen

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!


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)


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:


x t2=k 1∗z1k 2∗z2

6


Herleitung● Der Schätzwert (rot) hat

eine geringere Varianz als die Einzelmessungen

● Die Faktoren k1 und k

2

werden durch Methoden der Statistik bestimmt


x t2=k 1∗z1k 2∗z2

7


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


xt1=z1

x t2= x t1K t∗ z2− x t1

K t

8

Übersicht


Initialisierung

(1) Berechnung Kalman Gain

(2) Aktualisierung Schätzung

(3) Aktualisierung Fehlerkovari-anz (Varianz des geschätzten Wertes)


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


x t n−= x t n−1u∗dt

x t n−

10

Übersicht


Initialisierung



(3) Aktualisierung Fehlerkovari-anz (Varianz des geschätzten Wertes)


(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


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


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


P k−=AP k−1 A

TQ

14

ÜbersichtTheoretischer Hintergrund

P k−=AP k−1 A

TQ

(1) Schätzung Systemzustand


xk−=A xk−1Buk

Zustandsvorhersage

Korrektur

Initialisierung



(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


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


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


P k=E−K k H P k−

18

ÜbersichtTheoretischer Hintergrund

P k−=AP k−1 A

TQ

(1) Schätzung Systemzustand


xk−=A xk−1Buk

Zustandsvorhersage

Korrektur

Initialisierung



(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

Beispiele

25

Ende Vortrag

Danke für Ihre Aufmerksamkeit!

Der diskrete Kalman Filter - sim.informatik.tu-darmstadt.de · Problemstellung Das Ziel ist die Simulation von physikalischen Prozessen Basis bei der Modellierung eines physikalischen

Documents