Kalman Filter

Warum wir vorhersagen müssen

Wir beginnen damit, das Problem zu formulieren, um zu verstehen, warum wir einen Algorithmus zur Zustandsschätzung und Vorhersage benötigen.

Zur Veranschaulichung betrachten wir das Beispiel eines Verfolgungsradars:

Angenommen, wir haben ein Radar, das ein Flugzeug verfolgt. In diesem Szenario ist das Flugzeug das System, und die zu schätzende Größe ist seine Position, also der Systemzustand (system state).

Das Radar erfasst das Ziel, indem es einen schmalen Richtstrahl (pencil beam) auf das Flugzeug ausrichtet und Positionsmessungen liefert. Auf Basis dieser Messungen können wir den Systemzustand (die Position des Flugzeugs) schätzen.

Um das Flugzeug zu verfolgen, muss das Radar das Ziel in regelmäßigen Abständen erneut anpeilen, indem es den Strahl wieder in seine Richtung richtet. Das bedeutet: Das Radar muss die zukünftige Position des Flugzeugs für den nächsten Scan vorhersagen können. Gelingt das nicht, wird der Strahl möglicherweise in die falsche Richtung gelenkt und die Spur geht verloren. Für diese Vorhersage brauchen wir Wissen darüber, wie sich das Flugzeug bewegt. Anders gesagt benötigen wir ein Modell, das das Verhalten des Systems über die Zeit beschreibt: das dynamische Modell (dynamic model).

Um das Beispiel zu vereinfachen, betrachten wir eine eindimensionale Welt, in der sich das Flugzeug auf einer Geraden entweder auf das Radar zu bewegt oder sich davon entfernt.

Der Systemzustand ist hier die Entfernung des Flugzeugs vom Radar, bezeichnet mit \( r \). Das Radar sendet einen Impuls in Richtung des Flugzeugs; dieser wird am Ziel reflektiert und kehrt zum Radar zurück. Durch Messung der Zeit zwischen Senden und Empfangen und mit dem Wissen, dass sich der Impuls als elektromagnetische Welle mit Lichtgeschwindigkeit ausbreitet, lässt sich die Entfernung \( r \) leicht berechnen. Zusätzlich zur Entfernung kann das Radar auch die Geschwindigkeit \( v \) des Flugzeugs messen – ähnlich wie eine Polizeiradarpistole die Geschwindigkeit eines Autos über den Doppler-Effekt (Doppler effect) bestimmt.

Nehmen wir an, dass das Radar zum Zeitpunkt \( t_{0} \) Entfernung und Geschwindigkeit mit sehr hoher Genauigkeit und Präzision misst. Die gemessene Entfernung beträgt 10.000 Meter und die Geschwindigkeit 200 Meter pro Sekunde. Das ergibt den Systemzustand:

\[ r_{t_{0}} = 10,000m \]

Der nächste Schritt ist die Vorhersage des Systemzustands zum Zeitpunkt \( t_{1}=t_{0}+\Delta t \), wobei \( \Delta t \) die Wiederholrate (Revisit Time) der Zielanpeilung ist. Da wir erwarten, dass das Flugzeug seine Geschwindigkeit ungefähr konstant hält, kann ein dynamisches Modell mit konstanter Geschwindigkeit verwendet werden, um die zukünftige Position zu prognostizieren.

Die im Zeitintervall \( \Delta t \) zurückgelegte Strecke ergibt sich zu:

\[ \Delta r = v \cdot \Delta t \]

Bei einem Abtastintervall von 5 Sekunden ist die vorhergesagte Position zum Zeitpunkt \( t_{1} \):

\[ r_{t_{1}} = r_{t_{0}} + \Delta r = 10,000 + 200 \cdot 5 = 11,000m \]

Das ist ein elementarer Algorithmus, der auf einfachen Prinzipien beruht: Der aktuelle Systemzustand wird aus der Messung abgeleitet, und das dynamische Modell wird verwendet, um den zukünftigen Zustand vorherzusagen.

In der Realität ist es komplexer. Erstens sind Radarmessungen nicht perfekt präzise: Sie werden von Rauschen beeinflusst und enthalten einen gewissen Zufallsanteil. Wenn zehn verschiedene Radare zum selben Zeitpunkt die Entfernung des Flugzeugs messen würden, erhielten wir zehn leicht unterschiedliche Ergebnisse. Diese wären wahrscheinlich ähnlich, aber nicht identisch. Die Streuung der Messungen wird durch Messrauschen verursacht.

Das führt zur nächsten Frage: Wie sicher ist unsere Schätzung? Wir benötigen einen Algorithmus, der nicht nur eine Schätzung liefert, sondern auch angibt, wie zuverlässig sie ist.

Ein weiteres Problem ist die Genauigkeit des dynamischen Modells. Auch wenn wir annehmen, dass sich das Flugzeug mit konstanter Geschwindigkeit bewegt, können externe Einflüsse wie Wind Abweichungen verursachen. Solche unvorhersehbaren Einflüsse bezeichnet man als Prozessrauschen (process noise).

