Zustandsextrapolationsgleichung (State Extrapolation Equation)
Die erste Kalman-Filter-Gleichung (Kalman Filter equation), die ich beschreiben möchte, ist die Zustandsextrapolationsgleichung (state extrapolation equation).
Mit der Zustandsextrapolationsgleichung können wir den nächsten Systemzustand auf Basis des aktuellen Zustands vorhersagen. Sie extrapoliert den Zustandsvektor (state vector) aus der Gegenwart (Zeitschritt \( n \)) in die Zukunft (Zeitschritt \( n + 1 \)).
Die Zustandsextrapolationsgleichung beschreibt das Modell des dynamischen Systems. In der Literatur wird sie auch genannt:
- Prädiktor-Gleichung (Predictor Equation)
- Übergangsgleichung (Transition Equation)
- Vorhersagegleichung (Prediction Equation)
- Dynamisches Modell (Dynamic Model)
- Zustandsraummodell (State Space Model)
Die allgemeine Form der Zustandsextrapolationsgleichung in Matrixnotation lautet:
| \( \boldsymbol{\hat{x}}_{n+1,n} \) | ist ein vorhergesagter Systemzustandsvektor zum Zeitschritt \( n + 1 \) |
| \( \boldsymbol{\hat{x}}_{n,n} \) | ist ein geschätzter Systemzustandsvektor zum Zeitschritt \( n \) |
| \( \boldsymbol{u}_{n} \) | ist eine Steuervariable (control variable) oder Eingangsvariable (input variable) – ein messbarer (deterministischer) Eingang des Systems |
| \( \boldsymbol{w}_{n} \) | ist Prozessrauschen (process noise) oder eine Störung – ein nicht messbarer Eingang, der den Zustand beeinflusst |
| \( \boldsymbol{F} \) | ist eine Zustandsübergangsmatrix (state transition matrix) |
| \( \boldsymbol{G} \) | ist eine Steuermatrix (control matrix) oder Eingangsübergangsmatrix (input transition matrix) (Abbildung des Eingangs auf die Zustandsvariablen) |
Die folgende Abbildung zeigt eine schematische Darstellung der Zustandsextrapolationsgleichung.
Die Zustandsvariablen können Eigenschaften des Systems darstellen, die wir kennen möchten.
Beispielsweise hat ein sich bewegendes Fahrzeug drei Eigenschaften: Position, Geschwindigkeit und Beschleunigung.
Man kann sich fragen: Welche Eigenschaften sind Zustandsvariablen und welche sind Eingänge des Systems?
- Bewegte mechanische Systeme haben Eigenschaften wie Position, Geschwindigkeit, Beschleunigung und Luftwiderstand.
- Eine Kraft, die auf ein System wirkt, sollte als äußere Anregung betrachtet werden, d. h. als Eingang des Systems, der den Zustandsvektor steuert (im Fall konstanter Beschleunigung: Position und Geschwindigkeit).
- Newtons zweites Gesetz besagt \( F = ma \). Daher kann man die Beschleunigung als äußeren Eingang des Systems betrachten.
- Position und Geschwindigkeit sind die primären Zustandsvariablen von Interesse.
Zum Beispiel ist bei einem Feder-System die auf die Feder wirkende Kraft \( F(t) \) ein Eingang \( u(t) \), während die Feder-Auslenkung \( x(t) \) den Systemzustand darstellt.
Bei einem fallenden Körper sind die Eingänge die Gravitationskraft \( F_{g} \) und die Widerstandskraft \( F_{drag}(t) \), während Höhe \( h(t) \) und Geschwindigkeit \( v(t) \) die Systemzustände sind.
Schauen wir uns mehrere Beispiele der Zustandsextrapolationsgleichung an.
Beispiel – Flugzeug ohne Steuereingang
In diesem Beispiel definieren wir die Zustandsextrapolationsgleichung (State Extrapolation Equation) für ein Flugzeug, wobei wir ein Modell konstanter Beschleunigung annehmen.
In diesem Beispiel gibt es keinen Steuereingang (control input).
Wir nehmen an, dass kein Steuereingang vorliegt. Den Steuereingang sehen wir im nächsten Beispiel.
\[ \boldsymbol{u}_{n} = 0 \]
Betrachten wir ein Flugzeug, das sich mit konstanter Beschleunigung im dreidimensionalen Raum bewegt. Der Zustandsvektor \( \boldsymbol{\hat{x}_{n}} \), der die geschätzte Flugzeugposition, -geschwindigkeit und -beschleunigung in einem kartesischen Koordinatensystem \( \left( x,y,z \right) \) beschreibt, ist:
\[ \boldsymbol{\hat{x}}_{n}= \left[ \begin{matrix} \hat{x}_{n}\\ \hat{y}_{n}\\ \hat{z}_{n}\\ \hat{\dot{x}}_{n}\\ \hat{\dot{y}}_{n}\\ \hat{\dot{z}}_{n}\\ \hat{\ddot{x}}_{n}\\ \hat{\ddot{y}}_{n}\\ \hat{\ddot{z}}_{n}\\ \end{matrix} \right] \]
Die Zustandsübergangsmatrix \( \boldsymbol{F} \) lautet:
\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2}\\ 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \]
Die Zustandsextrapolationsgleichung lautet:
\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n} \]
\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \hat{\ddot{x}}_{n+1,n}\\ \hat{\ddot{y}}_{n+1,n}\\ \hat{\ddot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 & 0 \\ 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 & 0.5\Delta t^{2}\\ 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \hat{\ddot{x}}_{n,n}\\ \hat{\ddot{y}}_{n,n}\\ \hat{\ddot{z}}_{n,n}\\ \end{matrix} \right] \]
Die Matrixmultiplikation ergibt:
\[ \begin{cases} \hat{x}_{n+1,n} = \hat{x}_{n,n} + \hat{\dot{x}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{x}}_{n,n} \Delta t^{2}\\ \hat{y}_{n+1,n} = \hat{y}_{n,n} + \hat{\dot{y}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{y}}_{n,n} \Delta t^{2}\\ \hat{z}_{n+1,n} = \hat{z}_{n,n} + \hat{\dot{z}}_{n,n} \Delta t+ \frac{1}{2}\hat{\ddot{z}}_{n,n} \Delta t^{2}\\ \hat{\dot{x}}_{n+1,n} = \hat{\dot{x}}_{n,n} + \hat{\ddot{x}}_{n,n} \Delta t\\ \hat{\dot{y}}_{n+1,n} = \hat{\dot{y}}_{n,n} + \hat{\ddot{y}}_{n,n} \Delta t\\ \hat{\dot{z}}_{n+1,n} = \hat{\dot{z}}_{n,n} + \hat{\ddot{z}}_{n,n} \Delta t\\ \hat{\ddot{x}}_{n+1,n} = \hat{\ddot{x}}_{n,n}\\ \hat{\ddot{y}}_{n+1,n} = \hat{\ddot{y}}_{n,n}\\ \hat{\ddot{z}}_{n+1,n} = \hat{\ddot{z}}_{n,n}\\ \end{cases} \]
Beispiel – Flugzeug mit Steuereingang
Dieses Beispiel ist dem vorherigen ähnlich, aber jetzt haben wir einen Sensor, der mit den Steuerelementen des Piloten verbunden ist. Dadurch haben wir zusätzliche Informationen über die Flugzeugbeschleunigung basierend auf den Befehlen des Piloten.
Der Zustandsvektor \( \boldsymbol{\hat{x}_{n}} \), der die geschätzte Flugzeugposition und -geschwindigkeit in einem kartesischen Koordinatensystem \( \left( x,y,z \right) \) beschreibt, ist:
\[ \boldsymbol{\hat{x}}_{n}= \left[ \begin{matrix} \hat{x}_{n}\\ \hat{y}_{n}\\ \hat{z}_{n}\\ \hat{\dot{x}}_{n}\\ \hat{\dot{y}}_{n}\\ \hat{\dot{z}}_{n}\\ \end{matrix} \right] \]
Der Steuervektor \( \boldsymbol{u_{n}} \), der die gemessene Flugzeugbeschleunigung in einem kartesischen Koordinatensystem \( \left( x,y,z \right) \) beschreibt, ist:
\[ \boldsymbol{u}_{n}= \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right] \]
Die Zustandsübergangsmatrix \( \boldsymbol{F} \) lautet:
\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \]
Die Steuermatrix \( \boldsymbol{G} \) lautet:
\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \]
Die Zustandsextrapolationsgleichung lautet:
\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} + \boldsymbol{Gu}_{n,n} \]
\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{z}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\dot{z}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & 0 & 0 & \Delta t & 0 & 0\\ 0 & 1 & 0 & 0 & \Delta t & 0\\ 0 & 0 & 1 & 0 & 0 & \Delta t\\ 0 & 0 & 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{y}_{n,n}\\ \hat{z}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\dot{z}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} & 0 & 0 \\ 0 & 0.5\Delta t^{2} & 0 \\ 0 & 0 & 0.5\Delta t^{2} \\ \Delta t & 0 & 0 \\ 0 & \Delta t & 0 \\ 0 & 0 & \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} \ddot{x}_{n}\\ \ddot{y}_{n}\\ \ddot{z}_{n}\\ \end{matrix} \right] \]
Beispiel – frei fallender Körper
Betrachten wir einen frei fallenden Körper. Der Zustandsvektor enthält die Höhe \( h \) und die Geschwindigkeit des Körpers \( \dot{h} \):
\[ \boldsymbol{\hat{x}}_{n}= \left[ \begin{matrix} \hat{h}_{n}\\ \hat{\dot{h}}_{n}\\ \end{matrix} \right] \]
Die Zustandsübergangsmatrix \( \boldsymbol{F} \) lautet:
\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \]
Die Steuermatrix \( \boldsymbol{G} \) lautet:
\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5\Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \]
Die Eingangsvariable \( \boldsymbol{u}_{n} \) ist:
\[ \boldsymbol{u}_{n}= \left[ \begin{matrix} g \end{matrix} \right] \]
Dabei ist \( g \) die Erdbeschleunigung.
Wir haben keinen Sensor, der die Beschleunigung misst, aber wir wissen, dass bei einem frei fallenden Körper die Beschleunigung gleich \( g \) ist.
Die Zustandsextrapolationsgleichung sieht dann wie folgt aus:
\[ \left[ \begin{matrix} \hat{h}_{n+1,n}\\ \hat{\dot{h}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} \hat{h}_{n,n}\\ \hat{\dot{h}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5\Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \left[ \begin{matrix} g \end{matrix} \right] \]
Die Matrixmultiplikation ergibt:
\[ \begin{cases} \hat{h}_{n+1,n} = \hat{h}_{n,n} + \hat{\dot{h}}_{n,n} \Delta t + 0.5 \Delta t^{2} g\\ \hat{\dot{h}}_{n+1,n} = \hat{\dot{h}}_{n,n} + \Delta t g\\ \end{cases} \]
Dimensionen der Zustandsextrapolationsgleichung
Die folgende Tabelle gibt die Matrixdimensionen der Variablen der Zustandsextrapolationsgleichung an:
| Variable | Beschreibung | Dimension |
|---|---|---|
| \( \boldsymbol{x} \) | Zustandsvektor | \( n_{x} \times 1 \) |
| \( \boldsymbol{F} \) | Zustandsübergangsmatrix | \( n_{x} \times n_{x} \) |
| \( \boldsymbol{u} \) | Eingangsvariable | \( n_{u} \times 1 \) |
| \( \boldsymbol{G} \) | Steuermatrix | \( n_{x} \times n_{u} \) |
| \( \boldsymbol{w} \) | Prozessrauschvektor | \( n_{x} \times 1 \) |
Lineare zeitinvariante Systeme
Der lineare Kalman-Filter setzt das LTI-Systemmodell (Linear Time-Invariant) voraus.
Was bedeutet also „linear“ und was bedeutet „zeitinvariant“?
Lineare Systeme werden durch Gleichungssysteme beschrieben, in denen Variablen nie miteinander multipliziert werden, sondern nur mit Konstanten, und anschließend aufsummiert werden. Lineare Systeme werden verwendet, um sowohl statische als auch dynamische Beziehungen zwischen Variablen zu beschreiben.
Ein lineares System ist ein System, dessen Ausgangsfunktion \( y(t) \) die folgende Gleichung erfüllt:
\[ y(t) = \mathcal{F} \left( a \times g \left( t \right) +b \times h \left( t \right) \right) = a \times \mathcal{F} \left( g \left( t \right) \right) + b \times \mathcal{F} \left( h \left( t \right) \right) \]
Dabei gilt:
\( a \) und \( b \) sind konstante reelle Zahlen
\( g \) und \( h \) sind beliebige Funktionen der unabhängigen Variablen \( t \)
Ein lineares System folgt zwei grundlegenden Regeln:
- Man kann konstante multiplikative Skalierungsfaktoren „ausklammern“ (\( a \) und \( b \) oben).
- Die Systemantwort auf eine Summe von Eingängen ist die Summe der Antworten auf jeden Eingang einzeln.
Ein zeitinvariantes System besitzt eine Systemfunktion, die keine direkte Funktion der Zeit ist.
Betrachten wir als Beispiel einen Verstärker mit Verstärkung \( G = 10 \).
Dieses System ist zeitinvariant. Obwohl sich der Ausgang des Systems mit der Zeit ändert, ist die Systemfunktion nicht zeitabhängig.
Ein zeitinvariantes System ist ein System, bei dem eine Zeitverzögerung (oder Verschiebung) der Eingangsfolge eine äquivalente Zeitverzögerung in der Ausgangsfolge bewirkt.