Ich arbeite derzeit an einem Projekt für die Schule, bei dem ich einen erweiterten Kalman-Filter für einen Punktroboter mit einem Laserscanner implementieren muss. Der Roboter kann sich mit einem Wenderadius von 0 Grad drehen und vorwärts fahren. Alle Bewegungen sind stückweise linear (fahren, drehen, fahren).
Der von uns verwendete Simulator unterstützt keine Beschleunigung, alle Bewegungen erfolgen sofort.
Wir haben auch eine bekannte Karte (PNG-Bild), in der wir lokalisieren müssen. Wir können Strahlspuren im Bild verfolgen, um Laserscans zu simulieren.
Mein Partner und ich sind wenig verwirrt über die Bewegungs- und Sensormodelle, die wir verwenden müssen.
Bisher modellieren wir den Zustand als Vektor .
Wir verwenden die Aktualisierungsgleichungen wie folgt
void kalman::predict(const nav_msgs::Odometry msg){
this->X[0] += linear * dt * cos( X[2] ); //x
this->X[1] += linear * dt * sin( X[2] ); //y
this->X[2] += angular * dt; //theta
this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
this->F(1,2) = linear * dt * cos( X[2] ); //t+1 ?
P = F * P * F.t() + Q;
this->linear = msg.twist.twist.linear.x;
this->angular = msg.twist.twist.angular.z;
return;
}
Wir dachten, wir hätten alles zum Laufen gebracht, bis wir bemerkten, dass wir die Initialisierung vergessen hatten P
und dass es Null war, was bedeutete, dass keine Korrektur stattfand. Anscheinend war unsere Ausbreitung sehr genau, da wir noch kein Rauschen in das System eingeführt haben.
Für das Bewegungsmodell verwenden wir die folgende Matrix für F:
Da es der Jacobian unserer Update-Formeln ist. Ist das richtig?
Das andere Problem war, wie man P initialisiert. Wir haben 1.10.100 ausprobiert und alle platzieren den Roboter außerhalb der Karte bei (-90, -70), wenn die Karte nur 50x50 ist.
Den Code für unser Projekt finden Sie hier: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp
Jeder Rat wird sehr geschätzt.
BEARBEITEN:
Zu diesem Zeitpunkt habe ich den Filter dazu gebracht, sich mit Grundbewegungsgeräuschen zu stabilisieren, aber ohne tatsächliche Bewegung. Sobald sich der Roboter zu bewegen beginnt, divergiert der Filter ziemlich schnell und verlässt die Karte.
quelle
Zunächst haben Sie nichts darüber erwähnt, welche Art von Lokalisierung Sie verwenden. Ist es mit bekannten oder unbekannten Korrespondenzen?
Um nun den Jacobianer in Matlab zu erwerben, können Sie Folgendes tun
Das Ergebnis
Für den erweiterten Kalman-Filter muss das System linear und das Rauschen Gaußsch sein. Das System bedeutet hier, dass die Bewegungs- und Beobachtungsmodelle linear sein müssen. Lasersensoren geben die Reichweite und den Winkel zu einer Landmarke vom Rahmen des Roboters an, sodass das Messmodell nicht linear ist. Über
P
, wenn man es groß sein , übernimmt dann wird Ihr EKF Schätzer am Anfang schlecht sein und stützt sich auf den Messungen , da der vorherige Zustandsvektor nicht verfügbar ist. Sobald sich Ihr Roboter jedoch zu bewegen und zu erfassen beginnt, wird EKF besser, da es die Bewegungs- und Messmodelle verwendet, um die Pose des Roboters zu schätzen. Wenn Ihr Roboter keine Orientierungspunkte erkennt, steigt die Unsicherheit drastisch an. Außerdem müssen Sie auf den Winkel achten. In C ++cos and sin
sollte der Winkel im Bogenmaß sein. Ihr Code enthält nichts zu diesem Problem. Wenn Sie während Ihrer Berechnung im Bogenmaß das Rauschen des Winkels in Grad annehmen, erhalten Sie möglicherweise seltsame Ergebnisse, da ein Grad als Rauschen, während die Berechnungen im Bogenmaß als riesig angesehen werden.quelle