Das Kalman‐Filter

Beispiel: Positionsschätzung

2

Beispiel: Positionsschätzung

1. Messung:            mit Varianz  1Y1

2Yσ

Daraus abgeleitete Positionsschätzung:                        mit Varianz  1 1X̂ Y=1

2 21 Yσ σ=

f

1 1( )Yf y

1y

3

Beispiel: Positionsschätzung

2. Messung:            mit Varianz  2Y2

2Yσ

Wie kann aus der bisherigen Schätzung mit der neuen Messung eine verbesserte Schätzung berechnet werden?

f

( )f2

ˆ 2( )Xf x

1ˆ 1( )Xf x

1 2,y y

4

Kombination bisherige Schätzung / neue Messung 

Nach Varianzen gewichtetes Mittel  Die neue Varianz der ghat die geringste erwartete quadratische Abweichung:

verbesserten Schätzung:quadratische Abweichung:

1 1 1⎛ ⎞ 1 1 1

2

2 1 22 21

2 2

1 1 1ˆ ˆ1 1

Y

X X Yσ σ

σ σ

⎛ ⎞= +⎜ ⎟⎜ ⎟

⎝ ⎠+ 2

2 2 22 1

1 1 1

Yσ σ σ= +

2

2

1

2 21

1 2ˆ

Y

Y X Y

σ σ

σ σ= +

( )2 2

1 22 2 2 21 1

21ˆ ˆ

Y Y

X Y

X Y X

σ σ σ σ

σ

++ +

( )2

11 2 12 2

1 Y

X Y Xσ σ

= + −+

5

Kombination bisherige Schätzung / neue Messung 

Zwei Iterationsgleichungen: Ei fü d S hät d i fü i V iEine für den Schätzer und eine für seine Varianz

( ) 1 1 1( )2 1 2 2 1ˆ ˆ ˆX X W Y X= + −

2

2 2 22 1

1 1 1

Yσ σ σ= +

Gewichtetes Mittel zwischen aktueller Schätzung und Abweichung der Messung von der Schätzung (Innovation)Abweichung der Messung von der Schätzung (Innovation)

G i ht f kt21σGewichtungsfaktor:   

2

12 2 2

1 Y

W σσ σ

=+

6

Aber das Boot bewegt sich

X X&X

7

Prozessmodell

Bisherige Annahme: Der richtige Wert ist eine KonstanteBisherige Annahme: Der richtige Wert ist eine Konstante, d.h. von einer Messung zur nächsten tritt keine Veränderung auf Was ist aber wenn Veränderung durch Bewegungauf. Was ist aber, wenn Veränderung durch Bewegung entsteht?

Bewegungsmodell mit zwei Zustandsvariablen Position und Geschwindigkeit (hier eindimensionales Bewegungsmodell):

1k k kX X X τ+ = + &

& &k

k

XX

X⎛ ⎞

= ⎜ ⎟⎝ ⎠&

1k kX X+ =kkX⎜ ⎟

⎝ ⎠

1

10 1k k kX X FX

τ+

⎛ ⎞= =⎜ ⎟⎝ ⎠

8

⎝ ⎠

Messgleichung

G i d b di P itiGemessen wird aber nur die Position 

… und zwar mit einem Messfehler… und zwar mit einem Messfehler

(1 0) kXY V

⎛ ⎞+⎜ ⎟(1 0)k k

k

Y VX

= +⎜ ⎟⎝ ⎠&

Y H X Vk k kY H X V= +

Messmatrix Messrauschen

9

Zustandsraummodell zur Prozessbeschreibung

kD

kG 1z− k

H+ +kU kYkX1kX +

kVk

Fk

MessungSystemstörung

Messung

System

Das Kalman‐Filter benutzt ein Zustandsraummodell zur Beschreibung des beobachteten Prozesses.

10

Das Modell muss vollständig bekannt sein.Das Modell darf sich in jedem Schritt k ändern

Was muss bekannt sein?Die Matrizen des Zustandsraummodells

, ,k k k

H F G

Die Kovarianzmatrizen der Rauschprozesse mit 

E ( )Tk l

kQU U k lδ= − E ( )T

k l kRV V k lδ= −

E E 0k kV U= =

Die Rauschprozesse müssen stochastisch unabhängig sein

k k

E 0Tk lU V =

Die Kovarianzmatrix und der Erwartungswert des Startzustandes 

