Mathematische Grundlagen Kalman Filter Beispielprogramm. Kalman Filter. Stephan Meyer

Transkript

1 Kalman Filter Stephan Meyer FWPF Ortsbezogene Anwendungen und Dienste Georg-Simon-Ohm-Hochschule Nürnberg

2 Outline 1 Mathematische Grundlagen 2 Kalman Filter 3 Beispielprogramm

3 Mathematische Grundlagen 1 Mathematische Grundlagen Matrizen Zufallsvariablen Gaußverteilung Kovarianz und Kovarianz-Matrizen 2 Kalman Filter 3 Beispielprogramm

4 Matrizen Matrizen Matrix Rechteckiges Zahlenschema ( ) Einfache 2 3 Matrix n m bedeutet Zeilen Spalten Oft mit Großbuchstaben angegeben, die die Zeilen-/Spalteninformation im Index tragen a 11 a 12 a 13 A 43 = a 21 a 22 a 23 a 31 a 32 a 33 a 41 a 42 a 43

5 Matrizen Rechengesetze Addition: ( ) ( ) ( ) a11 a A 22 + B 22 = 12 b11 b + 12 a11 + b = 11 a 12 + b 12 a 21 a 22 b 21 b 22 a 21 + b 21 a 22 + b 22 Multiplikation: a 11 a 12 ( ) A 32 B 22 = a 21 a 22 b11 b 12 = b a 31 a 21 b a 11 b 11 + a 12 b 21 a 11 b 12 + a 12 b 22 = a 21 b 11 + a 22 b 21 a 21 b 12 + a 22 b 22 a 31 b 11 + a 32 b 21 a 31 b 12 + a 32 b 22 Matrizenmultiplikation ist nicht kommutativ! A B B A

6 Matrizen Transponierte Matrix Transponierte Matrix Spiegelung der Matrix an der Diagonalen von links oben nach rechts unten a 11 a 12 a 13 a 11 a 21 a 31 A 33 = a 21 a 22 a 23, A T 33 = a 12 a 22 a 32 a 31 a 32 a 33 a 13 a 23 a 33

7 Matrizen Inverse Matrix Inverse Matrix Eine Matrix multipliziert mit ihrer Inversen ergibt die Einheitsmatrix A A 1 = E Berechnung mit: Gauß-Jordan-Algorithmus LR-Zerlegung Cholesky-Zerlegung

8 Zufallsvariablen Zufallsvariablen Definition (Papula): Eine Zufallsvariable oder Zufallsgröße X ist eine Funktion, die jedem Elementarereignis ω aus der Ergebnismenge Ω genau eine reelle Zahl X (ω) zuordnet. Unterscheide zwischen: Diskreten ZV (Augensumme bei mehrmaligem Würfeln) Stetigen ZV (Prozess in der Natur)

9 Zufallsvariablen Erwartungswert und Varianz Gewünscht: Wie sind die Werte einer Zufallsvariablen ungefähr verteilt? Erwartungswert Varianz Angegeben durch E oder µ Entspricht z.b. dem Mittelwert der möglichen Variablen-Werte Angegeben durch σ 2 Maß für die mittlere quadratische Abweichung der Werte vom Erwartungswert

10 Gaußverteilung Gauß- bzw. Normalverteilung Bezeichnung X N(µ, σ 2 ) Eine Zufallsvariable X ist gaußverteilt mit Erwartungswert µ und Varianz σ 2 Dichtefunktion f (t) = 1 σ (t µ) 2 2π e 2σ 2

11 Gaußverteilung Gaußkurve

12 Kovarianz und Kovarianz-Matrizen Kovarianz Kovarianz Maßzahl für Zusammenhang zweier Zufallsvariablen X,Y Formeln Positiv, wenn lineare Abhängigkeit Negativ, wenn reziproke Abhängigkeit Null, wenn nicht voneinander abhängig Hoher Betrag hohe Abhängigkeit Cov(X, Y ) := µ((x µ(x ))(Y µ(x ))) Cov(X, X ) := σ 2 (X )

13 Kovarianz und Kovarianz-Matrizen Kovarianzmatrix Definition Eine Kovarianzmatrix beschreibt die paarweisen Abhängigkeiten der Komponenten eines Zufallsvektors. Beispiel: Zufallsvektor und Kovarianzmatrix X x = Y Z Cov(X, X ) Cov(X, Y ) Cov(X, Z) Cov(x) = Cov(Y, X ) Cov(Y, Y ) Cov(Y, Z) Cov(Z, X ) Cov(Z, Y ) Cov(Z, Z)