So wie wir die Sicherheit unserer messbasierten Schätzung bewerten möchten, wollen wir auch wissen, wie verlässlich unsere Vorhersage ist.

Der Kalman-Filter ist ein Algorithmus zur Zustandsschätzung, der sowohl eine Schätzung des aktuellen Zustands als auch eine Vorhersage des zukünftigen Zustands liefert – jeweils mit einem Maß für die Unsicherheit. Außerdem ist er ein optimaler Algorithmus, der die Unsicherheit der Zustandsschätzung minimiert. Deshalb ist der Kalman-Filter heute so weit verbreitet und bewährt.

Beispiel zum Kalman-Filter

Beginnen wir mit einem einfachen Beispiel: einem eindimensionalen Radar, das Entfernung und Geschwindigkeit misst, indem es einen Impuls in Richtung eines Flugzeugs sendet und das reflektierte Echo empfängt. Die Laufzeit zwischen dem Senden und dem Empfang des Echos liefert Informationen über die Entfernung \(r\), und die Frequenzverschiebung des Echos liefert Informationen über die Geschwindigkeit \(v\) (Doppler-Effekt).

In diesem Beispiel wird der Systemzustand durch die Entfernung \(r\) und die Geschwindigkeit \(v\) beschrieben. Wir definieren den Systemzustand durch den Zustandsvektor (state vector) \(\boldsymbol{x}\), der beide Größen enthält:

\[ \boldsymbol{x}=\left[\begin{matrix}r\\v\\\end{matrix}\right] \]

Vektoren (vectors) kennzeichnen wir durch fettgedruckte Kleinbuchstaben und Matrizen (matrices) durch fettgedruckte Großbuchstaben.

Da der Systemzustand mehr als eine Variable umfasst, verwenden wir Werkzeuge der linearen Algebra, z. B. Vektoren und Matrizen, um die Mathematik des Kalman-Filters zu beschreiben. Wenn du dich mit linearer Algebra nicht wohlfühlst, sieh dir bitte den Abschnitt Kalman-Filter in einer Dimension im Online-Tutorial oder im Buch an. Dort werden die Kalman-Filter-Gleichungen und ihre Herleitung mit Schulmathematik erklärt – inklusive vier vollständig gelöster Beispiele.

Iteration 0

Initialisierung des Filters

In diesem Beispiel verwenden wir die erste Messung, um den Kalman-Filter zu initialisieren (mehr zu Initialisierungstechniken und ihrem Einfluss auf die Filterleistung in Kapitel 21 des Buchs). Zum Zeitpunkt \(t_0\) misst das Radar eine Entfernung von \(10{,}000m\) und eine Geschwindigkeit von \(200m/s\). Messungen bezeichnen wir mit \(\boldsymbol{z}\).
Wir fassen die Messungen im Messvektor (measurement vector) \(\boldsymbol{z}\) zusammen:

