Void’s Vault

Knowledge source for efficiency.

SLAM With EKF Filtering Algorithm

I recently implemented a SLAM simulation with the help of my book of Probabilistic Robotics. Code is detailed in this post.

We have a robot that will go through an unknown map with random landmarks. The simulation will draw the robot’s real position, the estimation of the pose and the estimated position of the discovered landmarks. The simulation will redraw itself every time step, so we can see a very cool animation of the robot’s movement with the manipulation over the landmarks’ covariances.

First, small drawing functions that are used in the simulation script:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
function plotCircle(x,y,r,symbol,n)
    if nargin < 5
        n = 3600;
    end
    ang = linspace(0, 2*pi, n);
    xp = r*cos(ang);
    yp = r*sin(ang);
    plot(x+xp, y+yp, symbol);
end

function plotError(center,C,symbol,n)
% center: 2x1 position vector
% C: 2x2 covariance matrix
% symbol: plot symbol, eg 'r'
% n: number of points
if nargin < 4
    n=100; % Number of points around ellipse
end
p=0:pi/n:2*pi; % angles around a circle
[eigvec,eigval] = eig(C); % Compute eigen-stuff
xy = [cos(p'),sin(p')] * sqrt(eigval) * eigvec'; % Transformation
x = center(1) + xy(:,1);
y = center(2) + xy(:,2);
plot(x, y, symbol)

