Extended Kalman Filter (EKF)

Der erweiterte Kalman-Filter (EKF) ist ein rekursiver Algorithmus zur Schätzung des Zustands eines Systems mit nichtlinearer Bewegung oder nichtlinearen Messungen, etwa der Ausrichtung einer Drohne oder der Position eines Ziels, dessen Entfernung und Peilung ein Radar erfasst. Der standardmäßige Kalman-Filter (KF) ist nur für lineare Modelle mit gaußschem Rauschen exakt. Der EKF verarbeitet nichtlineare Modelle, indem er sie in jedem Schritt um die aktuelle Schätzung mithilfe von Jakobi-Matrizen der ersten Ableitungen linearisiert und anschließend die bekannten Prädiktions- und Aktualisierungsgleichungen anwendet. Gerald Smith, Stanley Schmidt und Leonard McGee von der NASA Ames entwickelten ihn Anfang der 1960er-Jahre für die Bordnavigation eines Raumfahrzeugs für einen Flug um den Mond. Diese Arbeit floss in den Apollo-Navigationscomputer ein. Der EKF ist nach wie vor der De-facto-Standard für nichtlineare Zustandsschätzungen in Navigationssystemen, GPS-Empfängern, Drohnen und Robotern.

So funktioniert der EKF#

Wie der Kalman-Filter führt der EKF eine Zustandsschätzung x und eine Kovarianz P, die die Unsicherheit beschreibt. Das Bewegungsmodell f und das Messmodell h können jedoch beliebige differenzierbare Funktionen sein. Jeder Schritt besteht aus zwei Phasen:

1. Prädiktion: Die Schätzung wird durch das nichtlineare Bewegungsmodell x = f(x) geleitet. Der Filter wertet die Jakobi-Matrix F = ∂f/∂x an der Schätzung aus und vergrößert die Unsicherheit mithilfe des Prozessrauschens Q: P = F P Fᵀ + Q.
2. Aktualisierung: Eine Messung z trifft ein. Der Filter wertet die Mess-Jakobi-Matrix H = ∂h/∂x am vorhergesagten Zustand aus, berechnet den Residualwert y = z − h(x) und den Verstärkungsfaktor K = P Hᵀ (H P Hᵀ + R)⁻¹, wobei R das Messrauschen bezeichnet, und korrigiert die Schätzung mithilfe von x = x + K y und P = (I − K H) P.

Der Zustand und die vorhergesagte Messung durchlaufen die tatsächlichen nichtlinearen Funktionen. Nur für Kovarianz und Verstärkungsfaktor werden die linearisierten Jakobi-Matrizen verwendet. Wenn f und h linear sind, entsprechen die Jakobi-Matrizen den Modellmatrizen, und der EKF ist exakt der Kalman-Filter. Die Näherung ist gut, wenn die Modelle über die Streuung der Unsicherheit hinweg annähernd linear sind. Sie wird schlechter, wenn die Modelle innerhalb dieses Bereichs stark gekrümmt sind.

Ein Radar, das Entfernung und Peilung misst, ist ein klassisches Beispiel: Die geradlinige Bewegung eines Ziels in x- und y-Richtung ist in diesen Koordinaten linear, aber Entfernung √(x² + y²) und Peilung atan2(y, x) sind es nicht. Dieser EKF verfolgt ein solches Ziel und schätzt dessen Geschwindigkeit, die das Radar nicht misst:

import numpy as np

rng = np.random.default_rng(0)
F = np.eye(4) + np.eye(4, k=2)  # constant velocity, state = [x, y, vx, vy], 1 s per step
Q, R = 1e-4 * np.eye(4), np.diag([1.0**2, 0.02**2])  # process noise; radar noise of 1 m and 0.02 rad
s = np.array([20.0, 50.0, 1.0, -0.5])  # true state
x, P = np.array([20.0, 50.0, 0.0, 0.0]), np.diag([1.0, 1.0, 4.0, 4.0])  # estimate starts with unknown velocity

raw_err, ekf_err = [], []
for _ in range(60):
    s = F @ s
    z = np.array([np.hypot(s[0], s[1]), np.arctan2(s[1], s[0])]) + rng.normal(0, [1.0, 0.02])  # range, bearing
    x, P = F @ x, F @ P @ F.T + Q  # predict
    r = np.hypot(x[0], x[1])
    H = np.array([[x[0] / r, x[1] / r, 0, 0], [-x[1] / r**2, x[0] / r**2, 0, 0]])  # Jacobian of h(x)
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)
    x = x + K @ (z - np.array([r, np.arctan2(x[1], x[0])]))  # update with the nonlinear residual
    P = (np.eye(4) - K @ H) @ P
    raw_err.append(np.hypot(z[0] * np.cos(z[1]) - s[0], z[0] * np.sin(z[1]) - s[1]))
    ekf_err.append(np.hypot(x[0] - s[0], x[1] - s[1]))

