Kalman-Filter in einer Dimension
In diesem Kapitel leiten wir den Kalman-Filter (Kalman Filter) in einer Dimension her. Das Hauptziel dieses Kapitels ist es, das Konzept des Kalman-Filters einfach und intuitiv zu erklären, ohne Mathematikwerkzeuge zu verwenden, die komplex und verwirrend wirken können.
Wir arbeiten uns Schritt für Schritt zu den Kalman-Filter-Gleichungen vor.
Im ersten Abschnitt leiten wir die Kalman-Filter-Gleichungen ohne Prozessrauschen (process noise) her. Im zweiten Abschnitt fügen wir das Prozessrauschen hinzu.
Eindimensionaler Kalman-Filter ohne Prozessrauschen
Wie ich bereits erwähnt habe, basiert der Kalman-Filter auf fünf Gleichungen. Mit zwei davon sind wir bereits vertraut:
- Die Zustandsaktualisierungsgleichung (state update equation)
- Die Gleichung des dynamischen Modells (dynamic model equation)
In diesem Kapitel leiten wir drei weitere Kalman-Filter-Gleichungen her und überarbeiten die Zustandsaktualisierungsgleichung.
Wie der \( \alpha -\beta -(\gamma) \)-Filter nutzt der Kalman-Filter den Algorithmus „Messen, Update, Vorhersagen“ („Measure, Update, Predict“).
Im Gegensatz zum \( \alpha -\beta -(\gamma) \)-Filter behandelt der Kalman-Filter Messungen, die aktuelle Zustandsschätzung (state estimation) und die nächste Zustandsschätzung (Vorhersagen; predictions) als normalverteilte Zufallsvariablen (random variables). Eine Zufallsvariable wird durch Mittelwert (mean) und Varianz (variance) beschrieben.
Das folgende Diagramm liefert eine schematische Übersicht des Kalman-Filter-Algorithmus:
Erinnern wir uns an unser erstes Beispiel (Gewichtsmessung eines Goldbarrens): Wir haben mehrere Messungen durchgeführt und die Schätzung durch Mittelung berechnet.
Wir erhielten folgendes Ergebnis:
Das obige Diagramm zeigt die wahren, gemessenen und geschätzten Werte in Abhängigkeit von der Anzahl der Messungen.
Schätzung als Zufallsvariable
Der Unterschied zwischen den Schätzungen (rote Linie) und den wahren Werten (grüne Linie) ist der Schätzfehler (estimation error). Wie man sieht, wird der Schätzfehler kleiner, je mehr Messungen wir durchführen, und er konvergiert gegen null, während der geschätzte Wert gegen den wahren Wert konvergiert. Wir kennen den Schätzfehler nicht, aber wir können die Unsicherheit (uncertainty) der Zustandsschätzung abschätzen.
Wir bezeichnen die Varianz der Zustandsschätzung mit \( p \).
Messung als Zufallsvariable
Die Messfehler sind die Unterschiede zwischen den Messungen (blaue Stichproben) und den wahren Werten (grüne Linie). Da die Messfehler zufällig sind, können wir sie durch die Varianz ( \( \sigma ^{2} \) ) beschreiben. Die Standardabweichung \( \left( \sigma \right) \) der Messfehler ist die Messunsicherheit (measurement uncertainty).
Wir bezeichnen die Messvarianz mit \( r \).
Die Varianz der Messfehler kann vom Hersteller des Messgeräts angegeben, berechnet oder empirisch durch ein Kalibrierverfahren bestimmt werden.
Wenn wir beispielsweise eine Waage verwenden, können wir sie kalibrieren, indem wir ein Objekt mit bekanntem Gewicht mehrfach messen und daraus empirisch die Standardabweichung bestimmen.
Bei fortgeschrittenen Sensoren wie Radar hängt die Messunsicherheit von mehreren Parametern ab, z. B. SNR (Signal-to-Noise Ratio; Signal-Rausch-Verhältnis), Strahlbreite, Bandbreite, Verweilzeit am Ziel, Taktstabilität und mehr. Jede Radarmessung hat eine andere SNR, Strahlbreite und Verweilzeit am Ziel. Deshalb berechnet das Radar die Unsicherheit jeder Messung und meldet sie an den Tracker.
Schauen wir uns die Wahrscheinlichkeitsdichtefunktion (PDF; Probability Density Function) der Gewichtsmessungen an.
Das folgende Diagramm zeigt zehn Messungen des Gewichts des Goldbarrens.
- Die blauen Kreise stellen die Messungen dar.
- Die wahren Werte werden durch die rote gestrichelte Linie dargestellt.
- Die grüne Linie zeigt die Wahrscheinlichkeitsdichtefunktion der Messung.
- Der grün hervorgehobene Bereich ist die Standardabweichung ( \( \sigma \) ) der Messung, d. h. mit einer Wahrscheinlichkeit von 68,26% liegt der Messwert in diesem Bereich.
Wie man sieht, liegen 7 von 10 Messungen innerhalb der Grenzen von \( 1 \sigma \).
Zustandsvorhersage
In unserem ersten Beispiel, der Gewichtsmessung des Goldbarrens, ist das dynamische Modell konstant:
Im zweiten Beispiel (eindimensionaler Radarfall) haben wir den aktuellen Zustand (Zielposition und -geschwindigkeit) mithilfe von Bewegungsgleichungen auf den nächsten Zustand extrapoliert:
d. h. die vorhergesagte Position ist gleich der aktuell geschätzten Position plus der aktuell geschätzten Geschwindigkeit multipliziert mit der Zeit. Die vorhergesagte Geschwindigkeit entspricht der aktuellen Geschwindigkeitsschätzung (unter Annahme eines Modells mit konstanter Geschwindigkeit).
Die Gleichung des dynamischen Modells hängt vom System ab.
Da der Kalman-Filter die Schätzung als Zufallsvariable behandelt, müssen wir auch die Schätzvarianz ( \( p_{n,n} \) ) in den nächsten Zustand extrapolieren.
In unserem ersten Beispiel (Gewichtsmessung eines Goldbarrens) ist das dynamische Modell des Systems konstant. Daher lautet die Extrapolation der Schätzunsicherheit:
Dabei gilt:
\( p \) ist die Schätzvarianz des Gewichts des Goldbarrens.
Im zweiten Beispiel lautet die Extrapolation der Schätzunsicherheit:
| \( p^{x} \) | ist die Varianz der Positionsschätzung |
| \( p^{v} \) | ist die Varianz der Geschwindigkeitsschätzung |
d. h. die vorhergesagte Varianz der Positionsschätzung ist gleich der aktuellen Varianz der Positionsschätzung plus der aktuellen Varianz der Geschwindigkeitsschätzung multipliziert mit der Zeit im Quadrat. Die vorhergesagte Varianz der Geschwindigkeitsschätzung entspricht der aktuellen Varianz der Geschwindigkeitsschätzung (unter Annahme eines Modells mit konstanter Geschwindigkeit).
Beachten Sie: Für jede normalverteilte Zufallsvariable \( x \) mit Varianz \( \sigma^{2} \) ist \( kx \) ebenfalls normalverteilt – mit Varianz \( k^{2}\sigma^{2} \). Deshalb wird der Zeitterm in der Gleichung zur Unsicherheitsextrapolation quadriert. Eine ausführliche Erklärung finden Sie im Anhang zur Herleitung des Erwartungswerts der Varianz.
Die Gleichung zur Extrapolation der Schätzunsicherheit heißt Gleichung zur Kovarianzextrapolation (Covariance Extrapolation Equation) und ist die dritte Kalman-Filter-Gleichung. Warum „Kovarianz“? Das sehen wir in den Kapiteln zum mehrdimensionalen Kalman-Filter.
Zustandsaktualisierung
Um den aktuellen Zustand des Systems zu schätzen, kombinieren wir zwei Zufallsvariablen:
- Die a-priori-Zustandsschätzung (prior state estimate; die aus dem vorherigen Zustand vorhergesagte aktuelle Schätzung)
- Die Messung
Der Kalman-Filter ist ein optimaler Filter (optimal filter). Er kombiniert die a-priori-Zustandsschätzung mit der Messung so, dass die Unsicherheit der aktuellen Zustandsschätzung minimiert wird.
Die aktuelle Zustandsschätzung ist ein gewichteter Mittelwert aus Messung und a-priori-Zustandsschätzung:
\[ \hat{x}_{n,n} = w_{1}z_{n} + w_{2}\hat{x}_{n,n-1} \] \[ w_{1} + w_{2} = 1 \]
Dabei sind \( w_{1} \) und \( w_{2} \) die Gewichte der Messung bzw. der a-priori-Zustandsschätzung.
Wir können \( \hat{x}_{n,n} \) wie folgt schreiben:
\[ \hat{x}_{n,n} = w_{1}z_{n} + (1 - w_{1})\hat{x}_{n,n-1} \]
Der Zusammenhang zwischen den Varianzen lautet:
\[ p_{n,n} = w_{1}^{2}r_{n} + (1 - w_{1})^{2}p_{n,n-1} \]
Dabei gilt:
\( p_{n,n} \) ist die Varianz der optimal kombinierten Schätzung \( \hat{x}_{n,n} \)
\( p_{n,n-1} \) ist die Varianz der a-priori-Schätzung \( \hat{x}_{n,n-1} \)
\( r_{n} \) ist die Varianz der Messung \( z_{n} \)
Zur Erinnerung: Für jede normalverteilte Zufallsvariable \( x \) mit Varianz \( \sigma^{2} \) ist \( kx \) normalverteilt mit Varianz \( k^{2}\sigma^{2} \). Eine ausführliche Erklärung finden Sie im Anhang zur Herleitung des Erwartungswerts der Varianz.
Da wir eine optimale Schätzung suchen, wollen wir \( p_{n,n} \) minimieren.
Um \( w_{1} \) zu finden, das \( p_{n,n} \) minimiert, leiten wir \( p_{n,n} \) nach \( w_{1} \) ab und setzen das Ergebnis gleich null.
\[ \frac{dp_{n,n}}{dw_{1}} = 2w_{1}r_{n} - 2(1 - w_{1})p_{n,n-1} = 0 \]
Daraus folgt:
\[ w_{1}r_{n} = p_{n,n-1} - w_{1}p_{n,n-1} \]
\[ w_{1}p_{n,n-1} + w_{1}r_{n} = p_{n,n-1} \]
\[ w_{1} = \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} \]
Setzen wir das Ergebnis in die aktuelle Zustandsschätzung \( \hat{x}_{n,n} \) ein:
\[ \hat{x}_{n,n} = w_{1}z_{n} + (1 - w_{1})\hat{x}_{n,n-1} = w_{1}z_{n} + \hat{x}_{n,n-1} - w_{1}\hat{x}_{n,n-1} = \hat{x}_{n,n-1} + w_{1}\left( z_{n} - \hat{x}_{n,n-1} \right) \]
Wir haben eine Gleichung hergeleitet, die der Zustandsaktualisierungsgleichung des \( \alpha -\beta -(\gamma) \)-Filters ähnelt. Das Gewicht der Innovation (innovation) heißt Kalman-Gain (Kalman Gain) und wird mit \( K_{n} \) bezeichnet.
Die Kalman-Gain-Gleichung (Kalman Gain equation) ist die vierte Kalman-Filter-Gleichung. In einer Dimension lautet sie:
| \( p_{n,n-1} \) | ist die extrapolierte Schätzvarianz |
| \( r_{n} \) | ist die Messvarianz |
Der Kalman-Gain ist eine Zahl zwischen null und eins:
\[ 0 \leq K_{n} \leq 1 \]
Schließlich müssen wir die Varianz der aktuellen Zustandsschätzung bestimmen. Wir haben gesehen, dass der Zusammenhang zwischen den Varianzen lautet:
\[ p_{n,n} = w^{2}_{1}r_{n} + \left( 1 - w_{1} \right)^{2}p_{n,n-1} \]
Das Gewicht \( w_{1} \) ist ein Kalman-Gain:
\[ p_{n,n} = K^{2}_{n}r_{n} + \left( 1 - K_{n} \right)^{2}p_{n,n-1} \]
Bestimmen wir den Term \( \left( 1 - K_{n} \right) \):
\[ \left( 1 - K_{n} \right) = \left( 1 - \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} \right) = \left( \frac{p_{n,n-1} + r_{n} - p_{n,n-1}}{p_{n,n-1} + r_{n}} \right) = \left( \frac{r_{n}}{p_{n,n-1} + r_{n}} \right) \]
| Hinweise | |
|---|---|
| \( p_{n,n} = \left( \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} \right)^{2}r_{n} + \left( \frac{r_{n}}{p_{n,n-1} + r_{n}} \right)^{2}p_{n,n-1} \) | Einsetzen von \( K_{n} \) und \( \left( 1 - K_{n} \right) \) |
| \( p_{n,n} = \frac{p_{n,n-1}^{2}r_{n}}{\left(p_{n,n-1} + r_{n}\right)^{2}} + \frac{r_{n}^{2}p_{n,n-1}}{\left(p_{n,n-1} + r_{n}\right)^{2}} \) | Ausmultiplizieren |
| \( p_{n,n} = \frac{p_{n,n-1}r_{n}}{p_{n,n-1} + r_{n}} \left( \frac{p_{n,n-1}}{p_{n,n-1} + r_{n}} + \frac{r_{n}}{p_{n,n-1} + r_{n}} \right) \) | |
| \( p_{n,n} = \left( 1 - K_{n} \right)p_{n,n-1}\left( K_{n} + \left( 1 - K_{n} \right) \right) \) | Einsetzen von \( K_{n} \) und \( \left( 1 - K_{n} \right) \) |
| \( p_{n,n} = \left( 1 - K_{n} \right)p_{n,n-1} \) |
Diese Gleichung aktualisiert die Schätzvarianz des aktuellen Zustands. Sie heißt Kovarianzaktualisierungsgleichung (Covariance Update Equation).
Aus der Gleichung ist klar, dass die Schätzunsicherheit mit jeder Filteriteration stetig abnimmt, da \( \left( 1-K_{n} \right) \leq 1 \). Wenn die Messunsicherheit hoch ist, ist der Kalman-Gain klein. Dann ist die Konvergenz der Schätzunsicherheit langsam. Ist die Messunsicherheit hingegen niedrig, ist der Kalman-Gain groß – und die Schätzunsicherheit konvergiert schnell gegen null.
Es liegt also an uns zu entscheiden, wie viele Messungen wir durchführen. Wenn wir die Höhe eines Gebäudes messen und uns eine Präzision von 3 Zentimetern ( \( \sigma \) ) interessiert, sollten wir so lange messen, bis die Varianz der Zustandsschätzung ( \( \sigma ^{2} \) ) kleiner als \( 9~centimeters^{2} \) ist.
Alles zusammenführen
Dieser Abschnitt fasst alle Bausteine zu einem einzigen Algorithmus zusammen.
Die Eingaben des Filters sind:
- Initialisierung
- Anfangszustand des Systems ( \( \hat{x}_{0,0} \) )
- Anfangsvarianz des Zustands ( \( p_{0,0} \) )
- Messung
- Gemessener Systemzustand ( \( z_{n} \) )
- Messvarianz ( \( r_{n} \) )
Die Initialisierung (initialization) wird nur einmal durchgeführt und liefert zwei Parameter:
Die Initialisierungsparameter können von einem anderen System, einem anderen Prozess (z. B. einem Suchprozess im Radar) oder durch eine fundierte Schätzung auf Basis von Erfahrung oder theoretischem Wissen geliefert werden. Selbst wenn die Initialisierungsparameter nicht präzise sind, kann der Kalman-Filter nahe an den wahren Wert konvergieren.
Die Messung (measurement) wird in jedem Filterzyklus durchgeführt und liefert zwei Parameter:
Die Ausgaben des Filters sind:
- Systemzustandsschätzung ( \( \hat{x}_{n,n} \) )
- Schätzvarianz ( \( p_{n,n} \) )
Die folgende Tabelle fasst die fünf Kalman-Filter-Gleichungen zusammen.
| Gleichung | Name der Gleichung | Alternative Bezeichnungen in der Literatur | |
|---|---|---|---|
| Zustandsaktualisierung | \( \hat{x}_{n,n}=~ \hat{x}_{n,n-1}+ K_{n} \left( z_{n}- \hat{x}_{n,n-1} \right) \) | Zustandsaktualisierung | Filtergleichung |
| \( p_{n,n}=~ \left( 1-K_{n} \right) p_{n,n-1} \) | Kovarianzaktualisierung | Korrekturgleichung | |
| \( K_{n}= \frac{p_{n,n-1}}{p_{n,n-1}+r_{n}} \) | Kalman-Gain | Gewichtungsgleichung | |
| Zustandsextrapolation |
\( \hat{x}_{n+1,n}= \hat{x}_{n,n} \) (Für konstante Dynamik) \( \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \) \( \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \) (Für konstante Geschwindigkeitsdynamik) |
Zustandsextrapolation |
Prädiktorgleichung Übergangsgleichung Vorhersagegleichung Dynamisches Modell Zustandsraummodell |
|
\( p_{n+1,n}= p_{n,n} \) (Für konstante Dynamik) \( p_{n+1,n}^{x}= p_{n,n}^{x} + \Delta t^{2} \cdot p_{n,n}^{v} \) \( p_{n+1,n}^{v}= p_{n,n}^{v} \) (Für konstante Geschwindigkeitsdynamik) |
Kovarianzextrapolation | Prädiktor-Kovarianzgleichung |
Die folgende Grafik beschreibt das Blockdiagramm des Kalman-Filters im Detail.
- Schritt 0: Initialisierung
- Anfangszustand des Systems ( \( \hat{x}_{0,0} \) )
- Anfangsvarianz des Zustands ( \( p_{0,0} \) )
- Schritt 1: Messung
- Gemessener Systemzustand ( \( z_{n} \) )
- Messvarianz ( \( r_{n} \) )
- Schritt 2: Zustandsaktualisierung
- Messwert ( \( z_{n} \) )
- Eine Messvarianz ( \( r_{n} \) )
- Eine a-priori (vorhergesagte) Systemzustandsschätzung ( \( \hat{x}_{n,n-1} \) )
- Eine a-priori (vorhergesagte) Schätzvarianz ( \( p_{n,n-1} \) )
- Aktuelle Systemzustandsschätzung ( \( \hat{x}_{n,n} \) )
- Aktuelle Schätzvarianz ( \( p_{n,n} \) )
- Schritt 3: Vorhersage
Wie oben erwähnt, wird die Initialisierung nur einmal durchgeführt und liefert zwei Parameter:
Auf die Initialisierung folgt die Vorhersage.
Der Messvorgang liefert zwei Parameter:
Die Zustandsaktualisierung ist für die Zustandsschätzung des aktuellen Systemzustands verantwortlich.
Die Eingaben der Zustandsaktualisierung sind:
Auf Basis dieser Eingaben berechnet die Zustandsaktualisierung den Kalman-Gain und liefert zwei Ausgaben:
Diese Größen sind die Ausgaben des Kalman-Filters.
Die Vorhersage extrapoliert die aktuelle Systemzustandsschätzung und ihre Varianz auf den nächsten Systemzustand – basierend auf dem dynamischen Modell des Systems.
In der ersten Filteriteration wird die Initialisierung als a-priori-Zustandsschätzung und -Varianz behandelt.
In den folgenden Iterationen werden die Vorhersageausgaben als a-priori (vorhergesagte) Zustandsschätzung und -Varianz verwendet.
Intuition zum Kalman-Gain
Schreiben wir die Zustandsaktualisierungsgleichung um:
\[ \hat{x}_{n,n} = \hat{x}_{n,n-1} + K_{n}\left( z_{n} - \hat{x}_{n,n-1} \right) = \left( 1 - K_{n} \right)\hat{x}_{n,n-1} + K_{n}z_{n} \]
Wie man sieht, ist der Kalman-Gain ( \( K_{n} \) ) das Gewicht der Messung, und der Term \( \left( 1 - K_{n} \right) \) ist das Gewicht der aktuellen Zustandsschätzung.
Der Kalman-Gain ist nahe bei null, wenn die Messunsicherheit hoch und die Schätzunsicherheit niedrig ist. Dann geben wir der Schätzung ein hohes Gewicht und der Messung ein geringes Gewicht.
Umgekehrt ist der Kalman-Gain nahe bei eins, wenn die Messunsicherheit niedrig und die Schätzunsicherheit hoch ist. Dann geben wir der Schätzung ein geringes Gewicht und der Messung ein hohes Gewicht.
Wenn die Messunsicherheit gleich der Schätzunsicherheit ist, dann ist der Kalman-Gain gleich 0,5.
Der Kalman-Gain definiert das Gewicht der Messung und das Gewicht der a-priori-Schätzung bei der Bildung einer neuen Schätzung. Er sagt uns, wie stark die Messung die Schätzung verändert.
Hoher Kalman-Gain
Eine niedrige Messunsicherheit im Verhältnis zur Schätzunsicherheit führt zu einem hohen Kalman-Gain (nahe 1). Die neue Schätzung liegt dann nahe an der Messung. Die folgende Grafik illustriert den Einfluss eines hohen Kalman-Gains auf die Schätzung in einer Flugzeug-Tracking-Anwendung.
Niedriger Kalman-Gain
Eine hohe Messunsicherheit im Verhältnis zur Schätzunsicherheit führt zu einem niedrigen Kalman-Gain (nahe 0). Die neue Schätzung liegt dann nahe an der a-priori-Schätzung. Die folgende Grafik illustriert den Einfluss eines niedrigen Kalman-Gains auf die Schätzung in einer Flugzeug-Tracking-Anwendung.
Jetzt verstehen wir den Kalman-Filter-Algorithmus und sind bereit für das erste numerische Beispiel.
Beispiel 5 – Schätzung der Höhe eines Gebäudes
Angenommen, wir möchten die Höhe eines Gebäudes mit einem ungenauen Höhenmesser (altimeter) schätzen.
Wir wissen, dass sich die Gebäudehöhe im Zeitverlauf nicht ändert – zumindest nicht während des kurzen Messvorgangs.
Das numerische Beispiel
- Die wahre Gebäudehöhe beträgt 50 Meter.
- Der Messfehler (Standardabweichung) des Höhenmessers beträgt 5 Meter.
- Die zehn Messungen sind: 49.03m, 48.44m, 55.21m, 49.98m, 50.6m, 52.61m, 45.87m, 42.64m, 48.26m, 55.84m.
Iteration 0
Initialisierung
Man kann die Höhe eines Gebäudes grob schätzen, indem man es einfach betrachtet.
Für die Initialisierung schätzen wir die Gebäudehöhe zu:
\[ \hat{x}_{0,0}=60m \]
Nun initialisieren wir die Schätzvarianz. Ein menschlicher Schätzfehler (Standardabweichung) liegt bei etwa 15 Metern: \( \sigma = 15m \). Folglich ist die Varianz 225: \( \sigma ^{2}=225m^{2} \).
\[ p_{0,0} = 225m^{2} \]
Vorhersage
Als Nächstes sagen wir den nächsten Zustand auf Basis der Initialisierungswerte voraus.
Da das dynamische Modell unseres Systems konstant ist, d. h. das Gebäude seine Höhe nicht ändert:
\[ \hat{x}_{1,0}=\hat{x}_{0,0}= 60m \]
Auch die extrapolierte Schätzvarianz ändert sich nicht:
\[ p_{1,0}= p_{0,0}=225m^{2} \]
Erste Iteration
Schritt 1 – Messen
Die erste Messung ist: \( z_{1}=49.03m \).
Da die Standardabweichung ( \( \sigma \) ) des Messfehlers des Höhenmessers 5m beträgt, ist die Varianz ( \( \sigma ^{2} \) ) gleich \( 25m^{2} \). Damit ist die Messvarianz: \( r_{1}=25m^{2} \).
Schritt 2 – Update
Berechnung des Kalman-Gains:
\[ K_{1}= \frac{p_{1,0}}{p_{1,0}+r_{1}}= \frac{225}{225+25}=0.9 \]
Schätzung des aktuellen Zustands:
\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ K_{1} \left( z_{1}- \hat{x}_{1,0} \right) =60+0.9 \left( 49.03-60 \right) =50.13m \]
Aktualisierung der aktuellen Schätzvarianz:
\[ p_{1,1}=~ \left( 1-K_{1} \right) p_{1,0}= \left( 1-0.9 \right) 225=22.5m^{2} \]
Schritt 3 – Vorhersage
Da das dynamische Modell unseres Systems konstant ist, d. h. das Gebäude seine Höhe nicht ändert:
\[ \hat{x}_{2,1}=\hat{x}_{1,1}= 50.13m \]
Auch die extrapolierte Schätzvarianz ändert sich nicht:
\[ p_{2,1}= p_{1,1}=22.5m^{2} \]
Zweite Iteration
Nach einer Zeiteinheit wird die vorhergesagte Schätzung aus der vorherigen Iteration zur a-priori-Schätzung in der aktuellen Iteration:
\[ \hat{x}_{2,1}=50.13m \]
Die extrapolierte Schätzvarianz wird zur a-priori-Schätzvarianz:
\[ p_{2,1}= 22.5m^{2} \]
Schritt 1 – Messen
Die zweite Messung ist: \( z_{2} = 48.44m \)
Die Messvarianz ist: \( r_{2}=25m^{2} \)
Schritt 2 – Update
Berechnung des Kalman-Gains:
\[ K_{2}= \frac{p_{2,1}}{p_{2,1}+r_{2}}= \frac{22.5}{22.5+25}=0.47 \]
Schätzung des aktuellen Zustands:
\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ K_{2} \left( z_{2}- \hat{x}_{2,1} \right) =50.13+0.47 \left( 48.44-50.13 \right) =49.33m \]
Aktualisierung der aktuellen Schätzvarianz:
\[ p_{2,2}=~ \left( 1-K_{2} \right) p_{2,1}= \left( 1-0.47 \right) 22.5=11.84m^{2} \]
Schritt 3 – Vorhersage
Da das dynamische Modell unseres Systems konstant ist, d. h. das Gebäude seine Höhe nicht ändert:
\[ \hat{x}_{3,2}=\hat{x}_{2,2}= 49.33m \]
Auch die extrapolierte Schätzvarianz ändert sich nicht:
\[ p_{3,2}= p_{2,2}=11.84m^{2} \]
Iterationen 3–10
Die Berechnungen für die folgenden Iterationen sind in der folgenden Tabelle zusammengefasst:
| \( n \) | \( z_{n} \) | Aktuelle Schätzwerte ( \( K_{n} \) , \( \hat{x}_{n,n} \) , \( p_{n,n} \) ) | Vorhersage ( \( \hat{x}_{n+1,n} \) , \( p_{n+1,n} \) ) |
|---|---|---|---|
| 3 | \( 55.21m \) | \[ K_{3}= \frac{11.84}{11.84+25}=0.32 \] \[ \hat{x}_{3,3}=~ 49.33+0.32 \left( 55.21 -49.33 \right) =51.22m \] \[ p_{3,3}= \left( 1-0.32 \right) 11.84=8.04m^{2} \] | \[ \hat{x}_{4,3}= \hat{x}_{3,3}=51.22m \] \[ p_{4,3}= p_{3,3}=8.04m^{2} \] |
| 4 | \( 49.98m \) | \[ K_{4}= \frac{8.04}{8.04+25}=0.24 \] \[ \hat{x}_{4,4}= 51.22+0.24 \left( 49.98 -51.22 \right) =50.92m \] \[ p_{4,4}= \left( 1-0.24 \right) 8.04=6.08m^{2} \] | \[ \hat{x}_{5,4}= \hat{x}_{4,4}=50.92m \] \[ p_{5,4}= p_{4,4}=6.08m^{2} \] |
| 5 | \( 50.6m \) | \[ K_{5}= \frac{6.08}{6.08+25}=0.2 \] \[ \hat{x}_{5,5}= 50.92+0.2 \left( 50.6 -50.92 \right) =50.855m \] \[ p_{5,5}= \left( 1-0.2 \right) 6.08=4.89m^{2} \] | \[ \hat{x}_{6,5}= \hat{x}_{5,5}=50.855m \] \[ p_{6,5}= p_{5,5}=4.89m^{2} \] |
| 6 | \( 52.61m \) | \[ K_{6}= \frac{4.89}{4.89+25}=0.16 \] \[ \hat{x}_{6,6}=~ 50.855+0.16 \left( 52.61 -50.855 \right) =51.14m \] \[ p_{6,6}= \left( 1-0.16 \right) 4.89=4.09m^{2} \] | \[ \hat{x}_{7,6}= \hat{x}_{6,6}=51.14m \] \[ p_{7,6}= p_{6,6}=4.09m^{2} \] |
| 7 | \( 45.87m \) | \[ K_{7}= \frac{4.09}{4.09+25}=0.14 \] \[ \hat{x}_{7,7}= 51.14+0.14 \left( 45.87 -51.14 \right) =50.4m \] \[ p_{7,7}= \left( 1-0.14 \right) 4.09=3.52m^{2} \] | \[ \hat{x}_{8,7}= \hat{x}_{7,7}=50.4m \] \[ p_{8,7}= p_{7,7}=3.52m^{2} \] |
| 8 | \( 42.64m \) | \[ K_{8}= \frac{3.52}{3.52+25}=0.12 \] \[ \hat{x}_{8,8}= 50.4+0.12 \left( 42.64 -50.4 \right) =49.44m \] \[ p_{8,8}= \left( 1-0.12 \right) 3.52=3.08m^{2} \] | \[ \hat{x}_{9,8}= \hat{x}_{8,8}=49.44m \] \[ p_{9,8}= p_{8,8}=3.08m^{2} \] |
| 9 | \( 48.26m \) | \[ K_{9}= \frac{3.08}{3.08+25}=0.11 \] \[ \hat{x}_{9,9}= 49.44 + 0.11 \left( 48.26 - 49.44 \right) = 49.31m \] \[ p_{9,9}= \left( 1-0.11 \right) 3.08=2.74m^{2} \] | \[ \hat{x}_{10,9}= \hat{x}_{9,9} = 49.31m \] \[ p_{10,9}= p_{9,9}=2.74m^{2} \] |
| 10 | \( 55.84m \) | \[ K_{10}= \frac{2.74}{2.74+25}=0.1 \] \[ \hat{x}_{10,10}= 49.31 + 0.1 \left( 55.84 - 49.31 \right) = 49.96m \] \[ p_{10,10}= \left( 1-0.1 \right) 2.74=2.47m^{2} \] | \[ \hat{x}_{11,10}= \hat{x}_{10,10} = 49.96m \] \[ p_{11,10}= p_{10,10} = 2.47m^{2} \] |
Analyse der Ergebnisse
Zunächst möchten wir sicherstellen, dass der Kalman-Filter konvergiert. Der Kalman-Gain sollte sich schrittweise verringern, bis er einen stationären Zustand erreicht. Wenn der Kalman-Gain klein ist, ist auch das Gewicht der verrauschten Messungen klein. Die folgende Grafik zeigt den Kalman-Gain für die ersten hundert Iterationen des Kalman-Filters.
Wir sehen eine deutliche Abnahme des Kalman-Gains während der ersten zehn Iterationen. Nach ungefähr fünfzig Iterationen geht der Kalman-Gain in einen stationären Zustand über.
Wir möchten außerdem die Genauigkeit (accuracy) untersuchen. Genauigkeit beschreibt, wie nah die Messung am wahren Wert liegt. Die folgende Grafik vergleicht den wahren Wert, die Messwerte und die Schätzungen für die ersten 50 Iterationen.
Ein Schätzfehler ist die Differenz zwischen den wahren Werten (grüne Linie) und den KF-Schätzungen (rote Linie). Man sieht, dass die Schätzfehler unseres KF im Konvergenzbereich des Filters abnehmen.
Man kann Genauigkeitskriterien basierend auf den Anforderungen der jeweiligen Anwendung definieren. Typische Genauigkeitskriterien sind:
- Maximalfehler
- Mittlerer Fehler
- RMSE (Root Mean Square Error; Wurzel des mittleren quadratischen Fehlers)
Ein weiterer wichtiger Parameter ist die Schätzunsicherheit (estimation uncertainty). Wir möchten, dass die KF-Schätzungen präzise (precise) sind; daher interessiert uns eine geringe Schätzunsicherheit.
Angenommen, es gibt bei einer Anwendung zur Gebäudehöhenmessung die Anforderung eines 95%-Konfidenzniveaus (confidence). Die folgende Grafik zeigt die KF-Schätzungen und die wahren Werte mit 95%-Konfidenzintervallen (confidence intervals).
Die Konfidenzintervalle werden auf Basis der Unsicherheiten der Schätzungen berechnet. Die Anleitung zur Berechnung von Konfidenzintervallen finden Sie hier.
In der obigen Grafik sind die Konfidenzintervalle zu den Schätzungen (rote Linie) hinzugefügt. 95% der grünen Stichproben sollten innerhalb des 95%-Konfidenzbereichs liegen.
Man sieht, dass die Unsicherheit zu hoch ist. Verringern wir den Parameter der Messunsicherheit.
Die folgende Grafik zeigt die KF-Ausgabe für einen niedrigen Messunsicherheitsparameter.
Obwohl wir die Unsicherheit der Schätzungen verringert haben, liegen viele grüne Stichproben außerhalb des 95%-Konfidenzbereichs. Der Kalman-Filter ist überkonfident und zu optimistisch hinsichtlich seiner Genauigkeit.
Finden wir nun die Messunsicherheit, die die gewünschte Schätzunsicherheit ergibt.
Die obige Grafik zeigt, dass 2 von 50 Stichproben den 95%-Konfidenzbereich leicht überschreiten. Diese Leistung erfüllt unsere Anforderungen.
Zusammenfassung des Beispiels
In diesem Beispiel haben wir die Gebäudehöhe mit dem eindimensionalen Kalman-Filter gemessen. Im Gegensatz zum \( \alpha -\beta -(\gamma) \)-Filter ist der Kalman-Gain dynamisch und hängt von der Präzision (precision) des Messgeräts ab.
Der initiale Wert, den der Kalman-Filter verwendet, ist nicht präzise. Daher ist das Messgewicht in der Zustandsaktualisierungsgleichung hoch, und die Schätzunsicherheit ist hoch.
Mit jeder Iteration wird das Messgewicht kleiner; daher wird auch die Schätzunsicherheit kleiner.
Die Ausgabe des Kalman-Filters umfasst die Schätzung und die Schätzunsicherheit.