Beispiel: Positionsschätzung

Transkript

1 Das Kalman Filter

2 Beispiel: Positionsschätzung

3 Beispiel: Positionsschätzung. Messung: mit Varianz Daraus abgeleitete Positionsschätzung: mit Varianz ˆX = = f f ( y ) y 3

4 Beispiel: Positionsschätzung. Messung: mit Varianz Wie ann aus der bisherigen Schätzung mit der neuen Messung eine verbesserte Schätzung berechnet werden? f f ( x ) Xˆ f Xˆ ( x ) y, y 4

5 Kombination bisherige Schätzung / neue Messung Nach Varianzen gewichtetes Mittel hat die geringste erwartete quadratische Abweichung: Die neue Varianz der verbesserten Schätzung: X ˆ ˆ X = + = + + = X + ˆ + + = ˆ X + X ˆ + ( ) 5

6 Kombination bisherige Schätzung / neue Messung Zwei Iterationsgleichungen: Eine für den Schätzer Shät und eine für seine Varianz ( ) X ˆ = X ˆ + W X ˆ = + Gewichtetes Mittel zwischen atueller Schätzung und Abweichung der Messung von der Schätzung (Innovation) Gewichtungsfator: W = + 6

7 Aber das Boot bewegt sich X X & 7

8 Prozessmodell Bisherige Annahme: Der richtige Wert ist eine Konstante, d.h. von einer Messung zur nächsten tritt eine Veränderung auf. Was ist aber, wenn Veränderung durch Bewegung entsteht? Bewegungsmodell mit zwei Zustandsvariablen Position und Geschwindigeit (hier eindimensionales Bewegungsmodell): X X = X& X = X + + X & τ X& X& = + τ X = X = FX 0 + 8

9 Messgleichung Gemessen wird idaber nur die Position und zwar mit einem Messfehler X = ( 0) V X& + = HX + V Messmatrix Messrauschen 9

10 Zustandsraummodell zur Prozessbeschreibung D U X + G z X + + H V Systemstörung F Messung System Das Kalman Filter benutzt ein Zustandsraummodell zur Beschreibung des beobachteten Prozesses. Das Modell muss vollständig beannt sein. Das Modell darf sich in jedem Schritt ändern 0

11 Was muss beannt sein? Die Matrizen des Zustandsraummodells H, F, G Die Kovarianzmatrizen der Rauschprozesse mit l E U U = Q δ ( l) E V V = R δ ( l) l EV = EU = 0 Die Rauschprozesse müssen stochastisch unabhängig sein l EU V = 0 Die Kovarianzmatrix und der Erwartungswert des Startzustandes EXX X = Π E X 0 U = 0 EX = 0 0 X 0V EX V = 0

12 Vorhersage (Prediction) Der atuelle Schätzer des Zustandsvetors sei zusammen mit seiner Kovarianzmatrix beannt. Der Messwert im nächsten Zeitschritt soll mit der bisher besten Schätzung linear ombiniert werden. Dazu muss erst eine Vorhersage für den nächsten Wert und die zugehörige Kovarianzmatrix gemacht werden: Xˆ ˆ + = FX Cov( Xˆ ) F Cov( Xˆ ) F + = Anschließend ann diese Vorhersage mit dem neuen Messwert ombiniert werden und die Schätzung orrigiert werden.

13 Methode des Kalman Filters Vorhersage des nächsten Zustands und seiner Kovarianzmatrix mit physialischem Modell in Form einer Zustandsraumdarstellung Korretur Der Vorhersage mit Eintreffen des neuen Messwertes. Messwert und Innovation werden in Abhängigeit von den Kovarianzen gewichtet gemittelt. 3

14 Kalman Filtergleichungen () Salares Modell aus dem Beispiel Bi ilergab = + + ( ) X ˆ X ˆ X ˆ X ˆ X ˆ Korretur + = Prädition und wird nun vetoriell zu : ( ) X ˆ = X ˆ + W HX ˆ Korretur Xˆ FXˆ + = Prädition 4

15 Kalman Filtergleichungen () Die Bestimmung des Fators ist der aufwendigeeil der Herleitung und ergibt: ( ) W = P H H P H + R mit der Kovarianzmatrix der Innovation des Zustandes P = E ˆ ˆ ( X X )( X X ) = EX % X % Diese Kovarianzmatrix gibt ein Maß für die Güte des der Schätzung des Zustandsvetors 5

16 Vergleich mit dem eindimensionalen Beispiel W = = + + ( ) ( H ) W = H P H H P H + R Rüctransformation in den Raum des Zustandes Das Weglassen des blauen il eils vermeidet die Probleme bei singulären H Matrizen Die Unsicherheit der Innovation des Zustandes wird in eine Unsicherheit h itder Innovation der Messwerte übersetzt. Das entspricht der Unsicherheit Die Unsicherheit der atuellen Messung 6

17 Kalman Filtergleichungen Initialisierung Xˆ 0 = 0 P0 =Π 0 Reursion des nächsten Zustandsvetors ( ) X ˆ = F X ˆ + F W HX ˆ mit + ( ) W = P H H P H + R Reursion der Kovarianzmatrix der Innovation des Zustandsvetors ( ) P = F P F + G Q G F P H H P H + R H P F + 7