14 Der Kalman Filter 1 Mathematische Grundlagen 2 Kalman Filter Grundlegendes Anwendung Modell Formeln 3 Beispielprogramm

15 Grundlegendes Grundlegendes Was ist ein Kalman Filter? Mathematischer Formelsatz zur Werteschätzung in linearen dynamischen Systemen anhand fehlerbehafteter Messungen Vom ungarisch-amerikanischen Mathematiker Rudolf Emil Kalman 1960 entworfen

16 Grundlegendes Besonderheiten Besonderheiten Prediktor-Korrektor-Struktur Keine Wertespeicherung nötig Rekursion Funktionsweise als Black-Box Geringer Rechenaufwand Echtzeitfähigkeit Berücksichtigung von manuellen Änderungen am System Regelungstechnik

17 Anwendung Einsatzgebiete Beispiele GPS-/INS-Navigation Satellitengestützte Messungen in der Atmosphäre (z.b. Ozon, N 2 O,...) Navigation und Steuerung von Apollo-Raumkapseln Positionsbestimmung bei autonomen Fahrzeugen Generic Kalman Filter Software der NASA Filterung von Störsignalen bei Seismographen

18 Anwendung Varianten des Kalman Filters Varianten Extended Kalman Filter (nichtlineare Systeme) Kalman Schmidt Filter (Apollo) Kalman Bucy Filter (Regelungstechnik) Fuzzy Kalman Filter Local Ensemble Transform Kalman Filter

19 Modell Modell Zustandsvektor Beschreibt den aktuellen Systemzustand Z.B. Position, Geschwindigkeit, Beschleunigung,... Prediktor-Korrektor-Schema A priori Schätzung des Systemzustandes auf Basis des vorherigen Systemzustandes und manueller Änderungen Messung des Systemzustandes (fehlerbehaftet) A posteriori Schätzung des Systemzustandes auf Basis der Messung und der a priori-schätzung Prediktor-Phase wird als Time Update bezeichnet Korrektor-Phase wird Measurement Update genannt

20 Modell Modell

21 Formeln Der zu schätzende Prozess Zustandsvektor x k = A x k 1 + B u k + w k 1 Elemente x k aktueller Zustandsvektor (Schritt k) x k 1 vorheriger Zustandsvektor (Schritt k 1) A Transformationsmatrix für Abbildung x k 1 x k u k Vektor der manuellen Änderungen B Transformationsmatrix u k x k Vektor über Prozessrauschen w k 1

22 Formeln Der zu schätzende Prozess Messung z k = H x k + v k Elemente z k aktueller Messungsvektor (Schritt k) x k aktueller Zustandsvektor (Schritt k) H Transformationsmatrix für Abbildung x k z k Vektor über Messfehler v k

23 Formeln Fehlervektoren Prozessrauschen w k Transformationsfehler beim Übergang von x k 1 nach x k p(w) N(0, Q) Q ist Kovarianzmatrix von w k Messfehler v k Abweichung der Messung z k vom Systemzustand p(v) N(0, R) R ist Kovarianzmatrix von v k Vereinfachung: Q und R werden als konstant betrachtet

24 Formeln Time Update A priori Zustandsvektor und Fehlerkovarianzmatrix Elemente ˆx k,apri = A ˆx k 1,apos + B u k P k,apri = A P k 1,apos A T + Q ˆx k,apri a priori Schätzung des aktuellen Zustandsvektors ˆx k 1,apos a posteriori Schätzung des vorherigen Zustandsvektors P k,apri a priori Schätzung der aktuellen Fehlerkovarianzmatrix P k 1,apos a posteriori Schätzung der aktuellen Fehlerkovarianzm.

25 Formeln Measurement Update Kalman Gain und a posteriori Schätzungsvektoren K k = P k,apri H T (H P k,apri H T + R) 1 ˆx k,apos = ˆx k,apri + K k (z k H ˆx k,apri ) P k,apos = (E K k H) P k,apri Elemente K k ˆx k,apos P k,apos Kalman Gain a posteriori Schätzung des aktuellen Zustandsvektors a posteriori Schätzung der aktuellen Fehlerkovarianzm.

26 Beispielprogramm 1 Mathematische Grundlagen 2 Kalman Filter 3 Beispielprogramm

27 Beispielprogramm

28 Ende Vielen Dank für Ihre Aufmerksamkeit!