Erweiterter Kalman-Filter mit Laserscan + bekannter Karte

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 .$(x,y,\theta)$

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 Pund 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:

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

Da es der Jacobian unserer Update-Formeln ist. Ist das richtig?

$x$$y$$\theta$

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.

• Die Verwendung von EKF zur Lokalisierung basierend auf Laserscans und einer bekannten Karte ist eine schlechte Idee und funktioniert nicht, da eine der Hauptannahmen von EKF nicht gültig ist: Das Messmodell ist nicht differenzierbar.

Ich würde vorschlagen, sich mit der Monte-Carlo-Lokalisierung (Partikelfilter) zu befassen. Dies löst nicht nur das Problem mit Ihrem Messmodell, sondern ermöglicht auch eine globale Lokalisierung (anfängliche Pose innerhalb der Karte völlig unbekannt).

Edit: Es tut mir leid, dass ich vergessen habe, einen weiteren wichtigen Punkt zu erwähnen:

• $z_t = h(x_t) + N(0, Q_t)$$h(x_t)$ Teil und Hinzufügen von Gaußschem Rauschen.

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

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

Das Ergebnis

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

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 sinsollte 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.