Loading Video...

Das Extended Kalman Filter ausführlich erklärt [Video]

by Paul Balzer on 3. März 2014

3 Comments

Play Video Hide Video

Da die Beiträge zum Kalman Filter (Teil1 und Teil2) sowie der Beitrag zum Extended Kalman Filter die am Häufigsten gelesenen des Motorblogs sind, habe ich zum Extended Kalman Filter noch mal einen etwas detaillierteren Screencast aufgezeichnet, welcher recht ausführlich erläutert, wie dieser aufgesetzt wird und arbeitet.

Extended Kalman Filter with CTRV Vehicle Model in Python from Paul Balzer on Vimeo.

Extended Kalman Filter in Python

Das Extended Kalman Filter ermöglicht die Fusion mehrerer verrauschter Sensorsignale unter Zuhilfenahme eines (nicht linearen ) Dynamik-Modells, welches linearisiert wird. Die Linearisierung erfolgt dabei mit so genannten Jacobi Matrizen. Diese können numerisch berechnet werden, wie im Matlab Beispiel zum EKF gezeigt, oder eben analytisch, wie in dieser Python Implementierung. Dank Sympy, dem symbolischen Python Paket, funktioniert das relativ fehlerfrei und komfortabel.

Extended Kalman Filter

Extended Kalman Filter

Der vollständige Code sowie das IPython Notebook aus dem Video ist auf http://balzer82.github.io/Kalman/ zu finden.

Fahrzeug als CTRV Modell (Constant Turn Rate and Velocity)

Das implementierte Dynamikmodell ist ein Fahrzeug, welches mit konstanter Geschwindigkeit (zwischen zwei Filterschritten) und Gierrate fährt und nach [Schubert, R., Adam, C., Obst, M., Mattern, N., Leonhardt, V., & Wanielik, G. (2011). Empirical evaluation of vehicular models for ego motion estimation. 2011 IEEE Intelligent Vehicles Symposium (IV), 534–539. doi:10.1109/IVS.2011.5940526] gute Ergebnisse liefert.

Die Messung der Position erfolgt mit 10Hz und stellt die “Correction” des Systemzustands dar. Der Control Input des Filters ist die Gierrate und Geschwindigkeit, welche genau genommen auch gemessen werden, hier aber als Steuergrößen angenommen sind.

Positionsschätzung des Extended Kalman Filters mit CTRV Modell

Positionsschätzung des Extended Kalman Filters mit CTRV Modell

3 Comments

  1. Guten Tag Herr Balzer,

    erst mal vielen Dank für die schöne Übersicht und Erklärung zum Kalman Filter.
    Im Falle der Geradeaus fahrt, also der Gierrate=0, müsste hier nicht eine neue Jacobimatrix von A berechnet werden, da sich das dynamische Modell unterscheidet.

    Vielen Dank

    Mit freundlichen Grüßen
    Björn Beckmann

    1. Hallo Hr Beckmann,
      das ist korrekt, genau genommen müsste man das machen. Ich habe diesen “Hack” in die Prädiktion eingebaut, weil es sonst Fälle gibt, in denen das Filter numerisch nicht durchlaufen würde. Es gibt Logfiles, da hat der Drehratensensor keinen Wert geliefert und das Filter müsste durch 0 teilen. Um das abzufangen habe ich die paar Zeilen eingefügt, ohne das konsistent auch auf die Jacobi Matrizen durchzuziehen. Wenn man sich anschaut wie oft er dort in die exakte Geradeausfahrt springt sieht man, dass es sogut wie nie vorkommt. Der Fehler den man durch meine unvollständige Implementierung macht, ist also zu vernachlässigen.
      Danke für den Hinweis!

  2. Hallo Herr Balzer!

    Die Gleichung zur Prädiktion der Fehlerkovarianz verwirrt mich etwas. Sie weicht von der im Video ab. Warum wird hier die Prozessrauschkovarianzmatrix Q noch von links und rechts mit der Jakobischen von g multipliziert?

    lg Michael

Leave a Reply

Deine E-Mail-Adresse wird nicht veröffentlicht. Erforderliche Felder sind mit * markiert