Beispiele
Dies ist der letzte Teil des Kapitels „Mehrdimensionaler Kalman-Filter“ (Multivariate Kalman Filter).
Er enthält zwei numerische Beispiele. Im ersten Beispiel entwerfen wir einen sechsdimensionalen Kalman-Filter ohne Steuereingang (control input). Im zweiten Beispiel entwerfen wir einen zweidimensionalen Kalman-Filter mit Steuereingang.
Beispiel 9 – Schätzung der Fahrzeugposition
Im folgenden Beispiel implementieren wir den mehrdimensionalen Kalman-Filter (Multivariate Kalman Filter) anhand des bisher Gelernten.
In diesem Beispiel schätzen wir die Fahrzeugposition in der \( XY \)-Ebene.
Das Fahrzeug hat einen bordeigenen Positionssensor, der die \( X \)- und \( Y \)-Koordinaten des Systems meldet.
Wir nehmen eine Dynamik mit konstanter Beschleunigung an.
Die Zustandsextrapolationsgleichung
Zunächst leiten wir die Zustandsextrapolationsgleichung (state extrapolation equation) her. Wie Sie sich erinnern, lautet die allgemeine Form der Zustandsextrapolationsgleichung in Matrixnotation:
| \( \boldsymbol{\hat{x}}_{n+1,n} \) | ist der vorhergesagte Systemzustandsvektor zum Zeitschritt \( n + 1 \) |
| \( \boldsymbol{\hat{x}}_{n,n} \) | ist der geschätzte Systemzustandsvektor zum Zeitschritt \( n \) |
| \( \boldsymbol{u}_{n} \) | ist eine Steuervariable |
| \( \boldsymbol{w}_{n}\) | ist das Prozessrauschen |
| \( \boldsymbol{F} \) | ist die Zustandsübergangsmatrix |
| \( \boldsymbol{G} \) | ist die Steuermatrix |
In diesem Beispiel gibt es keine Steuervariable \( \boldsymbol{u} \), da es keinen Steuereingang gibt.
Für dieses Beispiel lässt sich die Zustandsextrapolationsgleichung vereinfachen zu:
\[ \boldsymbol{\hat{x}}_{n+1,n} = \boldsymbol{F\hat{x}}_{n,n} \]
Der Systemzustand \( \boldsymbol{x}_{n} \) ist definiert durch:
\[ \boldsymbol{x}_{n}= \left[ \begin{matrix} x_{n}\\ \dot{x}_{n}\\ \ddot{x}_{n}\\ y_{n}\\ \dot{y}_{n}\\ \ddot{y}_{n}\\ \end{matrix} \right] \]
Der extrapolierte Fahrzeugzustand zum Zeitpunkt \( n + 1 \) kann durch folgendes Gleichungssystem beschrieben werden:
\[ \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{\dot{x}}_{n+1,n} = \hat{\dot{x}}_{n,n} + \hat{\ddot{x}}_{n,n}\Delta t\\ \hat{\ddot{x}}_{n+1,n} = \hat{\ddot{x}}_{n,n}\\ \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{\dot{y}}_{n+1,n} = \hat{\dot{y}}_{n,n} + \hat{\ddot{y}}_{n,n}\Delta t\\ \hat{\ddot{y}}_{n+1,n} = \hat{\ddot{y}}_{n,n} \end{cases} \]
In Matrixform:
\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \hat{\ddot{x}}_{n+1,n}\\ \hat{y}_{n+1,n}\\ \hat{\dot{y}}_{n+1,n}\\ \hat{\ddot{y}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & \Delta t & 0.5\Delta t^{2} & 0 & 0 & 0\\ 0 & 1 & \Delta t & 0 & 0 & 0\\ 0 & 0 & 1 & 0 & 0 & 0\\ 0 & 0 & 0 & 1 & \Delta t & 0.5\Delta t^{2}\\ 0 & 0 & 0 & 0 & 1 & \Delta t\\ 0 & 0 & 0 & 0 & 0 & 1\\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \hat{\ddot{x}}_{n,n}\\ \hat{y}_{n,n}\\ \hat{\dot{y}}_{n,n}\\ \hat{\ddot{y}}_{n,n}\\ \end{matrix} \right] \]
Die Kovarianzextrapolationsgleichung
Die allgemeine Form der Kovarianzextrapolationsgleichung lautet:
| \( \boldsymbol{P}_{n,n} \) | ist die Kovarianzmatrix der aktuellen Zustandsschätzung |
| \( \boldsymbol{P}_{n+1,n} \) | ist die Kovarianzmatrix der nächsten Zustandsschätzung (Vorhersage) |
| \( \boldsymbol{F} \) | ist die Zustandsübergangsmatrix, die wir im Abschnitt „Modellierung linearer dynamischer Systeme“ hergeleitet haben |
| \( \boldsymbol{Q} \) | ist die Prozessrauschmatrix |
Die Schätzkovarianzmatrix lautet:
\[ \boldsymbol{P} = \left[ \begin{matrix} p_{x} & p_{x\dot{x}} & p_{x\ddot{x}} & p_{xy} & p_{x\dot{y}} & p_{x\ddot{y}} \\ p_{\dot{x}x} & p_{\dot{x}} & p_{\dot{x}\ddot{x}} & p_{\dot{x}y} & p_{\dot{x}\dot{y}} & p_{\dot{x}\ddot{y}} \\ p_{\ddot{x}x} & p_{\ddot{x}\dot{x}} & p_{\ddot{x}} & p_{\ddot{x}y} & p_{\ddot{x}\dot{y}} & p_{\ddot{x}\ddot{y}} \\ p_{yx} & p_{y\dot{x}} & p_{y\ddot{x}} & p_{y} & p_{y\dot{y}} & p_{y\ddot{y}} \\ p_{\dot{y}x} & p_{\dot{y}\dot{x}} & p_{\dot{y}\ddot{x}} & p_{\dot{y}y} & p_{\dot{y}} & p_{\dot{y}\ddot{y}} \\ p_{\ddot{y}x} & p_{\ddot{y}\dot{x}} & p_{\ddot{y}\ddot{x}} & p_{\ddot{y}y} & p_{\ddot{y}\dot{y}} & p_{\ddot{y}} \\ \end{matrix} \right] \]
Die Elemente auf der Hauptdiagonalen der Matrix sind die Varianzen der Schätzung:
- \( p_{x} \) ist die Varianz der Positionsschätzung der \( X \)-Koordinate
- \( p_{\dot{x}} \) ist die Varianz der Geschwindigkeitsschätzung der \( X \)-Koordinate
- \( p_{\ddot{x}} \) ist die Varianz der Beschleunigungsschätzung der \( X \)-Koordinate
- \( p_{y} \) ist die Varianz der Positionsschätzung der \( Y \)-Koordinate
- \( p_{\dot{y}} \) ist die Varianz der Geschwindigkeitsschätzung der \( Y \)-Koordinate
- \( p_{\ddot{y}} \) ist die Varianz der Beschleunigungsschätzung der \( Y \)-Koordinate
- Die Einträge außerhalb der Diagonalen sind Kovarianzen
Wir nehmen an, dass die Schätzfehler in den \( X \)- und \( Y \)-Achsen nicht korreliert sind, sodass die Kreuzterme auf null gesetzt werden können.
Analyse des Beispiels
Die folgende Grafik zeigt die Schätzleistung des Kalman-Filters für Position und Geschwindigkeit.
Die linke Grafik vergleicht die wahren, gemessenen und geschätzten Werte der Fahrzeugposition. Die beiden rechten Grafiken vergleichen die wahren, gemessenen und geschätzten Werte der Geschwindigkeit in \( x \)-Richtung und \( y \)-Richtung.
Wie Sie sehen, gelingt es dem Kalman-Filter, das Fahrzeug zu verfolgen.
Zoomen wir in den linearen Teil der Fahrzeugbewegung und in den Teil mit dem Kurvenmanöver hinein.
Die Kreise im Diagramm stellen die 95%-Konfidenzellipse (confidence ellipse) dar. Da die Messfehler in \( x \)- und \( y \)-Richtung gleich sind, ist die Konfidenzellipse ein Kreis.
Während sich das Fahrzeug entlang einer Geraden bewegt, ist die Beschleunigung konstant und gleich null. Während des Kurvenmanövers erfährt das Fahrzeug jedoch aufgrund der Kreisbewegung eine Beschleunigung – die Winkelbeschleunigung (angular acceleration).
Aus der Physik-Einführung erinnern Sie sich vielleicht daran, dass die Winkelbeschleunigung \( \alpha = \frac{\Delta V}{R \Delta t} \) ist, wobei \( \Delta t \) das Zeitintervall, \( \Delta V \) die Geschwindigkeitsänderung innerhalb des Zeitintervalls und \( R \) der Kreisradius ist.
Obwohl die Winkelbeschleunigung konstant ist, ist ihre Projektion auf die \( x \)- und \( y \)-Achsen nicht konstant; daher sind \( \ddot{x} \) und \( \ddot{y} \) nicht konstant.
Unser Kalman-Filter ist für ein Modell mit konstanter Beschleunigung ausgelegt. Dennoch gelingt es ihm, das manövrierende Fahrzeug zu verfolgen – dank eines passend gewählten Parameters \( \sigma_{a}^{2} \).
Beispiel 10 – Schätzung der Raketenhöhe
In diesem Beispiel schätzen wir die Höhe einer Rakete. Die Rakete ist mit einem bordeigenen Höhenmesser (altimeter) ausgestattet, der Höhenmessungen liefert. Zusätzlich ist die Rakete mit einem Beschleunigungssensor (accelerometer) ausgestattet, der die Beschleunigung der Rakete misst.
Der Beschleunigungssensor dient dem Kalman-Filter als Steuereingang (control input).
Wir nehmen eine Dynamik mit konstanter Beschleunigung an.
Beschleunigungsmessungen enthalten die Erdbeschleunigung. Ein Beschleunigungssensor, der in Ruhe auf einem Tisch liegt, misst \( 1g \) nach oben, während ein Beschleunigungssensor im freien Fall eine Beschleunigung von null misst. Daher müssen wir die Erdbeschleunigungskonstante \( g \) von jeder Beschleunigungsmessung abziehen.
Die Beschleunigungsmessung im Zeitschritt \( n \) ist:
\[ a_{n} = \ddot{x} - g + \epsilon \]
Wobei:
- \( \ddot{x} \) die tatsächliche Beschleunigung des Objekts ist (die zweite Ableitung der Objektposition)
- \( g \) die Erdbeschleunigungskonstante ist; \( g = -9.8\frac{m}{s^{2}} \)
- \( \epsilon \) der Messfehler des Beschleunigungssensors ist
Die Zustandsextrapolationsgleichung
Die allgemeine Form der Zustandsextrapolationsgleichung in Matrixnotation lautet:
| \( \boldsymbol{\hat{x}}_{n+1,n} \) | ist der vorhergesagte Systemzustandsvektor zum Zeitschritt \( n + 1 \) |
| \( \boldsymbol{\hat{x}}_{n,n} \) | ist der geschätzte Systemzustandsvektor zum Zeitschritt \( n \) |
| \( \boldsymbol{u}_{n} \) | ist eine Steuervariable |
| \( \boldsymbol{w}_{n} \) | ist das Prozessrauschen |
| \( \boldsymbol{F} \) | ist die Zustandsübergangsmatrix |
| \( \boldsymbol{G} \) | ist die Steuermatrix |
In diesem Beispiel haben wir eine Steuervariable \( \boldsymbol{u} \), die auf der Beschleunigungsmessung basiert.
Der Systemzustand \( \boldsymbol{x}_{n} \) ist definiert durch:
\[ \boldsymbol{x}_{n} = \left[ \begin{matrix} x_{n} \\ \dot{x}_{n} \\ \end{matrix} \right] \]
Wobei:
- \( x_{n} \) die Raketenhöhe zum Zeitpunkt \( n \) ist
- \( \dot{x}_{n} \) die Raketengeschwindigkeit zum Zeitpunkt \( n \) ist
Wir können die Zustandsextrapolationsgleichung wie folgt ausdrücken:
\[ \left[ \begin{matrix} \hat{x}_{n+1,n}\\ \hat{\dot{x}}_{n+1,n}\\ \end{matrix} \right] = \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \left[ \begin{matrix} \hat{x}_{n,n}\\ \hat{\dot{x}}_{n,n}\\ \end{matrix} \right] + \left[ \begin{matrix} 0.5 \Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \left( a_{n} + g \right) \]
In der obigen Gleichung gilt:
\[ \boldsymbol{F}= \left[ \begin{matrix} 1 & \Delta t \\ 0 & 1 \\ \end{matrix} \right] \]
\[ \boldsymbol{G}= \left[ \begin{matrix} 0.5 \Delta t^{2} \\ \Delta t \\ \end{matrix} \right] \]
\[ \boldsymbol{u}_{n} = \left( a_{n} + g \right) \]
Die Kovarianzextrapolationsgleichung
Die allgemeine Form der Kovarianzextrapolationsgleichung lautet:
| \( \boldsymbol{P}_{n,n} \) | ist die Kovarianzmatrix der aktuellen Zustandsschätzung |
| \( \boldsymbol{P}_{n+1,n} \) | ist die Kovarianzmatrix der nächsten Zustandsschätzung (Vorhersage) |
| \( \boldsymbol{F} \) | ist die Zustandsübergangsmatrix, die wir im Abschnitt „Modellierung linearer dynamischer Systeme“ hergeleitet haben |
| \( \boldsymbol{Q} \) | ist die Prozessrauschmatrix |
Die Schätzkovarianzmatrix in Matrixform lautet:
\[ \boldsymbol{P} = \left[ \begin{matrix} p_{x} & p_{x\dot{x}} \\ p_{\dot{x}x} & p_{\dot{x}} \\ \end{matrix} \right] \]
Die Elemente der Hauptdiagonalen der Matrix sind die Varianzen der Schätzung:
- \( p_{x} \) ist die Varianz der Höhenschätzung
- \( p_{\dot{x}} \) ist die Varianz der Geschwindigkeitsschätzung
- Die Einträge außerhalb der Diagonalen sind Kovarianzen
Wir haben die Zustandsübergangsmatrix \( \boldsymbol{F} \) bereits hergeleitet. Nun leiten wir die Prozessrauschmatrix \( \boldsymbol{Q} \) her.
Die Prozessrauschmatrix
Wir nehmen ein diskretes Rauschmodell (discrete noise model) an – das Rauschen ist bei jedem Zeitschritt unterschiedlich, aber zwischen den Zeitschritten konstant.
Die Prozessrauschmatrix für ein Modell mit konstanter Beschleunigung sieht so aus:
\[ \boldsymbol{Q} = \left[ \begin{matrix} \sigma_{x}^{2} & \sigma_{x\dot{x}}^{2} \\ \sigma_{\dot{x}x}^{2} & \sigma_{\dot{x}}^{2} \\ \end{matrix} \right] \]
Wir haben die \( Q \)-Matrix für das Modell mit konstanter Beschleunigung bereits hergeleitet. Die \( Q \)-Matrix für unser Beispiel lautet:
\[ \boldsymbol{Q} = \left[ \begin{matrix} \frac{\Delta t^{4}}{4} & \frac{\Delta t^{3}}{2} \\ \frac{\Delta t^{3}}{2} & \Delta t^{2} \\ \end{matrix} \right] \epsilon^{2} \]
Wobei:
- \( \Delta t \) die Zeit zwischen aufeinanderfolgenden Messungen ist
- \( \epsilon^{2} \) die Varianz des Zufallsfehlers der Beschleunigungsmessung ist
Im vorherigen Beispiel haben wir die zufällige Varianz der Systembeschleunigung \( \sigma_{a}^{2} \) als Faktor für die Prozessrauschmatrix verwendet. Hier haben wir jedoch einen Beschleunigungssensor, der die zufällige Beschleunigung des Systems misst. Der Beschleunigungssensorfehler \( v \) ist viel kleiner als die zufällige Systembeschleunigung; daher verwenden wir \( \epsilon^{2} \) als Faktor für die Prozessrauschmatrix.
Dadurch wird unsere Schätzunsicherheit deutlich geringer!
Nun können wir die Kovarianzextrapolationsgleichung für unser Beispiel aufschreiben:
Analyse des Beispiels
Die folgende Grafik vergleicht die wahren, gemessenen und geschätzten Werte der Raketenhöhe.
Wir sehen eine gute Tracking-Performance und Konvergenz des Kalman-Filters.
Die folgende Grafik vergleicht die wahren, gemessenen und geschätzten Werte der Raketengeschwindigkeit.
Es dauert etwa 2,5 Sekunden, bis die Schätzwerte auf die wahre Raketengeschwindigkeit konvergieren.
Am Anfang wird die geschätzte Höhe stark von den Messungen beeinflusst und stimmt nicht gut mit der wahren Raketenhöhe überein, da die Messungen sehr verrauscht sind.
Wenn der Kalman-Filter konvergiert, haben die verrauschten Messungen weniger Einfluss, und die geschätzte Höhe stimmt gut mit der wahren Höhe überein.
In diesem Beispiel gibt es keine Manöver, die Beschleunigungsänderungen verursachen. Wenn es sie gäbe, würde der Steuereingang (Beschleunigungssensor) die Zustandsextrapolationsgleichung aktualisieren.