Erweitertes Kalmanfilter: Zustandsschätzung für einen dreige

MrLin · Gast

Meine Frage:
Der Zustand eines dreigelenkigen planaren Roboterarms soll durch die Positionen und Winkelgeschwindigkeiten in den drei Antrieben beschrieben sein. Es gilt also

. Die Steuereingaben

zu jedem Zeitpunkt sind die Beschleunigungen in den drei Gelenken.
a)Ermitteln Sie für den Roboterarm die Systemgleichung

unter der Verwendung der Zeitdiskretisierung

.
b)Stellen Sie die Gleichungen zur Berechnung des Time-Updates auf.
c)Als Sensorsysteme soll der Roboterarm über Winkelgeschwindigkeitssensoren in den Gelenken und über ein Positionsmesssystem im Ursprung des Endeffektor-Koordinatensystems verfügen. Alle Sensoren sollen gleichzeitig messen. Die Messung liefert somit einen Vektor

. Stellen Sie die Gleichungen für das Measurement-Update (inklusive der benötigten Jacobimatrizen) auf.
Hinweis: Die Vorwärtskinematik des dreigelenkigen planaren Roboterarms ist durch

gegeben, wobei

und

die Längen der einzelnen Glieder des Roboterarms sind.
Ich verstehe bei aktuell nicht so ganz, was ich darstellen/berechnen soll. Besonders bei a) und b). Die Formeln sind mir zwar bekannt, aber ich weiß nicht, was man als Ergebnis sehen will.

Meine Ideen:
a) Ansatz(Allgemeines Roboterbewegungsmodell):

Führt zu

Nur denk ich nicht, dass es das richtige Ergebnis ist, weil die Herleitung im Skript gegeben ist. Wie würdet ihr die Frage verstehen?
b)

(Q=gegebene Kovarianzmatrix)

Das sind die Formeln für das Time-Update, aber damit kann ich nichts anfangen. Hier fehlt mir die Idee.