Kalman Filter (KF)

Ein Kalman-Filter (KF) ist ein rekursiver Algorithmus, der anhand einer Reihe verrauschter Messungen den verborgenen Zustand eines sich verändernden Systems schätzt, etwa die Position und Geschwindigkeit eines Objekts. Bei jedem Zeitschritt sagt er den nächsten Zustand anhand eines Modells der Systembewegung voraus und korrigiert diese Vorhersage dann mit der neuen Messung. Dabei gewichtet er beide danach, wie unsicher sie sind. Rudolf E. Kálmán veröffentlichte das Verfahren 1960. Bei linearen Systemen mit gaußschem Rauschen liefert es die Schätzung mit dem kleinsten mittleren quadratischen Fehler. Der Filter kommt in GPS-Empfängern, der Flugzeug- und Raumfahrzeugnavigation, der Roboterlokalisierung und nahezu jedem Mehrziel-Tracker in der Computer Vision zum Einsatz. Dort glättet er springende Erkennungen und sagt voraus, wo jedes Objekt als Nächstes auftaucht.

So funktioniert der Kalman-Filter#

Der Filter speichert zwei Größen: eine Zustandsschätzung x, zum Beispiel Position und Geschwindigkeit, sowie eine Kovarianzmatrix P, die die Unsicherheit dieser Schätzung beschreibt. Jeder Zyklus besteht aus zwei Schritten:

1. Vorhersage: Ein Bewegungsmodell F projiziert den Zustand vorwärts, x = F x, und die Unsicherheit wächst durch das Prozessrauschen Q, das Bewegungen abdeckt, die das Modell nicht erfasst: P = F P Fᵀ + Q.
2. Aktualisierung: Eine neue Messung z trifft ein, die über das Messmodell H mit dem Zustand zusammenhängt. Der Filter berechnet die Abweichung z − H x und den Kalman-Verstärkungsfaktor K = P Hᵀ (H P Hᵀ + R)⁻¹, wobei R das Messrauschen bezeichnet. Der korrigierte Zustand lautet x + K (z − H x), und die Unsicherheit sinkt auf (I − K H) P.

Der Kalman-Verstärkungsfaktor bestimmt das Gleichgewicht zwischen Modell und Sensor. Sind die Messungen im Verhältnis zur Vorhersage präzise, nähert sich der Faktor 1, und die Schätzung folgt der Messung. Sind die Messungen verrauscht, nähert er sich 0, und die Schätzung folgt der Vorhersage. Da der Zustand Größen enthalten kann, die nie direkt gemessen werden, leitet der Filter auch diese ab: Ein Tracker, der nur Positionen beobachtet, schätzt dennoch die Geschwindigkeit. Fehlt eine Messung, überspringt der Filter die Aktualisierung und setzt die Vorhersage fort:

Kalman-Filter bei der Objektverfolgung#

Bei der Objektverfolgung besitzt jede Spur einen eigenen Kalman-Filter. Der Zustand umfasst den Mittelpunkt und die Größe der Begrenzungsbox sowie deren Geschwindigkeiten. Ein Modell mit konstanter Geschwindigkeit sagt voraus, wo sich die Box im nächsten Frame befinden wird. Diese Vorhersagen werden anhand der Schnittmenge über Vereinigungsmenge (IoU) neuen Erkennungen zugeordnet. Jede zugeordnete Erkennung dient als Messung zur Aktualisierung ihrer Spur. Wird ein Objekt einige Frames lang verdeckt, bewegt sich die Spur entlang ihres vorhergesagten Pfads weiter. Wenn das Objekt wieder auftaucht, liegt seine Erkennung nahe genug an der Vorhersage, um die alte ID beizubehalten.

Jeder Tracker in Ultralytics YOLO funktioniert im Modus zur Objektverfolgung auf diese Weise: Der Tracker ByteTrack verfolgt mithilfe von KalmanFilterXYAH einen 8-dimensionalen Zustand, der den Mittelpunkt der Box, das Seitenverhältnis, die Höhe und die jeweiligen Geschwindigkeiten umfasst; BoT-SORT und TrackTrack verwenden eine Variante mit Breite und Höhe, und beide skalieren ihr Rauschen anhand der Boxgröße, sodass große Objekte in der Nähe größere Sprünge in Pixeln tolerieren. Dieselben Vorhersage- und Aktualisierungsschritte lassen sich für eine Koordinate des Mittelpunkts einer Box mit wenigen Codezeilen in NumPy umsetzen:

