KF & EKF
schreibe vorne
Dieser Artikel bezieht sich auf Kapitel 3 von „Probabilistic Robot“.
Lassen Sie mich zunächst erklären, wofür KF und EKF verwendet werden können. Aus meiner Sicht geht es darum, das Slam-Problem zu untersuchen und die Kalman-Filterung zu lernen. Intuitiv können einige komplexe Zustandsschätzungsprobleme gelöst werden. Aus Sicht mobiler Roboter kommt es zu Regelabweichungen, da die Bewegung des Roboters von der Steuerung gesteuert wird und die Wahrnehmung des Roboters aufgrund von Sensormessungen zu Abweichungen führt, was zu Unsicherheiten im Robotersystem führt. KF und EKF können auf die Unsicherheiten im Robotersystem abzielen und ein Gaußsches Rauschmodell in die Bewegung und Beobachtung des Roboters einführen, sodass das Robotersystem trotz vieler Unsicherheiten immer noch gut funktioniert. Als nächstes werden die theoretischen Grundformeln von KF und EKF im Detail vorgestellt.
Hinweis: In diesem Artikel werden nur die Ursprungs- und Schlussfolgerungsformeln klar erläutert und nicht zu viel über die detaillierten mathematischen Beweise und die detaillierte Ableitung von KF und EKF erklärt. Es sollte in der Lage sein, die Anforderungen im technischen Einsatz zu erfüllen. Wenn Sie Bedenken hinsichtlich seiner mathematisch-theoretischen Ableitung haben, lesen Sie bitte den Originaltext von „Probabilistic Robot“.
KF
Aufgrund der vielen Unsicherheiten im Robotersystem können Roboter in der realen Welt die vorgegebene Position durch Algorithmen nicht genau erreichen. Daher kann für den Zustand des Roboters ein Intervall definiert werden. Innerhalb dieses Zustandsintervalls wird der Die Wahrscheinlichkeit, dass sich der Roboter in einem Zustand innerhalb des Intervalls befindet, wird üblicherweise einer Gaußschen Verteilung gehorchen.
Die Bewegungsgleichung des Roboters ist unten definiert:
wobei x der Zustand des Roboters ist, die Matrizen A und B die Zustandsübergangsmatrix bzw. die Kontrollmatrix sind , und u ist der Steuervektor. Der letztere Term ist ein Gaußscher Zufallsvektor, der die Unsicherheit der Roboterbewegungssteuerung darstellt, in der die Gaußschen Rauschparameter u, σ sind. In einem weiteren Schritt können wir die Posterior-Verteilungswahrscheinlichkeitsdichtefunktion des installierten Zustands x erhalten:
Dieselbe Logik wie die obige Bewegungsgleichung, definieren Sie die Beobachtungsgleichung:
In der obigen Formel ist C die Beobachtungsmatrix und das letzte Element ist der Gaußsche Rauschterm. Dann wird die Posterior-Verteilungswahrscheinlichkeitsdichtefunktion der Beobachtungsgleichung erhalten:
Angenommen, die anfängliche Der Zustand des Roboters folgt ebenfalls der Gaußschen Verteilung:
Auf diese Weise wird die mathematische Logik der Kalman-Filterung definiert. Bevor ich mit der Einführung des KF-Prozesses beginne, möchte ich die Formel erläutern. In Bezug auf die mehrdimensionale Gaußsche Funktion müssen Sie sich über deren Bedeutung im Klaren sein.
Anhand der obigen Bewegungs- und Beobachtungsgleichungen können wir erkennen, dass im Roboterzustand Unsicherheit besteht. Der Ansatz von KF besteht darin, den Zustand auf Grundlage der Bewegung abzuschätzen und den Zustand auf Grundlage von Beobachtungen zu aktualisieren. Der spezifische Algorithmusablauf ist wie folgt:
wobei K der Kalman-Gewinn ist, der aus der Kovarianz der Beobachtungsmatrix und dem Beobachtungsrauschen berechnet wird und die Methode zur Aktualisierung des Zustands basierend auf der Beobachtung widerspiegelt. Wenn Sie Wenn Sie an der konkreten Ableitung interessiert sind, lesen Sie bitte das Original. Das Bild unten zeigt ein einfaches Beispiel für einen Kalman-Filter:
EKF
Wenn Sie genau hinschauen, werden Sie feststellen, dass in den obigen Bewegungs- und Beobachtungsgleichungen des Roboters alle ihre Ausdrücke linear sind, sodass der vorherige Zustand des Roboters der Gaußschen Verteilung folgt. Nach einer linearen Änderung wird der neue Zustand Gehorchen a neue Gaußsche Verteilung, so dass die Zustandsschätzung zum neuen Zeitpunkt erhalten wird. In der tatsächlichen Bewegung des Roboters gibt es jedoch fast keine linearen Bewegungsgleichungen und Beobachtungsgleichungen, und die meisten davon sind nichtlinear. Was passiert also, wenn die Gaußsche Verteilung, der der Zustand im vorherigen Moment gehorchte, eine nichtlineare Transformation erfährt? Durch den Vergleich des ersten und zweiten Bildes unten wird die Gaußsche Verteilung nach der linearen Transformation zu einer neuen Gaußschen Verteilung. Nach einer nichtlinearen Transformation ist sie keine Gaußsche Verteilung mehr.
Das bedeutet, dass KF das eigentliche Problem der Roboterzustandsschätzung in der realen Welt nicht lösen kann. Was sollen wir tun? Eine wichtige Idee ist die Linearisierung (lineare Näherung).
Jetzt definieren wir die Bewegungsgleichung und Beobachtungsgleichung des Roboters neu:
Sowohl g als auch z sind hier nichtlinear. Diese beiden nichtlinearen Funktionen können durch die Taylor-Erweiterung linearisiert werden. Die Taylor-Erweiterung kann eine Näherungsfunktion der Funktion g aus der Wertwurzel und der Steigung von g konstruieren. Seine Steigung wird als partielle Ableitung ausgedrückt:
Für die nichtlineare Funktion g bei μ t-1 kann sie durch Taylor-Erweiterung erhalten werden:
Dann die obige Formel ist ungefähr eine lineare Transformation. In einem Schritt kann die Posteriorverteilungswahrscheinlichkeitsdichtefunktion des Zustands x zum Zeitpunkt t erhalten werden:
Gemäß der Linearisierungsidee der Taylor-Erweiterung der obigen Bewegungsgleichung , kann die Taylor-Erweiterung der Beobachtungsgleichung erhalten werden:
Darunter die Steigung:
Posterior-Verteilungswahrscheinlichkeitsdichtefunktion:
Nach der Linearisierung kann der Zustand wie KF geschätzt und aktualisiert werden. Der EKF-Prozess ist unten dargestellt:
Abschluss
Durch die obige Diskussion haben wir verstanden, welche Probleme KF und EKF theoretisch lösen und warum EKF (Linearisierung nichtlinearer Probleme) vorgeschlagen wird. Abschließend wird ein Bild im Originalartikel verwendet, um den Vergleich des Algorithmusflusses und der Berechnung zwischen KF und EKF zu veranschaulichen:
Hinweis: Tatsächlich habe ich das Prinzip der Kalman-Filterung schon einmal gelernt, aber weil ich von der Jacobi-Matrix verwirrt war, sind die Kovarianzmatrix und die Jacobi-Matrix nach systematischer Betrachtung aller Formeln genau das. Auch wenn ich es erklären möchte Zu viel, ich kann es nicht aufschreiben.