Filtre Kalman étendu avec balayage laser + carte connue

Je travaille actuellement sur un projet pour l'école où j'ai besoin d'implémenter un filtre de Kalman étendu pour un robot ponctuel avec un scanner laser. Le robot peut tourner avec un rayon de braquage de 0 degré et avancer. Tous les mouvements sont linéaires par morceaux (entraînement, rotation, entraînement).

Le simulateur que nous utilisons ne prend pas en charge l'accélération, tout mouvement est instantané.

Nous avons également une carte connue (image png) que nous devons localiser. Nous pouvons ray tracer dans l'image afin de simuler des balayages laser.

Mon partenaire et moi sommes peu confus quant aux modèles de mouvement et de capteur que nous devrons utiliser.

Jusqu'à présent, nous modélisons l'état comme un vecteur .$(x,y,\theta)$

Nous utilisons les équations de mise à jour comme suit

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;
}

Nous pensions que tout fonctionnait jusqu'à ce que nous remarquions que nous avons oublié d'initialiser Pet que c'était zéro, ce qui signifie qu'aucune correction ne s'est produite. Apparemment, notre propagation était très précise car nous n'avons pas encore introduit de bruit dans le système.

Pour le modèle de mouvement, nous utilisons la matrice suivante pour F:

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

Comme son jacobien de nos formules de mise à jour. Est-ce correct?

$x$$y$$\theta$

L'autre problème était d'avoir comment initialiser P. Nous avons essayé 1,10,100 et ils placent tous le robot en dehors de la carte à (-90, -70) lorsque la carte n'est que de 50x50.

Le code de notre projet se trouve ici: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Tout conseil est grandement appréciée.

ÉDITER:

À ce stade, le filtre s'est stabilisé avec un bruit de mouvement de base, mais aucun mouvement réel. Dès que le robot commence à bouger, le filtre diverge assez rapidement et quitte la carte.

• L'utilisation d'EKF pour la localisation basée sur des balayages laser et une carte connue est une mauvaise idée et ne fonctionnera pas car l'une des principales hypothèses d'EKF n'est pas valide: le modèle de mesure n'est pas différenciable.

Je suggérerais d'étudier la localisation de Monte Carlo (filtres à particules). Cela résoudra non seulement le problème avec votre modèle de mesure, mais permettra également une localisation globale (pose initiale dans la carte complètement inconnue).

Edit: je suis désolé d'avoir oublié de mentionner un autre point important:

• $z_t = h(x_t) + N(0, Q_t)$$h(x_t)$ partie et en ajoutant du bruit gaussien.

Tout d'abord, vous n'avez rien dit sur le type de localisation que vous utilisez. Est-ce avec des correspondances connues ou inconnues?

Maintenant, pour acquérir le jacobien dans Matlab, vous pouvez faire ce qui suit

>> 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)

Le résultat

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

Le filtre de Kalman étendu nécessite que le système soit linéaire et que le bruit soit gaussien. Le système signifie ici que les modèles de mouvement et d'observation doivent être linéaires. Les capteurs laser donnent la portée et l'angle vers un point de repère à partir du cadre du robot, de sorte que le modèle de mesure n'est pas linéaire. À propos P, si vous supposez qu'il est grand, votre estimateur EKF sera médiocre au début et il s'appuie sur les mesures car le vecteur d'état précédent n'est pas disponible. Cependant, une fois que votre robot commencera à se déplacer et à détecter, l'EKF s'améliorera car il utilise les modèles de mouvement et de mesure pour estimer la pose du robot. Si votre robot ne détecte aucun point de repère, l'incertitude augmente considérablement. En outre, vous devez faire attention à l'angle. En C ++,cos and sin, l'angle doit être en radian. Il n'y a rien dans votre code concernant ce problème. Si vous supposez le bruit de l'angle en degrés lors de votre calcul en radian, vous pourriez obtenir des résultats étranges car un degré de bruit alors que les calculs en radian sont considérés comme énormes.