print(f"Mean position error: raw radar {np.mean(raw_err):.2f} m, EKF {np.mean(ekf_err):.2f} m")
print(f"Estimated velocity: ({x[2]:.2f}, {x[3]:.2f}) m/s, true (1.00, -0.50)")

Wenn jede rohe Entfernungs- und Peilungsmessung in x- und y-Koordinaten umgerechnet wird, beträgt der mittlere Positionsfehler über 60 Schritte 1,43 m. Die EKF-Schätzung weicht dagegen um 0,57 m ab, und der Filter ermittelt die Geschwindigkeit als (1,01, −0,50) m/s.

Einschränkungen und Varianten#

Der EKF ist kein optimaler Schätzer. Wenn die anfängliche Schätzung stark abweicht oder die Modelle stark nichtlinear sind, können Linearisierungsfehler den Filter zu großer Zuversicht verleiten und schließlich zu seiner Divergenz führen. Deshalb erhöhen Ingenieurinnen und Ingenieure häufig vorsichtshalber das Prozessrauschen. Die Jakobi-Matrizen müssen von Hand oder mithilfe automatischer Differenzierung hergeleitet werden; eine falsche Ableitung ist eine häufige Ursache unbemerkter Fehler. Residuen von Winkeln wie Peilung oder Kurs müssen außerdem auf den Bereich von −π bis π begrenzt werden. Mehrere Varianten gehen diese Probleme an: Der iterierte EKF linearisiert das Messmodell während der Aktualisierung erneut, und der Fehlerzustands-EKF schätzt eine kleine Korrektur um einen nominalen Zustand. Dieses Verfahren wird in der Inertialnavigation häufig verwendet, um die 3D-Ausrichtung mit Quaternionen zu verfolgen.

EKF im Vergleich zu anderen Filtern#

Die Wahl des Filters hängt davon ab, wie stark die Modelle von der Linearität abweichen und welche Form die Unsicherheit hat:

• Kalman-Filter: Optimal und kostengünstiger, wenn beide Modelle linear sind. Tracker für Begrenzungsboxen wie ByteTrack messen die Boxkoordinaten direkt. Für sie genügt daher ein linearer Filter.
• Unscented-Kalman-Filter (UKF): Leitet eine kleine, deterministische Menge von Sigma-Punkten durch die nichtlinearen Funktionen, statt sie zu linearisieren. Er benötigt keine Jakobi-Matrizen und ist bei stark gekrümmten Modellen meist genauer als der EKF.
• Partikelfilter: Stellt die Verteilung durch viele gewichtete Zufallsstichproben dar. Dadurch kann er multimodale Verteilungen verfolgen, etwa wenn ein Roboter nicht weiß, in welchem von zwei Fluren er sich befindet, allerdings mit deutlich höherem Rechenaufwand.
• Hidden-Markov-Modell (HMM): Wendet dieselbe rekursive Prädiktion und Aktualisierung auf eine endliche Menge diskreter Zustände an.

Anwendungen in der Praxis#

• Drohnen und Luftfahrzeuge: Der EKF2-Schätzer des PX4-Autopiloten führt einen EKF mit 24 Zuständen aus, der IMU-, GPS-, Magnetometer-, Barometer- und weitere Sensordaten kombiniert. Daraus schätzt er Ausrichtung, Geschwindigkeit, Position, Sensorabweichungen, das Erdmagnetfeld und den Wind.
• Robotik: Das ROS-Paket robot_localization stellt EKF- und UKF-Knoten bereit, die Raddrehzahlmessungen, IMUs und GPS kombinieren. EKF-SLAM ist der klassische Ansatz für die simultane Lokalisierung und Kartenerstellung. Die visuell-inertiale Odometrie auf EKF-Basis, etwa der Multi-State Constraint Kalman Filter (MSCKF), verfolgt die Kamerapose anhand von Bildmerkmalen und IMU-Messungen.
• Automobile Sensorfusion: Fahrerassistenzsysteme und Systeme für autonomes Fahren kombinieren Entfernungs- und Peilungsmessungen des Radars mit Erkennungen durch Kamera und Lidar, um Position und Geschwindigkeit umliegender Fahrzeuge zu verfolgen.
• Kamerabasierte Lokalisierung: Eine einzelne Kamera misst nur die Richtung zu einem Objekt. Verarbeitungsketten, die erkannte Objekte in einer Karte verorten, verwenden Erkennungen von Ultralytics YOLO26 als EKF-Messungen: Die Mitte jeder Box ist eine Pixelposition und entspricht einer nichtlinearen Projektion der 3D-Position des Objekts durch das Kameramodell. Der Filter kombiniert diese Messung mit der Radarentfernung oder den GPS- und IMU-Daten der Kameraplattform.