\[ \boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

Der Index \(0\) kennzeichnet den Zeitpunkt \(t_0\).

Eine Messung entspricht nicht exakt dem wahren Systemzustand. Messwerte werden durch zufälliges Rauschen verfälscht; daher ist jede Messung eine Zufallsvariable.

Können wir dieser Messung vertrauen? Wie sicher ist sie? Jede Messung wird von einem quadrierten Wert der Messunsicherheit (measurement uncertainty) begleitet (auch Messfehler (measurement error) genannt). Diese quadrierte Unsicherheit ist die Varianz (variance) der Messung. Mehr zur Varianz findest du im Abschnitt Wesentliche Grundlagen I. Eine ausführlichere Diskussion der Messunsicherheit gibt es im Abschnitt Kalman-Filter in einer Dimension.

In Radarsystemen wird die Messunsicherheit weitgehend durch das Verhältnis von empfangener Signalstärke zu Rauschen bestimmt. Je höher das Signal-Rausch-Verhältnis (signal-to-noise ratio), desto kleiner ist die Messvarianz und desto größer ist unser Vertrauen in die Messung.

Die folgende Abbildung vergleicht einen Fall mit schwachem Signal und einen Fall mit starkem Signal in Gegenwart von Rauschen.

Nehmen wir an, die Standardabweichung (standard deviation) der Entfernungs-Messung beträgt \( 4m \) und die Standardabweichung der Geschwindigkeits-Messung beträgt \( 0.5m/s \). Da die Varianz das Quadrat der Standardabweichung ist, ist die quadrierte Messunsicherheit (bezeichnet mit \( \boldsymbol{R} \)):

\[ \boldsymbol{R}_0=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

\( \boldsymbol{R} \) ist eine Kovarianzmatrix (covariance matrix). Die Hauptdiagonale enthält die Varianzen, und die Nebendiagonalen enthalten die Kovarianzen zwischen den Messgrößen.

\[ \boldsymbol{R}=\left[\begin{matrix}\sigma_r^2&\sigma_{rv}^2\\[0.5em]\sigma_{vr}^2&\sigma_v^2\\\end{matrix}\right] \]

In diesem Beispiel nehmen wir an, dass die Fehler in der Entfernungs- und Geschwindigkeitsmessung nicht miteinander zusammenhängen. Daher setzen wir die Nebendiagonalelemente der Messkovarianzmatrix auf Null.

Zur Auffrischung von Varianz und Standardabweichung siehe den Abschnitt Wesentliche Grundlagen I im Online-Tutorial.
Zur Auffrischung von Kovarianzmatrizen siehe den Abschnitt Wesentliche Grundlagen II.

Während der Initialisierung ist unsere einzige Information eine einzelne Messung. In diesem Beispiel werden Messung und Systemzustand durch dieselben Größen beschrieben (\(r\) und \(v\)). Daher können wir die Messung als anfängliche Schätzung des Systemzustands verwenden. Das ist nur im Initialisierungsschritt möglich:

\[ \boldsymbol{\hat{x}}_{0,0}=\boldsymbol{z}_0=\left[\begin{matrix}10{,}000\\200\\\end{matrix}\right] \]

Der Index \(0,0\) bedeutet:

• Der erste Index bezieht sich auf die Systemzeit, hier also \(t_0\).
• Der zweite Index bezieht sich auf den Zeitpunkt, zu dem die Schätzung berechnet wurde, ebenfalls \(t_0\).

Mit anderen Worten: Die Schätzung gilt für \(t_0\) und wurde auch zum Zeitpunkt \(t_0\) berechnet.

Vorhersage

Nun sagen wir den nächsten Zustand vorher. Nehmen wir an, die Wiederholzeit beträgt 5 Sekunden \((\Delta t=5s)\), also ist \(t_1=5s\).

Um den zukünftigen Systemzustand zu schätzen, müssen wir beschreiben, wie sich das System über die Zeit entwickelt. In diesem Beispiel nehmen wir ein dynamisches Modell mit konstanter Geschwindigkeit an (Bewegungsmodell):

\[ v_{1} = v_{0} = v \] \[ r_{1} = r_{0} + v_{0}\Delta t \]

(Beispiele für beschleunigte dynamische Modelle findest du in Kapitel 9 des Buchs.)

Beschreiben wir das dynamische Modell in Matrixform:

\[ {\hat{\boldsymbol{x}}}_{1,0}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{0,0} \]

Der Index \(1,0\) bedeutet:

• Der erste Index bezieht sich auf die Systemzeit, hier \(t_1\).
• Der zweite Index bezieht sich auf den Zeitpunkt, zu dem die Schätzung berechnet wurde, hier \(t_0\).

Damit ist \( \hat{\boldsymbol{x}}_{1,0} \) unsere Schätzung des Systemzustands zum Zeitpunkt \(t_1\), berechnet mit Informationen, die zum Zeitpunkt \(t_0\) verfügbar waren. Mit anderen Worten: Es ist eine Vorhersage des zukünftigen Zustands.

Die Matrix \( \boldsymbol{F} \) heißt Zustandsübergangsmatrix (state transition matrix) und beschreibt, wie sich der Systemzustand über die Zeit entwickelt:

\[ {\hat{\boldsymbol{x}}}_{1,0}=\left[\begin{matrix}{\hat{r}}_{1,0}\\{\hat{v}}_{1,0}\\\end{matrix}\right]=\left[\begin{matrix}1&\Delta t\\0&1\\\end{matrix}\right]\left[\begin{matrix}{\hat{r}}_{0,0}\\{\hat{v}}_{0,0}\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}10,000\\200\\\end{matrix}\right]=\left[\begin{matrix}11,000\\200\\\end{matrix}\right] \]

Anhang C des Buchs beschreibt eine Methode, um die Dynamik beliebiger linearer Systeme zu modellieren.

Die Gleichung

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} \]

ist die Gleichung zur Zustandsextrapolation (state extrapolation) bzw. Vorhersage. Sie sagt uns, wie wir den nächsten Zustand aus dem aktuellen berechnen: Sie nimmt unsere aktuelle Zustandsschätzung und nutzt das Bewegungsmodell, um den Zustand im nächsten Zeitschritt vorherzusagen.

Die vollständige Form der Zustandsextrapolationsgleichung lautet:

\[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]

wobei:

• \(\boldsymbol{u}_{n}\) ist eine Eingangs- bzw. Steuervariable (control variable)
• \(\boldsymbol{G}\) ist eine Eingangsübergangsmatrix (input transition matrix)

Der Eingangsvektor (input vector) repräsentiert zusätzliche Informationen, die dem Kalman-Filter zur Verfügung gestellt werden, z. B. Messwerte eines Bord-Beschleunigungssensors.

In diesem einfachen Beispiel nehmen wir keinen Eingang an, also \(\boldsymbol{u}_n=0\).

Ein Beispiel mit Eingangsterm findest du auf der Seite Gleichung zur Zustandsextrapolation im Online-Tutorial oder im vollständig gelösten Beispiel 10 im Buch.

Jede Messung und jede Schätzung im Kalman-Filter enthält Unsicherheitsinformation. Nach der Vorhersage des nächsten Zustands sollten wir daher auch fragen: Wie präzise ist diese Vorhersage?

Die quadrierte Unsicherheit der aktuellen Zustandsschätzung wird durch die Kovarianzmatrix dargestellt:

