Visualisieren und Debuggen eines EKF

Ich teste und optimiere gerade einen EKF (Extended Kalman Filter). Die Aufgabe ist die klassische Verfolgung mobiler Roboterposen, bei denen Orientierungspunkte AR-Marker sind.

Manchmal wundere ich mich, wie sich eine Messung auf die Schätzung auswirkt. Wenn ich die betroffenen Zahlen und Matrizen betrachte und berechne, kann ich herausfinden, wie der Aktualisierungsschritt ausgeführt wurde, was und warum genau passiert ist, aber das ist sehr mühsam.

Ich frage mich also, ob irgendjemand eine Technik, einen Trick oder eine clevere Visualisierung verwendet, um ein besseres Gefühl dafür zu bekommen, was im EKF-Aktualisierungsschritt passiert.

UPDATE # 1 (wird genauer sein und eine erste Annäherung dessen zeigen, was ich vorhabe)

Was ich suche, ist eine Möglichkeit, einen Aktualisierungsschritt auf eine Weise zu visualisieren, die mir ein Gefühl dafür gibt, wie sich jede Komponente der Messung auf jede Komponente des Zustands auswirkt.

Meine allererste Idee ist es, die Messung und ihre Vorhersage zusammen mit einigen Vektoren aus der K-Matrix zu zeichnen. Die Vektoren von K geben an, wie sich der Innovationsvektor (Messung - Messvorhersage, nicht dargestellt) auf jede Komponente des Zustands auswirkt.

Derzeit arbeite ich mit einem EKF, bei dem der Status 2D-Pose (x, y, Winkel) ist und die Messungen auch 2D-Posen sind.

In dem angehängten Bild (öffnen Sie es in einer neuen Seite / Registerkarte, um es in voller Auflösung zu sehen) sollte der (skalierte) Vektor K (1,1: 2) (MATLAB-Syntax, um eine Submatrix aus einer 3x3-Matrix zu nehmen) eine Idee geben, wie die erste Die Komponente des EKF-Zustands ändert sich mit dem aktuellen Innovationsvektor, K (2,1: 2), wie sich die zweite Komponente des EKF ändert, usw. In diesem Beispiel hat der Innovationsvektor eine relativ große x-Komponente und ist mit dieser ausgerichtet Vektor K (2,1: 2) - Die zweite Komponente des Zustands (y-Koordinate) ändert sich am stärksten.

Ein Problem in diesem Diagramm ist, dass es keinen Eindruck davon gibt, wie sich die dritte Komponente (der dritte Winkel) des Innovationsvektors auf den Zustand auswirkt. Die erste Komponente des Zustands nimmt etwas zu, entgegen dem, was K (1: 1: 2) angibt - die dritte Komponente der Innovation bewirkt dies, aber ich kann dies derzeit nicht visualisieren.

Die erste Verbesserung wäre, zu visualisieren, wie sich die dritte Komponente der Innovation auf den Staat auswirkt. Dann wäre es schön, Kovarianzdaten hinzuzufügen, um ein Gefühl dafür zu bekommen, wie die K-Matrix erstellt wird.

UPDATE Nr. 2 Das Diagramm enthält nun Vektoren im Zustandsraum, die zeigen, wie jede Messkomponente die Position ändert. Aus diesem Diagramm kann ich ersehen, dass die dritte Komponente der Messung den Zustand am stärksten verändert.

Eine sehr informative Möglichkeit, den Effekt von Messungen zu visualisieren, besteht (für mich) darin, den Zustand des Roboters (Mittelwert mit Kovarianzellipse) vor und nach jeder Messung aufzuzeichnen. Nehmen Sie dann die einzelnen Komponenten der Messung (Peilung, Bereich für AR-Marker) und wenden Sie sie separat an, um ein Gefühl dafür zu bekommen.

Um dies zu tun:

Ich benutze eine oder mehrere dieser Funktionen , um die Ellipse zu zeichnen. Um die Konstanten , beachten Sie, dass sie die Quadratwurzeln der Eigenwerte der Kovarianzmatrix sind. Dann nehme den Winkel über und finde den Bereich aus der Hypothese. unter Verwendung der verknüpften Gleichung. Ich empfehle insbesondere die Verwendung dieser Gleichung :$a,b$$\theta$$[0, 2\pi]$

Das Verfolgen der Kovarianz der vorherigen Hypothese, des gemessenen Zustands und der hinteren Hypothese ist normalerweise ausreichend, um festzustellen, ob die Gleichungen eines EKF korrekt angewendet werden.

Viel Glück und aktualisieren Sie Ihre Frage nicht zu häufig. Kommen Sie stattdessen mit neuen Fragen zurück.

Oft werden die Zustandsvariablen über die Zeit sowie ihre 3-Sigma-Intervalle aufgezeichnet. Punkte, an denen dieses Intervall kleiner wird, sind die Aktualisierungen, an denen Sie möglicherweise die Quelle der betreffenden Messungen mit Anmerkungen versehen können.

Neben Fehlern in der Implementierung, die überprüft werden sollten (nicht nur falsche Gleichungen, sondern auch numerisch instabile Gleichungen), wird die Auswirkung von Aktualisierungen nur direkt durch die Differenz zwischen "erwartet" und "gemessen" und deren jeweiligen Unsicherheiten beeinflusst. Vielleicht möchten Sie herausfinden, wie Sie dieses Gleichgewicht in Bezug auf den Zeitverlauf in der ersten Handlung visualisieren können.