- Messungen von Position und Geschwindigkeit in der realen Welt schwanken immer, daher lässt sich der Kalman-Filter als ein Verfahren verstehen, das mehrere unvollständige Informationsquellen kombiniert, um eine verlässlichere Zustandsschätzung zu erzeugen
- Im Beispiel eines Schiffs weicht das Modell mit einer Motorgeschwindigkeit von 10 m/s wegen Wind und Wellen ab, und auch Sensoren wie GPS sind durch Rauschen oder Ausfälle nicht immer genau
- Der Beispielcode zeigt die Intuition des Kalman-Filters, indem 1000 Passagiere jeweils eine geschwindigkeitsbasierte Schätzung und eine Sensormessung erzeugen und beide Werte per gewichtetem Mittelwert zusammenführen
- Die Zuverlässigkeit der Informationsquellen wird über die Varianz der Messwerte berechnet; stärker schwankende Quellen werden weniger gewichtet, konsistentere stärker, mithilfe von
1/variance
- Wenn der Sensor normal arbeitet, folgt das System stärker dem Sensorwert; in schwankenden Phasen wie bei
t=3 und t=6 wird sein Einfluss automatisch reduziert, sodass eine stabilere Schätzung erhalten bleibt
Situationen, in denen ein Kalman-Filter nötig ist
- Der Kalman-Filter kann als Trichter verstanden werden, der mehrere verrauschte Informationsquellen zu einem genaueren statistischen Wert verdichtet
- Mathematisch spielen lineare Algebra, Wahrscheinlichkeitstheorie und Differentialrechnung eine Rolle, hier liegt der Fokus jedoch eher auf der Intuition als auf der vollständigen Theorie
- Es wird angenommen, dass ein Schiff im Hafen bei
x=0 startet, sich eindimensional bewegt und der Motor eine konstante Geschwindigkeit von 10 m/s liefert
- In einer idealen Welt läge die Position nach 2 Sekunden bei
2 * 10 = 20m, aber in der realen Welt bleiben Geschwindigkeit und Position wegen Motor, Wind und Wellen nicht exakt erhalten
- Deshalb ist es schwierig, allein mit der Positionsformel sicher zu sein, wo sich das Schiff tatsächlich befindet
Auch Sensoren sind nicht perfekt
- Mit einem Sensor wie GPS kann man die Position zu einem bestimmten Zeitpunkt direkt messen, aber auch Sensorwerte sind nicht immer exakt
- GPS kann zum Zeitpunkt 3 Sekunden Werte nahe an der tatsächlichen Position liefern, etwa 29.998 m oder 30.002 m, in sehr seltenen Fällen aber auch stark abweichen, etwa auf 100 m
- In Gebieten ohne Satellitenabdeckung kann ein GPS-Sensor praktisch nicht funktionieren
- Wäre ein Sensor niemals offline und könnte mit beliebiger Präzision stets den gewünschten Wert messen, bräuchte man keinen Kalman-Filter
- Der Kalman-Filter kann mehrere Informationsquellen wie geschwindigkeitsbasierte Positionsschätzung, GPS-Schätzung, Radar und Sonar kombinieren, um die Position genauer zu bestimmen
Positionsschätzung im Code
- Im Beispiel wird angenommen, dass sich 1000 Passagiere auf dem Schiff befinden und jeder ein eigenes GPS-Gerät hat
- Jeder Passagier schätzt zunächst eine neue Position, indem er zur vorherigen Position die Geschwindigkeit und die Schwankungen externer Faktoren addiert
from random import gauss
def new_position(last):
velocity = 10
wind = gauss(0, 2)
wave = gauss(0, 0.1)
return last + velocity + wind + wave
gauss erzeugt zufällige positive oder negative Werte, und der zweite Parameter steht für die Stärke der Schwankung- Da sich die Auswirkungen von Wind und Wellen nicht direkt messen lassen, modelliert das Beispiel das Rauschen mit Zufallswerten mit Mittelwert 0 und Standardabweichungen
2 und 0.1
- Im zweiten Schritt gibt der Sensor einen Messwert zurück, indem er zur tatsächlichen Position ein Sensorrauschen addiert
def sensor(t):
if t == 3:
# oops, passing through a thunderstorm. GPS fluctuating!
sensor_noise = gauss(5, 10)
elif t == 6:
# uh-oh, satellite unavailable!
sensor_noise = gauss(-5, 10)
else:
sensor_noise = gauss(0, 1)
return true_position[t] + sensor_noise
- Bei
t=3 wird wegen eines Gewitters ein schwankendes GPS modelliert, bei t=6 eine Situation ohne verfügbaren Satelliten
- Selbst zum selben Zeitpunkt fallen die Sensormessungen für verschiedene Passagiere unterschiedlich aus
Tatsächlicher Bewegungsverlauf und einfacher Mittelwert
- Die tatsächliche Position des Schiffs ist in der folgenden Liste gegeben
true_position = [0, 9, 19.2, 28, 38.1, 48.5, 57.2, 66.2, 77.5, 85, 95.2]
- Das Schiff startet im Hafen bei
x=0, liegt nach 1 Sekunde bei 9 m, nach 2 Sekunden bei 19.2 m und bewegt sich danach entsprechend den Werten in der Liste weiter
- Das Ziel der Passagiere ist es, die Position in jeder Sekunde mit verrauschten und unzuverlässigen Messwerten so genau wie möglich vorherzusagen
- Wenn bei
t=1 die geschwindigkeitsbasierte Schätzung eines Passagiers 9.37 und die Sensormessung 8.98 beträgt, ergibt der einfache Mittelwert 9.17
- Liegt die tatsächliche Position bei 9 m, dann ist dieser einfache Mittelwert zwar genauer als die Geschwindigkeitsschätzung, aber schlechter als genau diese Sensormessung
Gewichteter Mittelwert und Zuverlässigkeit
- Als bessere Methode als der einfache Mittelwert wird ein gewichteter Mittelwert verwendet
def combine(A, B, trustA, trustB):
total_trust = trustA + trustB
return (A * trustA + B * trustB) / total_trust
combine(9.37, 8.98, 10, 1) vertraut der Geschwindigkeitsschätzung stärker, daher liegt das Ergebnis mit 9.33 näher an 9.37combine(9.37, 8.98, 1, 10) vertraut der Sensormessung stärker, daher liegt das Ergebnis mit 9.01 näher an 8.98- Dieser zuverlässigkeitsbasierte gewichtete Mittelwert ist die zentrale Intuition des Kalman-Filters und das Kraftzentrum der Datenfusion
- Welche Informationsquelle stärker gewichtet wird, entscheidet die Varianz
- Informationsquellen, deren Schlussfolgerungen stark schwanken, wird weniger vertraut
- Informationsquellen mit konsistenten Schlussfolgerungen wird stärker vertraut
- Wenn von 10 Radiosendern 4 Regen und 6 Sonne melden, aber von 10 Websites 9 Regen melden, dann sind die Websites wegen der geringeren Varianz vertrauenswürdiger
Der Update-Schritt
- Das gesamte Update erzeugt zunächst für jeden Passagier eine geschwindigkeitsbasierte Schätzung und eine Sensormessung und berechnet dann die Zuverlässigkeit aus der Varianz beider Messmengen
from statistics import variance
def update(t, last):
velocity_updates = []
sensor_updates = []
for p in range(1000):
velocity_updates.append(new_position(last[p]))
sensor_updates.append(sensor(t))
fluctuation_velocity = variance(velocity_updates)
fluctuation_sensor = variance(sensor_updates)
trust_velocity = 1 / fluctuation_velocity
trust_sensor = 1 / fluctuation_sensor
combined = []
for p in range(1000):
combined.append(combine(
A=velocity_updates[p],
B=sensor_updates[p],
trustA=trust_velocity,
trustB=trust_sensor
))
return sensor_updates, velocity_updates, combined
- Da die Zuverlässigkeit mit steigender Varianz sinkt, wird
1/variance verwendet
- Jeder Passagier aktualisiert seine Position individuell
- Nachdem die Positionsupdates aller Passagiere abgeschlossen sind, kann die Schätzung der tatsächlichen Schiffsposition aus dem Mittelwert der Passagierpositionen abgeleitet werden
Wie man die Ergebnisse liest
- Die Funktion
update_plot speichert für die Darstellung im Plot die tatsächliche Position, die Sensorschätzung, die Geschwindigkeitsschätzung und die kombinierte Schätzung für jeden Zeitpunkt
- Die Hauptschleife aktualisiert die Positionsschätzung zu jedem Zeitpunkt fortlaufend anhand der aktuell besten Schätzungen der Passagiere
- Die envelope um die Linien im Plot steht für Unsicherheit; je breiter sie ist, desto größer ist die Unsicherheit dieses Werts
- Im Bereich von
t=0.75 bis t=1, in dem der Sensor normal funktioniert, ist die kombinierte Positionsschätzung besser als eine reine Geschwindigkeitsschätzung, kann aber schlechter sein als die reine Sensormessung
- Im Bereich von
t=2 bis t=4, in dem der Sensor ausfällt, liefert die kombinierte Schätzung bessere Ergebnisse als die alleinige Verwendung der fehlerhaften Sensormessung
- Im Bereich von
t=4 bis t=5, wenn sich der Sensor erholt, beginnt der Kalman-Filter wieder, den Sensor stärker zu bevorzugen
Anhang: gauss und Varianz
- Die
normal distribution function gauss(0, 0.1) und gauss(0, 2) erzeugen meist Zufallswerte nahe 0
- Der zweite Parameter, die Standardabweichung, steuert, wie stark die Messwerte schwanken
gauss(0, 0.1) liefert mit hoher Wahrscheinlichkeit kleine Werte nahe 0 wie 0.06, -0.07 oder 0.02gauss(0, 2) liefert mit höherer Wahrscheinlichkeit stärker verteilte Werte wie 1.05, -1.06, 1.29 oder -1.72
- Im Beispielcode wird angenommen, dass der Wind stärker schwankt und die Wellen geringer
- Varianz ist ein Maß für Konsistenz: hohe Konsistenz bedeutet niedrige Varianz, geringe Konsistenz hohe Varianz
- Eine Verteilung mit Standardabweichung 2 hat die Varianz 4, eine Verteilung mit Standardabweichung 0.1 die Varianz 0.01
1 Kommentare
Hacker-News-Kommentare
Der Artikel war unterhaltsam zu lesen, aber die Implementierung ist falsch. Der größte Fehler ist, dass die Unsicherheit nicht entlang der Zeitachse fortgeschrieben wird und der Fehler dadurch unterschätzt wird.
Das zeigt sich auch in der Grafik: Der Fehlerbereich sollte die meiste Zeit den tatsächlichen Zustand enthalten, tut es im Ergebnis aber nicht.
Nicht die Passagiere addieren Rauschen zum Schätzwert; vielmehr ist aus Sicht der Passagiere, gegeben der Zustand zum Zeitpunkt k, der Erwartungswert des Zustands zum Zeitpunkt k+1 schlicht
position_k+1 = position_k + velocity * Delta_t. In der realen Dynamik gibt es Rauschen, und im Filter wird das berücksichtigt, indem man es zur geschätzten Kovarianz addiert.Der Grund, warum der Code nicht sofort kaputtgeht, ist, dass mit 1000 Passagieren viele Stichproben der Dynamik gezogen und die Varianz des Ergebnisses numerisch berechnet wird; das unterscheidet sich ziemlich von dem, was man in der Praxis normalerweise macht.
Außerdem ist es ein verbreiteter Irrtum, dass GPS vom Wetter beeinflusst wird; tatsächlich ist das nicht der Fall. Und die im Artikel verwendete Definition von Konsistenz ist ebenfalls nicht standardmäßig. In der Schätztheorie bedeutet die Aussage, ein Schätzer sei konsistent, dass der Schätzwert mit wachsender Datenmenge gegen den wahren Wert konvergiert.
Es ist gut, dass der Artikel es für ein allgemeines Publikum verständlich aufbereitet, aber einige Missverständnisse scheinen Probleme zu verursachen. Ich bin Doktorand in Schätztheorie; wenn ich mehr dazu ausführen soll, helfe ich gern.
Es brachte mich fast dazu, mich zu fragen, warum der Kalman Filter in Signalverarbeitungs-Vorlesungen an der Universität nicht so einfach vermittelt wird. Mathematische Konzepte mathematisch zu lehren ist richtig, aber für Menschen ohne ausreichendes Vorwissen geht dabei Information verloren.
Früher habe ich die diskrete Kosinustransformation und die Wavelet-Transformation bildzentriert unterrichtet; zuerst Intuition statt Strenge zu vermitteln funktionierte immer besser als die umgekehrte Reihenfolge.
Dass Professoren nicht zuerst Intuition vermitteln, kann mehrere Gründe haben. Vielleicht haben sie selbst mehr tiefgehende Expertise in Zahlen und dem Umformen von Gleichungen als in Intuition; vielleicht wird Lehrkompetenz nicht belohnt, sodass sie die Zeit, die eine intuitive Aufbereitung kosten würde, lieber in Förderanträge oder die Betreuung von Doktoranden stecken; und vielleicht weigert sich das Gehirn, zurückzugehen, weil sich eine intuitive Erklärung nach dem Verstehen der Mathematik eher wie der „schwierige Weg“ anfühlt.
Vor allem den dritten Punkt halte ich für den wichtigsten. Das reicht über Mathematikdidaktik hinaus und betrifft auch den Unterschied zwischen Expertise und Lehrkompetenz. Wenn man den Golf-Drive lernen will, ist es womöglich besser, von jemandem zu lernen, der bei 100 Yards angefangen hat und sich dann stetig auf 300 Yards gesteigert hat, als von der Person, die am weitesten schlägt.
Als allgemeine Warnung für alle, die einen Kalman Filter implementieren wollen: Zum Umgang mit numerischer Instabilität lohnt es sich, die ersten paar Seiten von https://www.stat.berkeley.edu/~brill/Stat248/kalmanfiltering... zu lesen.
In der Praxis habe ich dabei zwar selten auffällige numerische Artefakte gesehen, aber es ist eine ziemlich attraktive Lösung.
Ergänzend zu diesem Artikel empfehle ich dieses Buch sehr, wenn man eine gründlichere und mathematischere Einführung in die Familie der Kalman Filter möchte: https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Pyt...
Das Buch wurde von einem Software Engineer geschrieben, der beruflich Kalman Filter implementieren musste; die Art, wie es die Konzepte motiviert und vermittelt, dürfte gut zu dieser Zielgruppe passen. Es ist als interaktives Jupyter Notebook geschrieben, sodass man das Repository klonen, es selbst ausführen und Schritt für Schritt mitmachen kann.
Es beginnt mit einfachen Filtern, führt dann die Bayes-Regel ein und erweitert das Ganze auf Wahrscheinlichkeitsverteilungen; so bietet es einen sanften Einstiegspfad zum Kalman Filter.
Ein Aspekt fehlt. Wenn man den gewichteten Durchschnitt aus Vorhersage und Messwert bildet, können sich die Gewichte des Kalman Filters im Zeitverlauf ändern. Sonst würde er, denke ich, anders heißen.
Ein gutes Beispiel ist die Messung eines sich langsam verändernden Werts mit einem einzelnen Sensor. Bei einer Tankanzeige etwa ist es sinnvoll, im Sekundenbereich keine Veränderung anzunehmen, während die Messwerte durch Rauschen wie das Schwappen des Kraftstoffs im Tank beeinflusst sein können.
In diesem Fall sieht der Kalman Filter wie ein Tiefpassfilter erster Ordnung mit exponentiell abklingender Verstärkung aus. Die Grenzfrequenz ändert sich, sodass der Anfangspegel innerhalb weniger Sekunden schnell gefunden wird und danach mit einer sehr niedrigen Grenzfrequenz wie 0,01 Hz Rauschen ignoriert werden kann.
Ein guter Artikel über ein wichtiges Werkzeug.
Nach meinem Verständnis ist der lineare Kalman-Filter die optimale Lösung für lineare Probleme und relativ leicht zu verstehen und zu implementieren. Die meisten Anwendungen, die ich gesehen habe, waren jedoch nichtlinear.
Der erweiterte Kalman-Filter und der Unscented Kalman-Filter sind deutlich schwerer zu verstehen und zu implementieren, und es gibt weniger sowie weniger nützliche Materialien und Bibliotheken dazu.
Ich habe zum Beispiel mit einem AHRS/GNSS-CAN-Gerät für kleine UAVs gearbeitet; der erweiterte Kalman-Filter, den ich in PX4 oder Ardupilot gesehen habe, war sehr komplex und hatte viele Parameter. Deshalb war ein Ansatz einfacher, der von den Grundprinzipien der Quaternionen ausgeht und die Gyro-Lösung Stück für Stück in Richtung „oben“ des Beschleunigungssensors und in Richtung des Neigungsvektors des Magnetometers korrigiert.
Wenn der Betrag der Beschleunigung stark von 1G abweicht oder der Magnetfeldvektor stark von der lokalen Erdmagnetfeldstärke abweicht, wird das Gewicht des entsprechenden Sensor-Updates verringert oder es wird übersprungen, und der Gyro läuft einfach weiter. Wahrscheinlich ist der EKF die richtige Antwort, aber ich habe es aufgegeben, ihn in eine Form zu bringen, die leicht zu verstehen, zu bauen, abzustimmen und zu diagnostizieren ist.
Bei Quadrotoren ist die große Schwierigkeit allerdings die Rotation. Das lineare Modell eines Kalman-Filters nimmt an, dass alles in einem euklidischen Raum liegt, Rotation befindet sich aber auf einer Mannigfaltigkeit. Im Fall von Quaternionen ist diese Mannigfaltigkeit die Menge der Einheitsquaternionen.
Wenn man naiv einen EKF anwendet, um ein Quaternion zu schätzen, ist es irgendwann kein Einheitsquaternion mehr, und die Schätzung geht kaputt. Es gibt bekannte Methoden, um mit dieser Mannigfaltigkeitsbedingung umzugehen, aber die Gleichungen, die ich in Code übertragen habe, gehörten zu den hässlichsten, die ich gesehen habe.
Als einfaches Beispiel kann man sich einen Zustand
(x, y)vorstellen, der aufgrund physikalischer Gesetze immer auf dem Einheitskreis liegen muss. Die echte Dynamikf(x, y)liefert einen neuen Punkt auf dem Kreis, aber die linearisierte Näherungsdynamik garantiert nicht, dass sie auf dem Einheitskreis bleibt, was zu unphysikalischen Zuständen oder EKF-Zustandsschätzungen führen kann.Optionen, die man sich ansehen kann, sind ForneyLab.jl, Infer.net, Gen.jl und Pyro.
Es gibt jedoch viele Arten, Kalman-Filter einzusetzen, und je nachdem, wo man anfängt, kann der korrekte Umgang mit nichtlinearen Transformationen extrem mühsam werden.
Ich bin kurzsichtig und habe Astigmatismus, und wenn ich ein Auge schließe und auf etwas wie eine Wanduhr schaue, merke ich, dass jedes Auge ein anders verzerrtes Bild erzeugt. Die beiden Augen unterscheiden sich ein wenig.
Wenn ich die Uhr aber mit beiden Augen ansehe, ist das Bild viel schärfer und besser als mit nur einem der beiden Augen. Das Szenario im Artikel, in dem 1000 Schiffspassagiere jeweils ihre GPS-Koordinaten melden, hat mich an dieses Phänomen erinnert.
Das Gehirn dürfte ebenfalls in großem Umfang clevere Algorithmen wie Kalman-Filter verwenden.
Ist es richtig, Kalman-Filter so zu verstehen, dass sie aus verrauschten Beobachtungen bessere Schätzwerte liefern als ein einfacher Mittelwert?
Wenn man zum Beispiel etwas dreimal misst und 7, 8, 9 erhält, würde man vermuten, dass der tatsächliche Wert 8 ist. Würde ein Kalman-Filter einen anderen Schätzwert ausgeben?
Kalman-Filter werden traditionell zur Schätzung von Dingen verwendet, die sich über die Zeit bewegen. Man kann an eine Person in einem Video oder an eine Art Random Walk denken.
Wenn man annimmt, dass zwischen zwei aufeinanderfolgenden Zeitpunkten oder Messwerten Beziehungen wie Geschwindigkeit und aktuelle Richtung bestehen, kann man die Informationen aus dem Bewegungsmodell und dem verrauschten Messmodell mischen, um Position oder Wert, oder auch die gesamte Bewegungshistorie, besser zu schätzen.
Wenn das Bewegungsmodell in sinnvoller Weise falsch ist, verbessert sich die Schätzung nicht. Viele spätere Erweiterungen konzentrieren sich darauf, ausgefeiltere Bewegungsmodelle einzubeziehen, etwa Radschlupf in der Robotik.
Wenn man zum Beispiel eine Konstante misst, kann man als Grundmodell eine Konstante mit anfänglicher Unsicherheit ansetzen, etwa eine Gaussian-Verteilung mit Standardabweichung, sowie Messwerte, die ebenfalls Gaussian-Rauschen und eine Standardabweichung haben. Man kann die anfängliche Unsicherheit um die zu schätzende Konstante und die Unsicherheit der Messwerte anpassen.
In diesem Beispiel verhält sich der Kalman-Filter nicht wie ein Mittelwert. Sind die Messwerte gut, also haben sie geringe Unsicherheit, konvergiert er schnell; sind die Messwerte schlecht, schwankt die Schätzung und braucht länger, um zu konvergieren.
Außerdem stimmt die Aussage nicht, dass Kalman-Filter nur für bewegte Dinge verwendet werden. Für die Schätzung von Konstanten werden sie ebenfalls ständig eingesetzt, sie sind nur bei bewegten Objekten bekannter.
Kalman-Filter behandeln Fälle, in denen Stichproben aufgrund irgendeiner linearen Dynamik korreliert sind. Der Messwert muss nicht einmal das interessierende Objekt selbst sein; er kann auch eine lineare Funktion dieses Objekts plus Gaussian-Rauschen sein.
Das Wissen, dass man bei der ersten Messung eine 7 gesehen hat, verändert daher die Wahrscheinlichkeit, bei der zweiten Messung eine 8 zu sehen. Wenn man wie oben nur den Stichprobenmittelwert bildet, konvergiert dieser im Allgemeinen nicht gegen den tatsächlichen Mittelwert.
Was sie im Beispiel ausgeben, hängt vom genauen Modell ab. Selbst im einfachsten vorstellbaren Szenario muss man wie bei Bayes festlegen, mit welchem Erwartungswert man beginnt.
Wenn man diese Darstellung als lineares Gaussian-Modell kodiert, erhält man einen Kalman-Filter.
Ein kurzer Text, der mir beim Verständnis des Kalman-Filters ebenfalls ein Aha-Erlebnis verschafft hat, war eine Notiz von John D. Cook: https://www.johndcook.com/blog/applied-kalman-filtering/
Traditionelle Systemmodellierung auf Basis von Analysis und Differentialgleichungen geht davon aus, dass die Daten keine Unsicherheit enthalten und alles im Systemmodell abgebildet ist. Datenbasierte Schätzer dagegen nehmen an, „alles steckt in den Daten“, und ignorieren das Modell des physikalischen Prozesses, der die Daten erzeugt, vollständig.
Die Schönheit des Kalman-Filters liegt darin, dass er diese beiden Ansätze verbindet.
Es gibt auch einen weiteren Artikel mit guten Visualisierungen: https://www.bzarg.com/p/how-a-kalman-filter-works-in-picture...
Dieser Artikel war bereits dreimal auf HN.