\[ \boldsymbol{P}_{0,0}=\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right] \]

Die Kovarianz der Vorhersage wird jedoch nicht so berechnet:

\[ \textcolor{red}{\xcancel{\textcolor{black}{ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0} }}} \]

Denn \(\boldsymbol{P}\) ist eine Kovarianzmatrix, und Varianzen/Kovarianzen enthalten quadratische Terme.

Die Kovarianzextrapolationsgleichung (covariance extrapolation equation) (ohne Prozessrauschen) lautet:

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T \]

Die vollständige Herleitung findest du im Abschnitt Kovarianzextrapolationsgleichung des Online-Tutorials.

In unserem Beispiel:

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\0&0.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}16&0\\1.25&0.25\\\end{matrix}\right]=\left[\begin{matrix}\colorbox{yellow}{$22.25$}&1.25\\1.25&\colorbox{yellow}{$0.25$}\\\end{matrix}\right] $$

Betrachte die Hauptdiagonale der Kovarianzmatrix.

Die Geschwindigkeitsvarianz \(\sigma_v^2\) ist weiterhin \(0.25 \, m^2/s^2\). Sie ändert sich nicht, weil das dynamische Modell konstante Geschwindigkeit annimmt.

Im Gegensatz dazu steigt die Entfernungsvarianz \(\sigma_r^2\) von \(16m^2\) auf \(22.25m^2\). Das spiegelt wider, dass Unsicherheit in der Geschwindigkeit im Laufe der Zeit zu wachsender Unsicherheit in der Entfernung führt.

Wie oben erwähnt, ist die Annahme konstanter Geschwindigkeit nicht vollständig korrekt. In der Realität kann die Geschwindigkeit des Flugzeugs durch externe, unbekannte Faktoren wie Wind beeinflusst werden. Dadurch ist die tatsächliche Vorhersageunsicherheit größer als die des einfachen Modells.

Diese unvorhersehbaren Einflüsse nennt man Prozessrauschen und bezeichnet sie mit \(\boldsymbol{Q}\). Um diese Effekte zu berücksichtigen, addieren wir \(\boldsymbol{Q}\) zur Vorhersagekovarianz-Gleichung:

\[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]

Um ein Gefühl dafür zu bekommen, wie Prozessrauschen die Leistung des Kalman-Filters beeinflusst, siehe Beispiel 6 im Online-Tutorial.

Nehmen wir an, die Standardabweichung der zufälligen Beschleunigung beträgt \(\sigma_a=0.2m/s^2\). Das repräsentiert die Unsicherheit der zufälligen Flugzeugbeschleunigung durch unvorhersehbare Umwelteinflüsse.

Entsprechend ist die Varianz der zufälligen Beschleunigung \(\sigma_a^2=0.04m^2/s^4\).

In unserem Beispiel ergibt sich die Prozessrauschmatrix zu:

$$ \boldsymbol{Q} = \left[\begin{matrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\[0.5em] \frac{\Delta t^3}{2} & \Delta t^2 \end{matrix}\right] \sigma_a^2 $$

Mit \(\Delta t=5\mathrm{s}\) und \(\sigma_a^2=0.04\,\mathrm{m}^2/\mathrm{s}^4\) wird daraus:

$$ \boldsymbol{Q}=\left[\begin{matrix}\frac{625}{4}&\frac{125}{2}\\[0.5em] \frac{125}{2}&25\\\end{matrix}\right]0.04=\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right] $$

Die Herleitung der Prozessrauschmatrix ist in Abschnitt 8.2.2 des Buchs beschrieben.

Nach Hinzufügen des Prozessrauschens ist die quadrierte Unsicherheit unserer Vorhersage:

$$ \boldsymbol{P}_{1,0}=\boldsymbol{F}\boldsymbol{P}_{0,0}\boldsymbol{F}^T+\boldsymbol{Q}\ =\left[\begin{matrix}22.25&1.25\\1.25&0.25\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]\ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] $$

Zusammenfassung: Iteration 0

• Initialisierung
  Wir haben den Kalman-Filter initialisiert, indem wir die erste Messung als anfängliche Zustandsschätzung \( {\hat{\boldsymbol{x}}}_{0,0} \) und die Messkovarianz als anfängliche Zustandskovarianz \(\boldsymbol{P}_{0,0}\) verwendet haben.
  Beachte: Das ist nur in der Initialisierungsphase möglich.