import numpy as np

F = np.array([[1.0, 1.0], [0.0, 1.0]])  # constant-velocity model, state = [position, velocity]
H = np.array([[1.0, 0.0]])  # the detector measures position only
Q = 0.001 * np.eye(2)  # process noise: how far real motion may stray from the model
R = np.array([[25.0]])  # measurement noise: detections jitter by about 5 px

rng = np.random.default_rng(0)
truth = 100 + 3.0 * np.arange(60)  # box center moving 3 px per frame
z = truth + rng.normal(0, 5.0, truth.size)  # noisy detections

x, P = np.array([z[0], 0.0]), np.diag([25.0, 100.0])  # start from the first detection
estimates = [x[0]]
for measurement in z[1:]:
    x, P = F @ x, F @ P @ F.T + Q  # predict
    K = P @ H.T @ np.linalg.inv(H @ P @ H.T + R)  # Kalman gain
    x = x + K @ (measurement - H @ x)  # update with the weighted residual
    P = (np.eye(2) - K @ H) @ P
    estimates.append(x[0])

print(f"Detection error: {np.sqrt(np.mean((z - truth) ** 2)):.2f} px")
print(f"Kalman error: {np.sqrt(np.mean((np.array(estimates) - truth) ** 2)):.2f} px")
print(f"Estimated velocity: {x[1]:.2f} px/frame")

Die Ausgabe zeigt einen mittleren quadratischen Fehler von 4.50 px für die unverarbeiteten Erkennungen und 2.48 px für die gefilterten Schätzungen. Die geschätzte Geschwindigkeit beträgt 2.98 px pro Frame und liegt damit nahe am tatsächlichen Wert von 3 px, obwohl die Geschwindigkeit nie gemessen wurde.

Verwandte Filter#

Der herkömmliche Kalman-Filter setzt lineare Bewegungs- und Messmodelle mit gaußschem Rauschen voraus. Verwandte Verfahren lockern diese Annahmen:

• Erweiterter Kalman-Filter (EKF): Linearisiert nichtlineare Modelle bei jedem Schritt mithilfe von Jacobi-Matrizen um die aktuelle Schätzung. Er ist die Standardwahl für GPS und Trägheitsnavigation.
• Unscented-Kalman-Filter (UKF): Führt eine kleine, sorgfältig ausgewählte Menge von Stichprobenpunkten durch das nichtlineare Modell. Das ist meist genauer als eine Linearisierung und erfordert keine Ableitungen.
• Partikelfilter: Stellt die Zustandsverteilung durch viele gewichtete Zufallsstichproben dar. Damit verarbeitet er nicht gaußsche und multimodale Wahrscheinlichkeitsverteilungen, etwa wenn ein Roboter nicht weiß, in welchem von zwei Korridoren er sich befindet – allerdings mit deutlich höherem Rechenaufwand.
• Verdecktes Markov-Modell (HMM): Verwendet dieselbe rekursive Vorhersage und Aktualisierung für diskrete Zustände. Der Kalman-Filter ist das entsprechende Verfahren für kontinuierliche Zustände mit linearer gaußscher Verteilung.

Anwendungen jenseits der Verfolgung#

• Robotik und Sensorfusion: Roboter kombinieren Radodometrie, IMUs, Lidar und Kameras zu einer gemeinsamen Pose-Schätzung. EKF-basierte Verfahren sind ein klassischer Ansatz für die simultane Lokalisierung und Kartierung.
• Erweiterte Realität: Headsets filtern Messungen der Kopfhaltung, damit virtuelle Objekte bei geringer Latenz stabil bleiben.
• Wirtschaft und Signalverarbeitung: Analysten verwenden Kalman-Glättung, um unbeobachtete Trends aus verrauschten Zeitreihen zu schätzen.
• Navigation: Flugzeuge, Schiffe, Drohnen und Smartphones kombinieren GPS mit Trägheitssensoren, damit die Positionsangabe zwischen Satellitenmessungen stabil bleibt. Die Navigation war auch die erste bedeutende Anwendung des Filters: Anfang der 1960er-Jahre passte Stanley F. Schmidt bei NASA Ames das Verfahren für die nichtlineare Flugbahnschätzung im Apollo-Navigationscomputer an.