E TX X Π E 0TX U0 0 0EX X = Π 0E 0kX U =

0E 0TkX V =0E 0X =

11

0E 0kX V0

Vorhersage (Prediction)

Der aktuelle Schätzer des Zustandsvektors sei zusammenDer aktuelle Schätzer des Zustandsvektors sei zusammen mit seiner Kovarianzmatrix bekannt. 

h h ll d b hDer Messwert im nächsten Zeitschritt soll mit der bisher besten Schätzung linear kombiniert werden. 

Dazu muss erst eine Vorhersage für den nächsten Wert und die zugehörige Kovarianzmatrix gemacht werden:

1| |ˆ ˆ

k k k kX FX+ =

1| |ˆ ˆ( ) ( )T

k k k kCov X F Cov X F+ =

Anschließend kann diese Vorhersage mit dem neuen Messwert kombiniert werden und die Schätzung korrigiert

12

Messwert kombiniert werden und die Schätzung korrigiert werden.

Methode des Kalman Filters

VorhersageVorhersage des nächsten Zustands und seiner Kovarianzmatrix mit physikalischem Modell in Form einer Zustandsraumdarstellung

KorrekturKorrektur Der Vorhersage mit Eintreffen des neuen Messwertes. Messwert und Innovation werden in Abhängigkeit von den Kovarianzen gewichtet gemittelt

13

Kovarianzen gewichtet gemittelt. 

Kalman Filtergleichungen (1)

Sk l M d ll d B i i l bSkalares Modell aus dem Beispiel ergab

( )2|ˆ ˆ ˆk kσ ( )| 1

| | 1 | 12 2| 1

ˆ ˆ ˆ1

ˆ ˆk

k kk k k k k k k

k k Y

X X Y Xσ

σ σ−

− −−

= + − ⋅+

Korrektur

1| |ˆ ˆ

k k k kX X+ = Prädiktion

und wird nun vektoriell zu :

( )ˆ ˆ ˆ( )| | 1 | 1ˆ ˆ ˆ

k k k k k kkX X W Y H X− −= + −

ˆ ˆX FX

Korrektur

1| |k k k kX FX+ = Prädiktion

14

Kalman Filtergleichungen (2)

Die Bestimmung des Faktors ist der aufwendige Teil derDie Bestimmung des Faktors ist der aufwendige Teil der Herleitung und ergibt:

( ) 1

| 1 | 1T T

k k k k k k k kW P H H P H R

−

− −= +

mit der Kovarianzmatrix der Innovation des Zustandes

ˆ ˆ TT % %| 1 | 1 | 1 | 1| 1

ˆ ˆ( )( ) TTk k k k k k k kk k

P E X X X X E X X− − − −−= − − =

Diese Kovarianzmatrix gibt ein Maß für die Güte des der Schätzung des ZustandsvektorsSchätzung des Zustandsvektors

15

Vergleich mit dem eindimensionalen Beispiel

( )2

1σ ( ) 1| 1 2 2 2| 1 | 12 2

| 1k

k

k kk k k k Y

k k Y

Wσ

σ σ σσ σ

−−− −

−

= = ++

( ) 11 T TW P H H P H RH H−

− ( )| 1 | 11 T T

k k k k kk k k kkW P H H P H RH H

− −= +

Die Unsicherheit der 

Die Unsicherheit der Innovation des Zustandes wird in eine U i h h it d I ti d

Rücktransformation in den Raum des Zustandes

aktuellen Messung

Unsicherheit der Innovation der Messwerte übersetzt. Das entspricht der Unsicherheit  2

| 1k kσ −

Zustandes

Das Weglassen des bl T il id t |k kblauen Teils vermeidet die Probleme bei singulären H‐Matrizen

16

Kalman Filtergleichungen

Initialisierung

0| 1ˆ 0X − =

0| 1 0P − = Π

( ) 1−

( )Rekursion des nächsten Zustandsvektors  

( ) 1

| 1 | 1T T

k k k k k k k kW P H H P H R

− −= +( )1| | 1 | 1

ˆ ˆ ˆk k k k k kkk k

X F X F W Y H X+ − −= + − mit

( ) 1T T T T T T−

Rekursion der Kovarianzmatrix der Innovation des Zustandsvektors

( )1| | 1 | 1 | 1 | 1T T T T T T

k k k k k k k k k k k k k k k k k k kk kkP F P F G Q G F P H H P H R H P F

+ − − − −= + − +

17