• Vorhersage
  Wir haben den Zustand und seine Unsicherheit für den nächsten Zeitschritt vorhergesagt, wenn das Radar das Flugzeug erneut anpeilt. Die Vorhersagegleichungen des Kalman-Filters sind:
  
  
  Gleichung zur Zustandsextrapolation (State Extrapolation Equation)
  \[ {\hat{\boldsymbol{x}}}_{n+1,n}=\boldsymbol{F}{\hat{\boldsymbol{x}}}_{n,n} + \boldsymbol{G}\boldsymbol{u}_n \]
  Gleichung zur Kovarianzextrapolation (Covariance Extrapolation Equation)
  \[ \boldsymbol{P}_{n+1,n}=\boldsymbol{F}\boldsymbol{P}_{n,n}\boldsymbol{F}^T + \boldsymbol{Q}\]
  wobei:
  
  • \(\hat{\boldsymbol{x}}_{n,n}\) ist der geschätzte Systemzustandsvektor im Zeitschritt \(n\)
  • \(\hat{\boldsymbol{x}}_{n+1,n}\) ist der vorhergesagte Systemzustandsvektor für \(n+1\), berechnet mit Informationen aus \(n\)
  • \(\boldsymbol{u}_n\) ist die Steuer-/Eingangsvariable und repräsentiert bekannte externe Eingänge des Systems
  • \(\boldsymbol{F}\) ist die Zustandsübergangsmatrix
  • \(\boldsymbol{G}\) ist die Eingangs- (Steuer-) Matrix und bildet Eingänge auf Zustandsvariablen ab
  • \(\boldsymbol{P}_{n,n}\) ist die Kovarianzmatrix (quadrierte Unsicherheit) des aktuellen Zustands
  • \(\boldsymbol{P}_{n+1,n}\) ist die Kovarianzmatrix (quadrierte Unsicherheit) des vorhergesagten Zustands
  • \(\boldsymbol{Q}\) ist die Prozessrauschmatrix

Iteration 1

Filter-Update

Nehmen wir die zweite Messung zum Zeitpunkt \(t_1\) an:

\[ \boldsymbol{z}_1=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] \]

Aufgrund eines starken Rauschspikes während dieser Messung ist das Signal-Rausch-Verhältnis deutlich kleiner als bei der ersten Messung. Dadurch ist die Unsicherheit der zweiten Messung größer.

Nehmen wir an, die Standardabweichung der Entfernungs-Messung beträgt \(6m\) und die Standardabweichung der Geschwindigkeits-Messung \(1.5m/s\). Die entsprechende Messkovarianzmatrix ist:

\[ \boldsymbol{R}_1=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

Wir möchten den aktuellen Systemzustand \(\hat{\boldsymbol{x}}_{1,1}\) schätzen. Zum Zeitpunkt \(t_1\) haben wir zwei Informationsquellen:

• den vorhergesagten Zustand \(\hat{\boldsymbol{x}}_{1,0}\) (aus dem vorherigen Schritt), und
• die neue Messung \(\boldsymbol{z}_1\)

Welcher Information sollten wir vertrauen?

Intuitiv würde man vielleicht die Messung als aktuelle Schätzung verwenden, also \(\hat{\boldsymbol{x}}_{1,1}=\boldsymbol{z}_1\), weil sie aktueller ist als die Vorhersage.

Andererseits ist die Messung auch verrauschter. Vergleichen wir die Hauptdiagonalelemente der Vorhersagekovarianz \(\boldsymbol{P}_{1,0}\) mit der Messkovarianz \(\boldsymbol{R}_1\), sehen wir, dass die Vorhersageunsicherheit kleiner ist als die Messunsicherheit:

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right] \]

Sollen wir also die neue Messung ignorieren und die Vorhersage beibehalten, also \(\hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}\)?

Dann würden wir jedoch die neue Information der aktuellen Messung verlieren.

Die zentrale Idee des Kalman-Filters ist, dass wir weder das eine noch das andere tun. Stattdessen kombinieren wir Vorhersage und Messung und gewichten dabei stärker die Information mit der kleineren Unsicherheit.

Die Lösung ist ein gewichtetes Mittel aus Messung und Vorhersage:

\[ \hat{x}_{1,1}=K_1 z_1\ +\ \left({1-\ K}_1\right){\hat{x}}_{1,0}, \quad 0\leq K_1 \leq 1 \]

Hier ist das Gewicht \(K_1\) die Kalman-Verstärkung (Kalman Gain). Sie legt fest, wie stark Messung und Vorhersage gewichtet werden – so, dass die Unsicherheit der Schätzung minimiert wird. Genau das macht den Kalman-Filter zu einem optimalen Filter (sofern sich System und Rauschen gemäß den Modellannahmen verhalten).

Die Kalman-Gain-Gleichung führen wir gleich ein, aber zuerst betrachten wir die Zustandsaktualisierungsgleichung (State Update Equation). In Matrixform lautet sie:

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + (\boldsymbol{I} - \boldsymbol{K}_1)\hat{\boldsymbol{x}}_{1,0} \]

wobei \(\boldsymbol{I}\) die Einheitsmatrix ist (eine quadratische Matrix mit Einsen auf der Hauptdiagonale und Nullen sonst).

Schreiben wir die Gleichung um:

\[ \hat{\boldsymbol{x}}_{1,1}=\boldsymbol{K}_1\boldsymbol{z}_1 + \hat{\boldsymbol{x}}_{1,0} - \boldsymbol{K}_1\hat{\boldsymbol{x}}_{1,0}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}) \]

Diese Form zeigt: Der aktualisierte Zustand ist die Vorhersage \(\hat{\boldsymbol{x}}_{1,0}\) plus ein Korrekturterm \(\boldsymbol{K}_1\left(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\right)\).

