SLAM = Simultaneous Localization and Mapping

Projet Robotique

ENSTA Robotique

Filtre de Kalman

Deux moyens de se repérer

I.Principe

II.Programmation

avec
Matlab

Les étapes

Les matrices

X* = [x +xu; y + yu,...] on rajoute l'odométrie

X: position absolue [x; y; xa1; ya1; ...]

 carte et position du robot

Y: coordonnées des amers Y[x1; y1; x2; y2;...]

issues de l'observation

 Y*: relatif [xa1 - x, ya1 - y,..., yan - y]

Les matrices (suite)

P: Matrice de covariance des amers

 Q: Matrice de covariance de l'odométrie

Mesure de l'incertitude

Erreur proportionnelle à la distance

Les données

percep : 2.610054 3.587491 1.731687 7.627319 5.969606 8.740084 
odom : 0.242237 2.635034
percep : 2.059525 1.537290 1.102366 5.364240 5.407859 6.277271
odom : 0.512546 2.822456
percep : 1.397940 -1.399142 0.491180 2.388502 4.589120 3.410499 
odom : 1.155917 1.797266
percep : -0.042466 -2.965487 -0.977701 0.817747 2.995906 1.842215 
odom : 2.034563 1.747939

Observations avec nombre d'amers fixe

id = fopen('FullObservation.data');
textscan(id,'percep : ');
Y = textscan(id,'%f');
Y = cell2mat(Y)

[X,P,A,B,H] = initialisation(Y);

graph(X,P)

a = 0.1 %facteur de proportionnalité
Py = Matrice_covariance(Y,a);

Initialisation

Traitements des nouvelles données

%On parcourt le fichier texte
while ~feof(id); 
    %Partie qui traite l'odométrie
    textscan(id,'odom : '); 
    u = textscan(id,'%f');
    u = cell2mat(u);
    Q = Covariance_odometrie(u,a);
    [Xet,Pet] = prediction(X,u,A,B,P,Q);
    
    graph(Xet,Pet)
    hold on
    
    Yet = H*Xet;
    
    textscan(id,'percep : ');
    Y = textscan(id,'%f');
    Y = cell2mat(Y);
    Py = Matrice_covariance(Y,a);
    
    [X,P] = correction_etat( Xet,Yet,Pet,Y,H,Py);
    
    graph(X,P)
end

Difficultés rencontrées

Lecture du fichier

Créer les matrices à la bonne taille

Observations partielles: taille de matrice variable

III.Résultat

Observation Partielle

Nombre Fixe