Then, the script that runs the algorithm. I’m sorry, since it was a work that I had to do for a research project, I was asked to comment this part in french. I hope that google will help to translate what I wrote!

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
clearvars;
% Description du programme :
%   Le script lance une simulation d'un robot qui va avancer dans une forme
%   de boucle dans un environnement inconnu rempli de landmarks
%   (Landmarks_Count indique le nombre total se trouvant dans la carte).
%   L'objectif est d'utiliser EKF_SLAM afin de faire du SLAM. Les
%   graphiques qui sont dessinés à chaque incréments de temps sont décrits
%   par ce qui suit :
%       La position réelle du robot est en bleue. 
%       L'estimation de la position du robot est en rouge.
%       Chaque fois qu'un landmark différent est trouvé, sa position est
%       affichée par un x rouge (position réelle en noir)
%       Les incertitudes sur les positions sont des ellipses rouges.
%       Un x cyan indique une mesure détectée à partir de la position
%       réelle du robot. les + verts représentent ces mesures mais
%       affichées par rapport à l'estimé de position du robot.
%   
%   Lorsque le robot va croiser un chemin qu'il a déjà parcouru, s'il y
%   détecte un landmark déjà rencontré, les erreurs des autres landmarks
%   seront ajustés rétroactivment, puisque l'incertitude sur la position du
%   robot se sera grandement réajustée.
% 
% Problèmes connus et explications :
%   En fonction du Threshold &quot;alpha&quot;, il arrive qu'un landmark déjà détecté
%   soit confondu pour un nouveau landmark. Étant donnée l'erreur sur la position
%   et sur la mesure bruitée, il arrive que l'erreur total fasse en sorte
%   que le threshold soit trop bas pour que soit reconnu un landmark. Si
%   cela arrive trop souvent, il suffit d'augmenter la valeur du threshold.
%   
%   Si le robot ne redétecte aucun landmark déjà découvert (la carte étant
%   générée aléatoirement, cela peut arriver) l'erreur sur la position
%   augmentera sans cesse. C'est tout à fait normal.
%   
%   En fonction du Threshold &quot;alpha&quot;, il arrive qu'un nouveau landmark ou
%   un landmark qui est déjà connu mais avec une signature proche d'un
%   autre landmark soit confondu avec un autre landmark déjà connu. Cela
%   peut arriver si deux landmarks de signature proche (s = 2 et s = 3 par
%   exemple, peuvent être confondu si l'erreur sur la signature est grande) 
%   et de distance relativement courte existent. Dans cette
%   situation, il peut arriver que l'erreur sur la position et sur la
%   mesure suffise au filtre EKF pour que soient confondus une mesure afin
%   qu'elle soit considérée comme une mesure du mauvais landmarks. Cela va
%   provoquer un repositionnement rapide du robot à un endroit à bonne
%   distance ou à fort angle de sa position réelle. Si cela arrive,
%   l'algorithme d'EKF slam réussit difficilement à retrouver sa position
%   réelle, car les filtres de kalman sont peut résilients au problème du
%   kidnapping. Une solution ici serait de réduire le threshold &quot;alpha&quot;
%   afin de rendre plus difficile la confusion. Une autre solution serait
%   d'utiliser une erreur sur le positionnement ou sur la mesure plus
%   faible. Enfin, une dernière solution est de distancer davantage les
%   landmarks. Ici, c'est souvent le principal problème. En effet, étant
%   donné que nos landmarks sont générés aléatoirement, ceux-ci sont
%   souvent un peu trop près les uns des autres, ce qui peut engender ce
%   type de problème.
%
% paramètres de l'environnement
max_range = 2;
map_width = 20;
map_height = 20;
Landmarks_Count = 40; %Nombre de landmarks dans la carte.
N = 0; %Nombre de landmarks détectés
dt = 1; %Intervalle de temps
alpha = 7.5; %Threshold pour la création d'un nouveau landmark

x = map_width*rand(1,Landmarks_Count);
y = map_height*rand(1,Landmarks_Count) - map_height/2;

% environnement
features = [x; y];
robotPos = [0 0 0]';
mu1 = robotPos;
sigma1 = [0.0001 0 0;0 0.0001 0; 0 0 0.0001]; %La position de départ est certaine

% bruits
sV = 0.05; %Bruit sur la vitesse translationnelle réelle du robot
sW = 0.1*pi/180; %Bruit sur la vitesse angulaire réelle du robot
sRange = 0.2; %Bruit sur la mesur de la distance d'un landmark
sAngle = 10.0*pi/180; %Bruit sur la mesure de l'angle d'un landmark (bearing)
sSignature = 0.4; %Bruit sur la signature
%Matrices de bruits utilisées par EKF_SLAM
R = [0.03 0 0;
     0 0.03 0;
     0 0 0.005]; % Covariance du bruit sur le mouvement [x,y,theta]
Q = [0.3 0 0;
     0 1.0 0;
     0 0 0.4]; % Covariance du bruit sur las mesures [range;bearing;signature]

U = [0.5*ones(1,95) ; ...
     0.001*ones(1,31) 10*pi/180*ones(1,18) 0.001*ones(1,10) 5*pi/180*ones(1,18) 0.001*ones(1,18)];

for cmd = 1:size(U,2)
    display(sprintf('Iteration %d',cmd));
    % déplacecment du robot
    v = U(1,cmd) + sV*randn;
    w = U(2,cmd) + sW*randn;
    robotPos(1) = robotPos(1) + -v/w*sin(robotPos(3)) + v/w*sin(wrapToPi(robotPos(3)+w*dt));
    robotPos(2) = robotPos(2) + v/w*cos(robotPos(3)) - v/w*cos(wrapToPi(robotPos(3)+w*dt));
    robotPos(3) = wrapToPi(robotPos(3) + w*dt);

    % affichage du robot
    figure(1)
    clf
    axis([0 map_width -map_height/2 map_height/2])
    hold on
    plot(features(1,:), features(2,:), '.k')
    plot(robotPos(1), robotPos(2), 'bo')
    line([robotPos(1) robotPos(1)+0.5*cos(robotPos(3))], [robotPos(2) robotPos(2)+0.5*sin(robotPos(3))], 'color', 'b')
    plotCircle(robotPos(1), robotPos(2), max_range, 'k')

    % mesures prises par le robot
    relativePos = features - repmat(robotPos(1:2,:),1,Landmarks_Count);
    ranges = sqrt(sum(relativePos.^2,1));
    % conserve uniquement les features visibles selon portée du scanner
    visible = find(ranges <= max_range);
    measures = relativePos(:,visible);
    ranges = ranges(visible);
    angles = wrapToPi(atan2(measures(2,:), measures(1,:)) - robotPos(3));
    % ajout de bruit sur les mesures
    ranges = ranges + sRange*randn(1,numel(ranges));
    angles = wrapToPi(angles + sAngle*randn(1,numel(angles)));
    if numel(visible) > 0
        visible = visible + sSignature*randn(1, numel(visible));
    end

    % paramètres à envoyer dans EKF_SLAM
    u = U(:,cmd);
    mu0 = mu1;
    sigma0 = sigma1;
    z = [ranges;angles;visible];

    % Affichage des mesures dans le repere local selon ground truth
    for indexZ = 1:size(ranges,2)
        plot(ranges(indexZ)*cos(wrapToPi(angles(indexZ)+robotPos(3,1)))+robotPos(1,1),ranges(indexZ)*sin(wrapToPi(angles(indexZ)+robotPos(3,1)))+robotPos(2,1),'cx');
    end

    [mu1, sigma1, N] = EKF_SLAM(mu0, sigma0, u, z, N, alpha, R, Q, dt);  % utilise les valeurs de ranges, angles et u

    % Affichage des mesures dans le repere local selon mu1
    for indexZ = 1:size(ranges,2)
        plot(ranges(indexZ)*cos(wrapToPi(angles(indexZ)+mu1(3,1)))+mu1(1,1),ranges(indexZ)*sin(wrapToPi(angles(indexZ)+mu1(3,1)))+mu1(2,1),'g+');
    end

    %Estimation de la position du robot par EKF_SLAM
    plot(mu1(1,1), mu1(2,1), 'ro')
    line([mu1(1,1) mu1(1,1)+0.5*cos(mu1(3,1))], [mu1(2,1) mu1(2,1)+0.5*sin(mu1(3,1))], 'color', 'r')
    C = sigma1(1:2,1:2);
    plotError([mu1(1,1) mu1(2,1)], C, 'r', 50)
    %Estimation des positions des landmarks par EKF_SLAM
    for i = 1:N
        x = mu1(1,i+1);
        y = mu1(2,i+1);
        C = sigma1(3*i+1:3*i+2, 3*i+1:3*i+2);
        plot(x, y, 'rx')
        plotError([x y], C, 'r', 50)
        htext = text(x+0.13,y,sprintf('%d',i));
        set(htext,'Color','r');
    end
    pause(0.05);
end

And now, the lines you were expecting : The EKF_SLAM algorithm. I hope this is clear. If you have anything to say, please contact me!

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
function [ mu1, sigma1, N ] = EKF_SLAM( mu0, sigma0, u, z1, N, alpha, R, Q, dt)
% EKF SLAM algorithm, as shown in Table 10.2.
% Usage: [mu1,sigma1,N] = EKF_SLAM( mu0, sigma0, u, z1, N, alpha)
% 
% arguments: (input)
%  mu0 -    matrix of the last pose and of the known positions of the
%           landmarks. The first column contains the pose [x,y,theta]', and 
%           each other column contains the values of the landmarks
%           [range,bearing,signature]'
% 
%  sigma0 - last pose's error and error on the landmarks' positions
% 
%  u    -   new command [v,w,theta]'
% 
%  z1   -   new landmarks measurements. Each column is [range,bearing,signature]
% 
%  N    -   Number of landmarks known in the last timestep. integer
%
%  alpha -  The threshold for the creation of a new landmark
% 
%  R     -  Covariance(3x3 matrix) of the additional motion noise [x,y,theta]
% 
%  Q     -  Covariance(3x3 matrix) of the additional measurement noise [range;bearing;signature]
% 
%  dt    -  Time step
% 
% arguments: (output)
%  mu1    - new matrix of the pose and the landmarks' positions. same form as mu0
% 
%  sigma1 - the new pose's error and landmarks' errors
%
%  N    -   Number of landmarks known in the current timestep. integer
% 
% Author: Olivier Dugas
% Release: 1.0
% Release date : 11/16/2012

% Calculation of the Jacobians
F = [eye(3) zeros(3,3*N)];  % State Transition Matrix

%Motion update
%new pose estimation
Fprod = F'*[-u(1)/u(2)*sin(mu0(3,1)) + u(1)/u(2)*sin(wrapToPi(mu0(3,1)+u(2)*dt)); ...
                 u(1)/u(2)*cos(mu0(3,1)) - u(1)/u(2)*cos(wrapToPi(mu0(3,1)+u(2)*dt)); ...
                 u(2)*dt ];