Die Korrektur ist proportional zur Differenz zwischen Messung und Vorhersage \(\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}\). Diese Differenz heißt Innovation (innovation) oder Residuum (residual).

In unserem Beispiel sind sowohl Systemzustand als auch Messung Vektoren derselben physikalischen Größen (Entfernung und Geschwindigkeit). Daher können wir \(\hat{\boldsymbol{x}}_{1,0}\) direkt von \(\boldsymbol{z}_1\) abziehen.

Das ist jedoch nicht immer so. Allgemein können Messung und Systemzustand in unterschiedlichen physikalischen Domänen liegen. Beispiel: Ein digitales Thermometer misst ein elektrisches Signal, während der Systemzustand die Temperatur ist.

Deshalb muss der vorhergesagte Zustand zunächst in den Messraum transformiert werden:

\[ \boldsymbol{H} \hat{\boldsymbol{x}}_{1,0} \]

Die Matrix \(\boldsymbol{H}\) heißt Beobachtungsmatrix (observation matrix) (auch Messmatrix (measurement matrix)). Sie bildet Zustandsvariablen auf die tatsächlich gemessenen Größen ab.

In unserem Beispiel ist die Beobachtungsmatrix einfach die Einheitsmatrix:

\[ \boldsymbol{H}=\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]=\boldsymbol{I} \]

Mehr zur Beobachtungsmatrix findest du im Abschnitt Messgleichung des Online-Tutorials sowie in Beispiel 9 und 10 im Buch.

Damit können wir die Zustandsaktualisierungsgleichung schreiben als:

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

Die Innovation \(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}\) repräsentiert neue Information.

Der Kalman-Gain bestimmt, wie stark diese neue Information den vorhergesagten Zustand verändern soll – also wie stark wir die Vorhersage korrigieren.

Eindimensionaler Fall

Im eindimensionalen Fall ist der Kalman-Gain gegeben durch:

\[ K_n=\frac{p_{n,\ n-1}}{p_{n,\ n-1}+r_n} \]

wobei:

• \(p_{n,\ n-1}\) eine vorhergesagte Zustandsvarianz ist
• \(r_n\) eine Messvarianz ist

Der Kalman-Gain wird so gewählt, dass die Varianz der aktualisierten Schätzung \(p_{n,n}\) minimiert wird. Deshalb ist der Kalman-Filter optimal.

Um Intuition aufzubauen und die vollständige Herleitung im eindimensionalen Fall zu sehen, lies den Abschnitt Kalman-Filter in einer Dimension im Online-Tutorial.

Mehrdimensionaler Fall

Beim mehrdimensionalen Kalman-Filter wird der Kalman-Gain zur Matrix und lautet:

\[ \boldsymbol{K}_n=\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1} \]

Die Herleitung der mehrdimensionalen Kalman-Gain-Gleichung findest du im Abschnitt Kalman Gain des Online-Tutorials.

Berechnen wir den Kalman-Gain für \(t_1\):

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1} \]

In unserem Beispiel gilt \(\boldsymbol{H}=\boldsymbol{I}\) und \(\boldsymbol{H}^T=\boldsymbol{I}\).

Setzen wir die Matrizen ein:

\[ \boldsymbol{P}_{1,0}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right], \quad \boldsymbol{R}_1=\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\boldsymbol{P}_{1,0}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{1,0}\boldsymbol{H}^T+\boldsymbol{R}_1\right)^{-1}=\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]+\left[\begin{matrix}36&0\\0&2.25\\\end{matrix}\right]\right)^{-1} =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left(\left[\begin{matrix}64.5&3.75\\3.75&3.5\\\end{matrix}\right]\right)^{-1} \]

\[ =\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]\left[\begin{matrix}0.0165&-0.0177\\-0.0177&0.3047\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

\[ \boldsymbol{K}_1=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right] \]

Die aktualisierte Zustandsschätzung ist:

\[ \hat{\boldsymbol{x}}_{1,1}=\hat{\boldsymbol{x}}_{1,0}+\boldsymbol{K}_1(\boldsymbol{z}_1 - \boldsymbol{H}\hat{\boldsymbol{x}}_{1,0}) \]

In unserem Beispiel ist \(\boldsymbol{H}=\boldsymbol{I}\), daher ist die Innovation einfach:

\[ \boldsymbol{z}_1 - \boldsymbol{I}\hat{\boldsymbol{x}}_{1,0}=\boldsymbol{z}_1 - \hat{\boldsymbol{x}}_{1,0}=\left[\begin{matrix}11{,}020\\202\\\end{matrix}\right] - \left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]=\left[\begin{matrix}20\\2\\\end{matrix}\right] \]

Nun wenden wir die Korrektur an:

\[ \boldsymbol{K}_1\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\left[\begin{matrix}20\\2\\\end{matrix}\right]=\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right] \]

Schließlich:

\[ \hat{\boldsymbol{x}}_{1,1}=\left[\begin{matrix}11{,}000\\200\\\end{matrix}\right]+\left[\begin{matrix}9.37\\1.43\\\end{matrix}\right]=\left[\begin{matrix}11{,}009.37\\201.43\\\end{matrix}\right] \]

