Kalman Filter - WordPress.com
Werbung
Mathematische Grundlagen Kalman Filter Kalman Filter Stephan Meyer FWPF Ortsbezogene Anwendungen und Dienste Georg-Simon-Ohm-Hochschule Nürnberg 07.12.2007 Beispielprogramm Mathematische Grundlagen Outline 1 Mathematische Grundlagen 2 Kalman Filter 3 Beispielprogramm Kalman Filter Beispielprogramm Mathematische Grundlagen Kalman Filter Mathematische Grundlagen 1 Mathematische Grundlagen Matrizen Zufallsvariablen Gaußverteilung Kovarianz und Kovarianz-Matrizen 2 Kalman Filter 3 Beispielprogramm Beispielprogramm Mathematische Grundlagen Kalman Filter Matrizen Matrizen Matrix Rechteckiges Zahlenschema 12 23 7 Einfache 2 × 3 Matrix 34 4 2 n × m bedeutet Zeilen × Spalten Oft mit Großbuchstaben angegeben, die die Zeilen-/Spalteninformation im Index tragen a11 a12 a13 a21 a22 a23 A43 = a31 a32 a33 a41 a42 a43 Beispielprogramm Mathematische Grundlagen Kalman Filter Beispielprogramm Matrizen Rechengesetze Addition: A22 + B22 = a11 a12 b11 b12 a11 + b11 a12 + b12 + = a21 a22 b21 b22 a21 + b21 a22 + b22 Multiplikation: a11 a12 b b = a21 a22 · 11 12 = b21 b22 a31 a32 A32 · B22 a11 · b11 + a12 · b21 a11 · b12 + a12 · b22 = a21 · b11 + a22 · b21 a21 · b12 + a22 · b22 a31 · b11 + a32 · b21 a31 · b12 + a32 · b22 Matrizenmultiplikation ist nicht kommutativ! A · B 6= B · A Mathematische Grundlagen Kalman Filter Beispielprogramm Matrizen Transponierte Matrix Transponierte Matrix Spiegelung“ der Matrix an der Diagonalen von ” rechts unten a11 a11 a12 a13 T A33 = a21 a22 a23 , A33 = a12 a13 a31 a32 a33 links oben nach a21 a31 a22 a32 a23 a33 Mathematische Grundlagen Kalman Filter 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 Beispielprogramm Mathematische Grundlagen Kalman Filter Beispielprogramm 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) Mathematische Grundlagen Kalman Filter Beispielprogramm Zufallsvariablen Erwartungswert und Varianz Gewünscht: Wie sind die Werte einer Zufallsvariablen ungefähr verteilt? Erwartungswert Angegeben durch E oder µ Entspricht z.B. dem Mittelwert der möglichen Variablen-Werte Varianz Angegeben durch σ 2 Maß für die mittlere quadratische Abweichung der Werte vom Erwartungswert Mathematische Grundlagen Kalman Filter Beispielprogramm Gaußverteilung Gauß- bzw. Normalverteilung Bezeichnung X ∼ N(µ, σ 2 ) Eine Zufallsvariable X ist gaußverteilt mit Erwartungswert µ und Varianz σ 2 Dichtefunktion (t−µ)2 1 f (t) = √ e − 2σ2 σ 2π Mathematische Grundlagen Gaußverteilung Gaußkurve Kalman Filter Beispielprogramm Mathematische Grundlagen Kalman Filter Kovarianz und Kovarianz-Matrizen Kovarianz Kovarianz Maßzahl für Zusammenhang zweier Zufallsvariablen X,Y Positiv, wenn lineare Abhängigkeit Negativ, wenn reziproke Abhängigkeit Null, wenn nicht voneinander abhängig Hoher Betrag → hohe Abhängigkeit Formeln Cov (X , Y ) := µ((X − µ(X ))(Y − µ(X ))) Cov (X , X ) := σ 2 (X ) Beispielprogramm Mathematische Grundlagen Kalman Filter Beispielprogramm 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 ) Mathematische Grundlagen Der Kalman Filter 1 Mathematische Grundlagen 2 Kalman Filter Grundlegendes Anwendung Modell Formeln 3 Beispielprogramm Kalman Filter Beispielprogramm Mathematische Grundlagen Kalman Filter Beispielprogramm 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 Mathematische Grundlagen Kalman Filter Beispielprogramm 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 Mathematische Grundlagen Kalman Filter Anwendung Einsatzgebiete Beispiele GPS-/INS-Navigation Satellitengestützte Messungen in der Atmosphäre (z.B. Ozon, N2 O,...) Navigation und Steuerung von Apollo-Raumkapseln Positionsbestimmung bei autonomen Fahrzeugen Generic Kalman Filter Software der NASA Filterung von Störsignalen bei Seismographen Beispielprogramm Mathematische Grundlagen Kalman Filter 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 Beispielprogramm Mathematische Grundlagen Kalman Filter Beispielprogramm 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 Mathematische Grundlagen Modell Modell Kalman Filter Beispielprogramm Mathematische Grundlagen Kalman Filter Formeln Der zu schätzende Prozess Zustandsvektor xk = A · xk−1 + B · uk + wk−1 Elemente xk xk−1 A uk B wk−1 aktueller Zustandsvektor (Schritt k) vorheriger Zustandsvektor (Schritt k − 1) Transformationsmatrix für Abbildung xk−1 → xk Vektor der manuellen Änderungen Transformationsmatrix uk → xk Vektor über Prozessrauschen Beispielprogramm Mathematische Grundlagen Kalman Filter Formeln Der zu schätzende Prozess Messung zk = H · xk + vk Elemente zk xk H vk aktueller Messungsvektor (Schritt k) aktueller Zustandsvektor (Schritt k) Transformationsmatrix für Abbildung xk → zk Vektor über Messfehler Beispielprogramm Mathematische Grundlagen Kalman Filter Formeln Fehlervektoren Prozessrauschen wk Transformationsfehler beim Übergang von xk−1 nach xk p(w ) ∼ N(0, Q) Q ist Kovarianzmatrix von wk Messfehler vk Abweichung der Messung zk vom Systemzustand p(v ) ∼ N(0, R) R ist Kovarianzmatrix von vk Vereinfachung: Q und R werden als konstant betrachtet Beispielprogramm Mathematische Grundlagen Kalman Filter Beispielprogramm Formeln Time Update A priori Zustandsvektor und Fehlerkovarianzmatrix x̂k,apri = A · x̂k−1,apos + B · uk Pk,apri = A · Pk−1,apos · AT + Q Elemente x̂k,apri a x̂k−1,apos a Pk,apri a Pk−1,apos a priori Schätzung des aktuellen Zustandsvektors posteriori Schätzung des vorherigen Zustandsvektors priori Schätzung der aktuellen Fehlerkovarianzmatrix posteriori Schätzung der aktuellen Fehlerkovarianzm. Mathematische Grundlagen Kalman Filter Beispielprogramm Formeln Measurement Update Kalman Gain und a posteriori Schätzungsvektoren Kk = Pk,apri · H T · (H · Pk,apri · H T + R)−1 x̂k,apos = x̂k,apri + Kk · (zk − H · x̂k,apri ) Pk,apos = (E − Kk · H) · Pk,apri Elemente Kk x̂k,apos Pk,apos Kalman Gain a posteriori Schätzung des aktuellen Zustandsvektors a posteriori Schätzung der aktuellen Fehlerkovarianzm. Mathematische Grundlagen Beispielprogramm 1 Mathematische Grundlagen 2 Kalman Filter 3 Beispielprogramm Kalman Filter Beispielprogramm Mathematische Grundlagen Beispielprogramm Kalman Filter Beispielprogramm Mathematische Grundlagen Kalman Filter Beispielprogramm Ende Vielen Dank für Ihre Aufmerksamkeit!