for col=1:numel(mu0(1,:))
    mu1_(:,col) = mu0(:,col) + Fprod((col-1)*3 +1:(col)*3);
end
mu1_(3,1) = wrapToPi(mu1_(3,1));

%Error propagation
g = [0, 0, -u(1)/u(2)*cos(mu0(3,1)) + u(1)/u(2)*cos(wrapToPi(mu0(3,1)+u(2)*dt)); ...
     0, 0, -u(1)/u(2)*sin(mu0(3,1)) + u(1)/u(2)*sin(wrapToPi(mu0(3,1)+u(2)*dt)); ...
     0, 0,  0];
G = eye(3*N+3) + F'*g*F;
sigma1_ = G*sigma0*G' + F'*R*F;

%Next loop is the measurement update
for i=1:(numel(z1)/3) %For all observed features
    %definition of the feature [range, bearing, signature]
    r=z1(1,i);
    b=z1(2,i);
    s=z1(3,i);
    feature = [r;b;s];
    %Hypothesis of a new landmark
    mu1_(:,1+N+1) = [mu1_(1,1);mu1_(2,1);s] + r*[cos(wrapToPi(b+mu1_(3,1)));
                                               sin(wrapToPi(b+mu1_(3,1)));
                                               0];
    temp = sigma1_;
    [m,n] = size(sigma1_);
    sigma1_ = eye(3*(N+2)).*999999;
    sigma1_(1:m, 1:n) = temp;

    %initialization of variables that will be updated inside the next loop
    zhat = zeros(3,N+1);
    H = zeros(3,3+3*(N+1),N+1);
    psi = zeros(3,3, N+1);
    pik = zeros(1,N+1); %Maximum likelihood correspondences
    %Loop through all known landmarks to compute various update quantities
    for k=2:1+N+1
        dx = mu1_(1,k)-mu1_(1,1);
        dy = mu1_(2,k)-mu1_(2,1);
        delta = [dx; dy];
        q = delta'*delta;

        zhat(:,k-1) = [sqrt(q);
                       wrapToPi(atan2(dy,dx)-mu1_(3,1));
                       mu1_(3,k)];

        Fk = [F zeros(3, (3*(N+2))-(numel(F(1,:))));
              zeros(3) zeros(3, 3*(k-2)) eye(3) zeros(3,3*((N+1)-(k-1)))];

        h = [-sqrt(q)*dx, -sqrt(q)*dy, 0, sqrt(q)*dx, sqrt(q)*dy, 0;
             dy,           -dx,       -q,    -dy,         dx,     0;
              0,            0,         0,      0,          0,     q];
        H(:,:,k-1) = (1/q)*h*Fk;

       %Psi is stored in it's inverted form, because only the inverted form is used.
        psi(:,:,k-1) = inv(H(:,:,k-1)*sigma1_*H(:,:,k-1)' + Q);

        error = feature-zhat(:,k-1);
        error(2) = wrapToPi(error(2));
        pik(k-1) = error' * psi(:,:,k-1) * error;
    end

    %Threshold for the creation of a new landmark
    pik(N+1) = alpha;

    %Maximum likelihood estimation
    min = Inf;
    j = 0;
    for k=1:N+1
        if pik(k) <= min
            j = k;
            min = pik(k);
        end
    end

    %Decide if the hypothesis of a new landmark is valid or not.
    %If landmark is accepted as a new one, increment N
    N = max(N,j);

    %Creation of the Kalman gain for the measurement update of the current feature
    K = sigma1_ * H(:,:,j)' * psi(:,:,j);

    %Update of the poses and their errors
    error = feature-zhat(:,j);
    error(2) = wrapToPi(error(2));
    %mu1_ = mu1_ + K * error;
    K_err_product = K * error;
    for col=1:numel(mu1_(1,:))
        mu1_(:,col) = mu1_(:,col) + K_err_product((col-1)*3 +1:(col)*3);
    end
    mu1_(3,1) = wrapToPi(mu1_(3,1));

    sigma1_ = (eye(numel(sigma1_(:,1)))-K*H(:,:,j))*sigma1_;
end

mu1 = mu1_(:,1:N+1);
sigma1 = sigma1_(1:3*N+3, 1:3*N+3);
end

That’s it! Enjoy! Also, refer to Probabilistic Robotics from Thrun, Burgard and Fox, chapter 10, for details about the meaning of every variables I used. They are (not all, but most) greatly described. You will also find mathematical derivation of the algorithm.