Nachdem wir den aktuellen Zustand geschätzt haben, möchten wir auch die Unsicherheit dieser Schätzung quantifizieren.

Eindimensionaler Fall

Im eindimensionalen Fall lautet die Kovarianzaktualisierungsgleichung (Covariance Update Equation):

\[ p_{n,n}=(1-K_n)p_{n,\ n-1} \]

Die Herleitung findest du im Abschnitt Kalman-Filter in einer Dimension des Online-Tutorials.

Mehrdimensionaler Fall

Joseph-Form

Beim mehrdimensionalen Kalman-Filter schreibt man die Kovarianzaktualisierung häufig in einer numerisch stabilen Form, der Joseph-Form (Joseph form), die von Peter Joseph eingeführt wurde.

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1}(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})^T + \boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]

wobei:

• \(\boldsymbol{P}_{n,n}\) ist die Kovarianz der aktualisierten (a-posteriori (posterior)) Zustandsschätzung
• \(\boldsymbol{P}_{n,n-1}\) ist die Kovarianz der vorhergesagten (a-priori (prior)) Zustandsschätzung
• \(\boldsymbol{K}_n\) ist der Kalman-Gain
• \(\boldsymbol{H}\) ist die Beobachtungs- (Mess-) Matrix
• \(\boldsymbol{R}_n\) ist die Messrausch-Kovarianzmatrix
• \(\boldsymbol{I}\) ist die Einheitsmatrix (quadratische Matrix mit Einsen auf der Hauptdiagonale und Nullen sonst)

Die Herleitung findest du im Abschnitt Kovarianzaktualisierungsgleichung des Online-Tutorials.

Vereinfachte Form

In der Literatur findet man häufig auch die vereinfachte Kovarianzaktualisierung:

\[ \boldsymbol{P}_{n,n}=(\boldsymbol{I} - \boldsymbol{K}_n\boldsymbol{H})\boldsymbol{P}_{n,n-1} \]

Die Herleitung dazu findest du im Abschnitt Vereinfachte Kovarianzaktualisierungsgleichung.

Beide Formen liefern bei exakter Arithmetik dasselbe Ergebnis. Für Computerimplementierungen wird jedoch meist die Joseph-Form bevorzugt, weil sie numerisch stabiler ist.

Nur für dieses Beispiel verwenden wir die vereinfachte Kovarianzaktualisierung:

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1\boldsymbol{H})\boldsymbol{P}_{1,0} \]

In unserem Beispiel ist \(\boldsymbol{H}=\boldsymbol{I}\), also:

\[ \boldsymbol{P}_{1,1}=(\boldsymbol{I} - \boldsymbol{K}_1)\boldsymbol{P}_{1,0} \]

Setzen wir die Matrizen ein:

\[ \boldsymbol{P}_{1,1}=\left(\left[\begin{matrix}1&0\\0&1\\\end{matrix}\right] - \left[\begin{matrix}0.4048&0.6377\\0.0399&0.3144\\\end{matrix}\right]\right)\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right] \]

\[ =\left[\begin{matrix}0.5952&-0.6377\\-0.0399&0.6856\\\end{matrix}\right]\left[\begin{matrix}28.5&3.75\\3.75&1.25\\\end{matrix}\right]=\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right] \]

Analyse des Ergebnisses

Die Unsicherheit der aktualisierten Schätzung ist kleiner als sowohl die Vorhersageunsicherheit als auch die Messunsicherheit:

\[ \boldsymbol{P}_{1,1}=\left[\begin{matrix}\colorbox{yellow}{$14.57$}&1.43\\1.43&\colorbox{yellow}{$0.71$}\\\end{matrix}\right]\ \ \ \ \ \ \boldsymbol{P}_{1,0}=\ \left[\begin{matrix}\colorbox{yellow}{$28.5$}&3.75\\3.75&\colorbox{yellow}{$1.25$}\\\end{matrix}\right]\ \ \ \ \ \boldsymbol{R}_\mathbf{1}=\left[\begin{matrix}\colorbox{yellow}{$36$}&0\\0&\colorbox{yellow}{$2.25$}\\\end{matrix}\right] \]

Durch die Kombination von Messung und Vorhersage und die Gewichtung mit dem Kalman-Gain erhalten wir eine Schätzung mit geringerer Unsicherheit.

Das Hinzufügen neuer Information reduziert die Schätzunsicherheit immer – selbst dann, wenn diese Information eine hohe Unsicherheit hat. Den mathematischen Beweis findest du im Kapitel Sensorfusion (Sensor Fusion) im Buch sowie in Anhang G und H. Aus theoretischer Sicht sollten neue Messungen daher niemals ignoriert werden.

In der Praxis ist es jedoch oft notwendig, bestimmte Messungen zu verwerfen. Praktische Methoden zum Umgang mit unzuverlässigen Messungen findest du im Kapitel Ausreißerbehandlung (Outlier Treatment) im Buch.

Vorhersage

