Kalman Filter – Optimale Zustandsschätzung
Vom State-Space-Modell über Predict-Update bis EKF, UKF und IMM – die vollständige Theorie der optimalen Schätzung für Radar-Tracking mit interaktiver Demonstration.
Warum brauchen wir den Kalman Filter?
Im Jahr 1960 veröffentlichte Rudolf E. Kálmán seinen wegweisenden Artikel „A New Approach to Linear Filtering and Prediction Problems“, der die optimale Zustandsschätzung für lineare dynamische Systeme mathematisch formulierte. Dieser Algorithmus – heute als Kalman Filter bekannt – wurde wenige Jahre später im Apollo-Navigationssystem der NASA eingesetzt und ist seitdem aus der modernen Signalverarbeitung nicht mehr wegzudenken.
Das Grundproblem:
Ein Radarsensor liefert Messungen von Position und Geschwindigkeit eines Ziels – aber diese Messungen sind verrauscht. Thermisches Rauschen, Clutter, Mehrwegeausbreitung und begrenzte Winkelauflösung führen dazu, dass jede einzelne Messung fehlerbehaftet ist. Gleichzeitig können wir ein physikalisches Bewegungsmodell formulieren (z.B. „das Ziel bewegt sich mit konstanter Geschwindigkeit“), das ebenfalls unsicher ist, da das Ziel manövrieren könnte.
Der Kalman Filter löst den fundamentalen Trade-off zwischen Messrauschen und Prozessunsicherheit: Er kombiniert die verrauschte Messung mit der Modellvorhersage und gewichtet beide Quellen entsprechend ihrer jeweiligen Unsicherheit. Das Ergebnis ist die im Sinne des minimalen mittleren quadratischen Fehlers (MMSE) optimale Schätzung des Systemzustands.
Anwendungen in der Radartechnik
• Zielverfolgung (Target Tracking) in 2D/3D
• Track-While-Scan (TWS) Systeme
• Multitarget Tracking mit GNN/JPDA
• Sensorintegration (Radar + Kamera + LIDAR)
• Eigenbewegungskompensation (IMU-gestützt)
Weitere Einsatzgebiete
• GPS-Navigation und INS/GPS-Fusion
• Robotik: SLAM, Lokalisierung
• Finanzmärkte: Zeitreihenfilterung
• Regelungstechnik: Zustandsregler
• Wettervorhersage und Klimamodelle
Zustandsraumdarstellung für Radar-Tracking
Die Grundlage des Kalman Filters ist die Beschreibung des Systems im Zustandsraum. Für ein zweidimensionales Radar-Tracking definieren wir den Zustandsvektor, der alle relevanten Größen des Ziels enthält.
Zustandsvektor (2D-Tracking, Constant Velocity):
x = [x, vx, y, vy]T
x, y = Position in kartesischen Koordinaten [m]
vx, vy = Geschwindigkeitskomponenten [m/s]
Zustandsgleichung (Prädiktionsmodell)
Die zeitliche Entwicklung des Zustands wird durch die Zustandsgleichung beschrieben. Für das Constant-Velocity-Modell (CV) lautet sie:
x(k+1) = F · x(k) + G · w(k)
F = Zustandsübergangsmatrix, G = Eingangsmatrix für Prozessrauschen,
w(k) ~ N(0, Q) = Prozessrauschen (modelliert Modell-Unsicherheiten und Manöver)
F-Matrix (Constant Velocity, Δt = Abtastintervall):
F = | 1 Δt 0 0 |
| 0 1 0 0 |
| 0 0 1 Δt |
| 0 0 0 1 |
Die obere 2×2-Blockdiagonale beschreibt die x-Dynamik, die untere die y-Dynamik. Position wird durch Position + Geschwindigkeit · Δt fortgeschrieben.
Q-Matrix (Prozessrausch-Kovarianz):
Q = q · | Δt³/3 Δt²/2 0 0 |
| Δt²/2 Δt 0 0 |
| 0 0 Δt³/3 Δt²/2 |
| 0 0 Δt²/2 Δt |
q bestimmt die Stärke der erwarteten Manöver. Hoher q-Wert = das Ziel könnte stark beschleunigen.
Messgleichung
Das Radar liefert eine verrauschte Messung z(k), die linear mit dem Zustand verknüpft ist:
z(k) = H · x(k) + v(k)
H = Messmatrix (bildet Zustand auf Messung ab)
v(k) ~ N(0, R) = Messrauschen (weiß, gauss-verteilt, unkorreliert)
H-Matrix (Position wird gemessen):
H = | 1 0 0 0 |
| 0 0 1 0 |
Das Radar misst nur Position (x, y), nicht Geschwindigkeit. Der Kalman Filter schätzt die Geschwindigkeit aus aufeinanderfolgenden Positionsmessungen.
R-Matrix (Messunsicherheit):
R = | σx² 0 |
| 0 σy² |
σx und σy repräsentieren die Standardabweichung der Positionsmessung in x- bzw. y-Richtung. Typisch für 77-GHz-Radar: 0.1–1.0 m.
Predict-Update Zyklus – Vollständige Herleitung
Der Kalman Filter arbeitet in einem rekursiven Zweischritt-Verfahren: Prädiktion (Vorhersage des nächsten Zustands basierend auf dem Modell) und Update (Korrektur der Vorhersage mit der neuen Messung). Dieser Zyklus wiederholt sich mit jedem Radar-Scan.
Schritt 1: Prädiktion (Time Update)
Zustandsvorhersage:
x̂(k|k−1) = F · x̂(k−1|k−1)
Der vorherige optimale Zustand x̂(k−1|k−1) wird mit der Zustandsübergangsmatrix F in die Zukunft propagiert. Physikalisch: „Wo erwarten wir das Ziel, wenn es sich wie bisher weiterbewegt?“
Kovarianzvorhersage:
P(k|k−1) = F · P(k−1|k−1) · FT + Q
Die Unsicherheit wächst durch die Prädiktion: Die vorherige Kovarianz wird propagiert (F·P·FT) und das Prozessrauschen Q wird addiert. Je länger wir vorhersagen ohne Messung, desto größer wird die Unsicherheits-Ellipse.
Schritt 2: Update (Measurement Update)
Innovation (Messfehler):
ỹ = z(k) − H · x̂(k|k−1)
Die Innovation ist die Differenz zwischen der tatsächlichen Messung z(k) und der vorhergesagten Messung. Sie enthält die „neue Information“, die noch nicht im Modell enthalten ist.
Innovationskovarianz:
S = H · P(k|k−1) · HT + R
S beschreibt die Unsicherheit der Innovation. Sie setzt sich zusammen aus der projizierten Zustandsunsicherheit (H·P·HT) und der Messunsicherheit (R). S wird auch für die Gating-Entscheidung verwendet: Liegt eine Messung innerhalb des durch S definierten Validierungsgates?
Kalman Gain:
K = P(k|k−1) · HT · S−1
Der Kalman Gain K ist das Herzstück des Algorithmus. Er bestimmt, wie stark die Vorhersage durch die neue Messung korrigiert wird. Wenn R klein ist (genaue Messung), ist K groß → Messung wird stark gewichtet. Wenn P klein ist (genaue Vorhersage), ist K klein → Modell wird vertraut.
Zustandsupdate:
x̂(k|k) = x̂(k|k−1) + K · ỹ
Der vorhergesagte Zustand wird um die gewichtete Innovation korrigiert. Dies ist die neue optimale Schätzung des Zustands zum Zeitpunkt k.
Kovarianzupdate:
P(k|k) = (I − K · H) · P(k|k−1)
Die Unsicherheit wird durch das Update immer kleiner (oder bleibt gleich). Die Messung bringt Information ins System, die die Unsicherheits-Ellipse schrumpfen lässt. Im stationären Zustand stellt sich ein Gleichgewicht zwischen Prädiktionswachstum und Update-Schrumpfung ein.
Pseudocode: Vollständiger Kalman Filter
x̂ = x0 // Anfangszustand
P = P0 // Anfangskovarianz (groß bei Unsicherheit)
for each timestep k:
// === PREDICT ===
x̂pred = F · x̂ // Zustandsvorhersage
Ppred = F · P · FT + Q // Kovarianzvorhersage
// === UPDATE ===
ỹ = z(k) − H · x̂pred // Innovation
S = H · Ppred · HT + R // Innovationskovarianz
K = Ppred · HT · inv(S) // Kalman Gain
x̂ = x̂pred + K · ỹ // Zustandsupdate
P = (I − K · H) · Ppred // Kovarianzupdate
output(x̂, P) // Geschätzter Zustand + Unsicherheit
EKF – Nichtlineare Messmodelle
Das klassische Kalman Filter setzt ein lineares Messmodell voraus (z = H·x + v). In der Praxis misst ein Radar jedoch in Polarkoordinaten: Entfernung (Range), Azimutwinkel und ggf. Elevation. Die Umrechnung von kartesischen Zustandsgrößen in Polarkoordinaten ist nichtlinear. Hier kommt das Extended Kalman Filter (EKF) zum Einsatz.
Nichtlineares Messmodell:
z = h(x) + v
mit der nichtlinearen Messfunktion:
h(x) = [ √(x² + y²) , arctan(y/x) ]T
Die erste Komponente ist die Range r = √(x² + y²), die zweite der Azimut θ = arctan(y/x).
Das EKF löst dieses Problem durch Linearisierung der Messfunktion h(x) an der aktuellen Schätzstelle x̂ mittels der Jacobi-Matrix:
Jacobi-Matrix für Radar-Messungen (2D, Range + Azimut):
r = sqrt(x̂² + ŷ²)
Hjac = | x̂/r 0 ŷ/r 0 |
| −ŷ/r² 0 x̂/r² 0 |
Die Jacobi-Matrix wird in jedem Zeitschritt an der aktuellen Zustandsschätzung x̂ neu berechnet. Im Update-Schritt ersetzt Hjac die lineare Messmatrix H.
Grenzen des EKF:
Die Linearisierung ist nur eine Taylor-Entwicklung 1. Ordnung. Bei stark nichtlinearen Funktionen oder großen Unsicherheiten (weite Kovarianz-Ellipse) kann die Linearisierung zu erheblichen Fehlern führen. In solchen Fällen divergiert das EKF möglicherweise. Abhilfe schaffen das Unscented Kalman Filter (UKF) oder Partikelfilter.
UKF – Sigmapoint-Transformation
Das Unscented Kalman Filter (UKF), entwickelt von Julier und Uhlmann (1997), verfolgt einen fundamental anderen Ansatz als das EKF: Anstatt die nichtlineare Funktion zu linearisieren, werden deterministische Abtastpunkte (Sigma Points) gewählt, durch die nichtlineare Funktion transformiert und anschließend Mittelwert und Kovarianz aus den transformierten Punkten rekonstruiert.
Kernidee:
„Es ist einfacher, eine Wahrscheinlichkeitsverteilung zu approximieren als eine beliebige nichtlineare Funktion.“ – Statt h(x) zu linearisieren, approximieren wir die Verteilung von h(x) durch eine endliche Menge deterministischer Punkte.
Algorithmus in drei Schritten
1. Sigma Points wählen
Für einen n-dimensionalen Zustand werden 2n+1 Sigma Points berechnet:
χ0 = x̂
χi = x̂ + (√((n+λ)P))i
χn+i = x̂ − (√((n+λ)P))i
λ ist ein Skalierungsparameter, der die Streuung der Sigma Points steuert.
2. Transformation
Jeder Sigma Point wird durch die nichtlineare Funktion propagiert:
γi = h(χi)
Keine Jacobi-Berechnung erforderlich! Die nichtlineare Funktion wird direkt ausgewertet.
3. Rekonstruktion
Mittelwert und Kovarianz werden aus den transformierten Punkten rekonstruiert:
ẑ = Σ wi · γi
S = Σ wi · (γi−ẑ)(γi−ẑ)T
Die Gewichte wi hängen von λ und n ab.
Vorteile des UKF gegenüber EKF:
• Keine Jacobi-Berechnung: Besonders vorteilhaft bei komplexen oder nicht analytisch differenzierbaren Funktionen.
• Höhere Genauigkeit: Die Unscented-Transformation erfasst nichtlineare Effekte bis zur 2. Ordnung (vs. 1. Ordnung beim EKF).
• Robuster: Konvergiert auch bei stark nichtlinearen Systemen, bei denen das EKF divergiert.
• Vergleichbarer Rechenaufwand: Nur O(n) zusätzliche Funktionsauswertungen, kein symbolisches Differenzieren nötig.
IMM – Adaptive Modellumschaltung
In der Realität bewegt sich ein Ziel nicht immer mit konstanter Geschwindigkeit. Eine Drohne kann plötzlich beschleunigen, eine Kurve fliegen oder stoppen. Ein einzelnes Bewegungsmodell kann diese Vielfalt nicht abbilden. Das Interacting Multiple Model (IMM) löst dieses Problem, indem es mehrere Kalman Filter parallel mit verschiedenen Bewegungsmodellen betreibt.
Typische Modelle im IMM:
CV – Constant Velocity: x(k+1) = x(k) + v·Δt. Für geradlinige Bewegung.
CA – Constant Acceleration: x(k+1) = x(k) + v·Δt + ½a·Δt². Für Beschleunigungsphasen.
CT – Constant Turn: Koordinierte Kurve mit konstanter Drehrate ω. Für Manöver.
IMM-Zyklus in drei Schritten
1. Mixing (Interaktion)
Die Zustände und Kovarianzen der einzelnen Modelle werden gemäß der Misch-Wahrscheinlichkeiten μij kombiniert. Dies ermöglicht einen fließenden Übergang zwischen Modellen.
μij = pij · μj / c̄i
2. Filterung
Jedes Modell führt seinen eigenen Kalman Filter Predict-Update-Zyklus durch, mit den gemischten Anfangszuständen aus Schritt 1. Die Innovation jedes Filters wird zur Berechnung der Modell-Likelihood verwendet.
3. Kombination
Die Modellwahrscheinlichkeiten μj werden bayesianisch aktualisiert basierend auf der Likelihood jedes Filters. Der Gesamt-Zustand wird als gewichtete Summe der Einzelzustände berechnet.
x̂ = Σ μj · x̂j
Modellübergangsmatrix (Markov-Kette):
// p_ij = P(Modell j zum Zeitpunkt k | Modell i zum Zeitpunkt k-1)
Π = | 0.95 0.03 0.02 | // CV → CV, CA, CT
| 0.03 0.95 0.02 | // CA → CV, CA, CT
| 0.03 0.02 0.95 | // CT → CV, CA, CT
Hohe Diagonalwerte (0.95) bedeuten, dass das Ziel wahrscheinlich im aktuellen Modus bleibt. Die Off-Diagonal-Elemente erlauben Modellwechsel mit kleiner Wahrscheinlichkeit pro Zeitschritt.
Kalman Tracking Demo
Beobachte in Echtzeit, wie der Kalman Filter verrauschte Radar-Messungen glättet und die wahre Trajektorie eines Ziels rekonstruiert. Ändere die Parameter Q (Prozessrauschen) und R (Messrauschen), um den Einfluss auf die Tracking-Performance zu verstehen.
Legende:
—— Wahre Trajektorie (verborgen)
• • • Verrauschte Radar-Messungen
—— Kalman-gefilterter Track
∘ Kovarianz-Ellipse (Unsicherheit)
Hoher Q-Wert: Filter vertraut Messungen mehr → Track folgt Rauschen.
Hoher R-Wert: Filter vertraut Modell mehr → glatterer Track.