Der Vorhersageschritt von Iteration 1 (von \( t_1 \) nach \( t_2 \)) ist identisch zum Vorhersageschritt von Iteration 0 (von \( t_0 \) nach \( t_1 \)) – mit dem Unterschied, dass wir jetzt von der aktualisierten Schätzung \(\hat{\boldsymbol{x}}_{1,1}\) und \(\boldsymbol{P}_{1,1}\) starten.

Zustandsvorhersage

\[ \hat{\boldsymbol{x}}_{2,1}=\boldsymbol{F}\hat{\boldsymbol{x}}_{1,1} \]

\[ \hat{\boldsymbol{x}}_{2,1}=\left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}11,009.37\\201.43\\\end{matrix}\right]=\left[\begin{matrix}12,016.5\\201.43\\\end{matrix}\right] \]

Kovarianzvorhersage

\[ \boldsymbol{P}_{2,1}=\boldsymbol{F}\boldsymbol{P}_{1,1}\boldsymbol{F}^\top + \boldsymbol{Q} \]

\[ \boldsymbol{P}_{2,1}=\ \left[\begin{matrix}1&5\\0&1\\\end{matrix}\right]\left[\begin{matrix}14.57&1.43\\1.43&0.71\\\end{matrix}\right]\left[\begin{matrix}1&0\\5&1\\\end{matrix}\right]+\left[\begin{matrix}6.25&2.5\\2.5&1\\\end{matrix}\right]=\left[\begin{matrix}52.86&7.47\\7.47&1.71\\\end{matrix}\right] \]

Man sieht, dass beide Varianzen im Vorhersageschritt wieder ansteigen. Das liegt daran, dass Unsicherheit mit der Zeit ohne neue Messung natürlicherweise wächst. Insbesondere führt Unsicherheit in der Geschwindigkeit zu zusätzlicher Unsicherheit in der Entfernung – deshalb steigt die Entfernungsvarianz schneller als die Geschwindigkeitsvarianz.

Zusammenfassung: Iteration 1

• Update
  
  • Wir schätzen den aktuellen Systemzustand \(\hat{\boldsymbol{x}}_{1,1}\) als gewichtete Kombination aus dem vorhergesagten Zustand \(\hat{\boldsymbol{x}}_{1,0}\) und der Messung \(\boldsymbol{z}_1\).
    Die Gewichtung wird durch den Kalman-Gain \(K_1\) bestimmt. Er wird aus der Vorhersagekovarianz \(\boldsymbol{P}_{1,0}\) und der Messkovarianz \(\boldsymbol{R}_1\) berechnet und minimiert die Unsicherheit der aktualisierten Schätzung \(\boldsymbol{P}_{1,1}\).
  • Die Update-Gleichungen des Kalman-Filters sind:
    
    
    Zustandsaktualisierungsgleichung
    \[ \hat{\boldsymbol{x}}_{n,n}=\hat{\boldsymbol{x}}_{n,n-1}+\boldsymbol{K}_n\left(\boldsymbol{z}_n\ -\ \boldsymbol{H}\hat{\boldsymbol{x}}_{n,n-1}\right) \]
    Kovarianzaktualisierungsgleichung (Joseph-Form)
    \[ \boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)^T+\boldsymbol{K}_n\boldsymbol{R}_n\boldsymbol{K}_n^T \]
    Oder die vereinfachte Form
    \[\boldsymbol{P}_{n,n}=\left(\boldsymbol{I}-\boldsymbol{K}_n\boldsymbol{H}\right)\boldsymbol{P}_{n,n-1}\]
    Kalman-Gain-Gleichung
    \[ \boldsymbol{K}_n=\ \boldsymbol{P}_{n,n-1}\boldsymbol{H}^T\left(\boldsymbol{H}\boldsymbol{P}_{n,n-1}\boldsymbol{H}^T+\boldsymbol{R}_n\right)^{-1}\]
  wobei:
  
  • \( \hat{\boldsymbol{x}}_{n,n} \) die aktualisierte Zustandsschätzung im Zeitschritt n ist
  • \( \hat{\boldsymbol{x}}_{n,n-1} \) der vorhergesagte Zustand im Zeitschritt n ist, berechnet mit Informationen aus n-1
  • \( \boldsymbol{z}_n \) der Messvektor ist
  • \( \boldsymbol{P}_{n,n} \) die Kovarianz der aktualisierten Zustandsschätzung ist
  • \( \boldsymbol{P}_{n,n-1} \) die Kovarianz der vorhergesagten Zustandsschätzung ist
  • \( \boldsymbol{K}_n \) der Kalman-Gain ist
  • \( \boldsymbol{H} \) die Beobachtungs- (Mess-) Matrix ist
  • \( \boldsymbol{R}_n \) die Messrausch-Kovarianzmatrix ist
  • \( \boldsymbol{I} \) die Einheitsmatrix ist
• Vorhersage
  Der Vorhersageschritt in Iteration 1 ist derselbe wie in Iteration 0.
  Wir propagieren die aktuelle Zustandsschätzung und ihre Kovarianz mit dem Zustandsübergangsmodell in den nächsten Zeitschritt, wenn das Radar das Flugzeug erneut anpeilt.

Zusammenfassung des Beispiels