EKF SLAM example for a 2D mobile robot

EKF_SLAM.m

function [] = ToyEKFSLAM()
randn('seed',1);
DTR = pi/180;   % degree to radian

%% True initial condition
% The size is `mu_gt` is 4 + 10 = 14
mu_gt = [1; 1; 45*DTR; 1;   % x, y, psi, V
     4.6454;  7.0242;   % landmark 1 (x1,y1)
     6.5198;  4.7523;   % landmark 2 (x2,y2)
     1.6836;  5.4618;   % landmark 3 (x3,y3)
     5.5984;  3.1042;   % landmark 4 (x4,y4)
     7.5626;  1.6814];  % landmark 5 (x5,y5)

% Initial error covariance
sigma = eye(14);
sigma(1,1) = 0;
sigma(2,2) = 0;

%% Set process and measurements noises
% Process noise
Re = zeros(14,14);

% Measurement noise
YAW_NOISE = 0.003; % It indicates variance (yaw is radian)
VEL_NOISE = 0.001; % It indicates variance

%STD: 1 sigma implies 68.27% of data are within the boundary
% Note that setting appropriate stdev. is important
% If bearing/range stdev becomes smaller 
% (i.e. measurements are more reliable)
% -> Then, the x and y of the robot may become imprecise
% If bearing/range stdev becomes larger
% (i.e. measurements are less reliable)
% -> It was shown that the x and y of the robot are more accurate
% Thus, balancing the ratio of covariances is important 
% depends on your situations
BEARING_STD = 10 * DTR; % Unit: rad
RANGE_STD = 0.15; % Unit: m
BEARING_NOISE = BEARING_STD  * BEARING_STD; % It indicates variance
RANGE_NOISE = RANGE_STD * RANGE_STD; % It indicates variance

b_message = sprintf('BEARING NOISE: %d', BEARING_NOISE);
r_message = sprintf('RANGE NOISE: %d', RANGE_NOISE);

disp(b_message);
disp(r_message);

Qe = eye(12);
Qe(1,1) = YAW_NOISE;
Qe(2,2) = VEL_NOISE;
for i=1:5
    % NOTE: (i * 2 + 1) and (i * 2 + 2)-th elements in the state
    % denote x and y values, respectively.
    % But measurements are (r, theta) coordinate; thus, its measurement
    % noises should not be set as following: 
    %Qe(i*2+1,i*2+1) = BEARING_NOISE;
    % Qe(i*2+2,i*2+2) = RANGE_NOISE;
    
    % Assume that the uncertainty is isotropic
    Qe(i*2+1,i*2+1) = max(BEARING_NOISE, RANGE_NOISE);
    Qe(i*2+2,i*2+2) = max(BEARING_NOISE, RANGE_NOISE);
end

%% Initial estimates
mu = mu_gt;
mu(5:14) = mu_gt(5:14) + sqrt(sigma(5:14,5:14)) * randn(10,1);

figure;
set(gcf,'position',[100 100 500 500])

%% Iterate Extended Kalman Filter
dt = 0.1;   
video = VideoWriter('ekf_slam.avi', 'Motion JPEG AVI');
video.Quality = 100;
open(video)
for t=0:dt:10
    % Move a robot: the robot just translates
    mu_gt = propagate_state(mu_gt, dt);  
    z = generate_pseudo_measurements_w_noises(mu_gt, BEARING_STD, RANGE_STD);     
    
    [mu_bar, sigma_bar] = predict(mu, sigma, Re, dt);  
    [mu, sigma] = update(mu_bar, z, sigma_bar, Qe, dt);  
    % -------------------------------------------------------------------------    
    % For visualization
    % -------------------------------------------------------------------------    
    clf
    show_data(t, mu_gt, mu, sigma);
    drawnow;
    frame = getframe(gcf);
    writeVideo(video,frame);
end
close(video)

function x = propagate_state(x,dt)
    xdot = zeros(14,1);
    xdot(1) = x(4)*cos(x(3));   % xdot
    xdot(2) = x(4)*sin(x(3));   % ydot
    xdot(3) = 0;    % yaw rate
    xdot(4) = 0;    % speed
    x = x + xdot*dt;    % time integration (Euler scheme)

function z = generate_pseudo_measurements(x)
z = zeros(12,1);
x_curr = x(1);  y_curr = x(2);  yaw_curr = x(3);
z(1:2) = x(3:4); 
for i=5:2:length(x)
    x_landmark = x(i); y_landmark = x(i+1);
    
    bearing_angle_measured = atan2(y_landmark - y_curr, x_landmark - x_curr) - yaw_curr;
    range_measured = sqrt((x_landmark - x_curr)^2 + (y_landmark - y_curr)^2); 
    
    z(i-2) = bearing_angle_measured; 
    z(i-1) = range_measured;
end

function z = generate_pseudo_measurements_w_noises(x, bearing_noise_std, range_noise_std)
z = zeros(12,1);
x_curr = x(1);  y_curr = x(2);  yaw_curr = x(3);
z(1:2) = x(3:4); 
for i=5:2:length(x)
    x_landmark = x(i); y_landmark = x(i+1);
    
    bearing_angle_measured = atan2(y_landmark - y_curr, x_landmark - x_curr) - yaw_curr;
    range_measured = sqrt((x_landmark - x_curr)^2 + (y_landmark - y_curr)^2); 
    
    z(i-2) = bearing_angle_measured + randn(1) * bearing_noise_std; 
    z(i-1) = range_measured + randn(1) * range_noise_std;
end

function [mu_bar, sigma_bar] = predict(mu_prev, sigma_prev, Re, dt)
% State
yaw_prev = mu_prev(3);  
vel_prev = mu_prev(4);
jacobian_of_motion = zeros(14);
jacobian_of_motion(1:2, 3:4) = [-vel_prev*sin(yaw_prev) cos(yaw_prev);
               vel_prev*cos(yaw_prev) sin(yaw_prev)];
G = eye(14) + jacobian_of_motion * dt;
% Step 2
mu_bar = propagate_state(mu_prev, dt);

R = Re * dt;
% Step 3
sigma_bar = G * sigma_prev * G.' + R;
  
function [mu, sigma] = update(mu_bar, z, sigma_bar, Qe, dt)
Q = Qe * dt;
H = calc_jacobian(mu_bar);
% Step 4
K = sigma_bar * H.' / (H * sigma_bar * H.' + Q);
% Step 5
mu = mu_bar + K*(z - generate_pseudo_measurements(mu_bar));
% Step 6
sigma = (eye(14) - K * H) * sigma_bar;

function [H] = calc_jacobian(xhat)
H = zeros(12, 14);
H(1, 3) = 1; H(2, 4) = 1;

x_curr = xhat(1); y_curr = xhat(2);

for i=5:2:length(xhat)
    x_landmark = xhat(i); y_landmark = xhat(i+1);
    
    % About dbeta/d{$PARAM}
    H(i-2, 1) = (y_landmark - y_curr) / ( (x_landmark - x_curr)^2 + (y_landmark - y_curr)^2 );
    H(i-2, 2) = (-1) * (x_landmark - x_curr)  / ( (x_landmark - x_curr)^2 + (y_landmark - y_curr)^2  );
    H(i-2, i) = -H(i-2, 1);
    H(i-2, i+1) = -H(i-2, 2);
    H(i-2, 3) = -1;
    
    % About droh/d{$PARAM}
    H(i-1, 1) = -(x_landmark - x_curr) / sqrt( (x_landmark - x_curr)^2 + (y_landmark - y_curr)^2 );
    H(i-1, 2) = -(y_landmark - y_curr) / sqrt( (x_landmark - x_curr)^2 + (y_landmark - y_curr)^2 );
    H(i-1, i) = -H(i-1, 1);
    H(i-1, i+1) = -H(i-1, 2);
    
end

% show data
%------------------------------------------------------------
function show_data(t,x,xhat,Phat)
hold on

L = 0.6; % vehicle length
B = 0.3;	% vehicle breadth
objx = L*[1 -1 -1 1]';
objy = B*[0,1,-1,0]';

posx = x(1) + objx*cos(x(3))-objy*sin(x(3));
posy = x(2) + objx*sin(x(3))+objy*cos(x(3));
plot(posx, posy, 'black');

posx = xhat(1) + objx*cos(xhat(3))-objy*sin(xhat(3));
posy = xhat(2) + objx*sin(xhat(3))+objy*cos(xhat(3));
plot(posx,posy, 'r', 'LineWidth', 1);

plot(0,0,'bo',x(1),x(2),'bo',xhat(1),xhat(2),'r+');
meanval = [xhat(1),xhat(2)];
sigma_u = Phat(1:2,1:2);

confidence = 0.95;  % confidence level
alpha = chi2inv(confidence,2);  
plot2Dellipse(alpha,meanval,sigma_u,'r');

for i=1:5
    plot(xhat(i*2+3),xhat(i*2+4),'r+',x(i*2+3),x(i*2+4),'k+');    
    meanval = [xhat(i*2+3),xhat(i*2+4)];
    sigma_u = Phat(i*2+3:i*2+4,i*2+3:i*2+4);
    plot2Dellipse(alpha,meanval,sigma_u,'r');
end

s = sprintf('Time = %8.2f',t);
text(1,9,s,'fontsize',15);
xlabel('x','fontsize',15), ylabel('y','fontsize',15);
set(gca,'fontsize',15,'PlotBoxAspectRatio',[500 500 100]);
axis([0 10 0 10]); 
grid on; box on;

% draw a confidence ellipse
%------------------------------------------------------------
function [] = plot2Dellipse(alpha,meanval,sigma,color)
[V,D] = eig(sigma);

a = sqrt(alpha*D(1,1));
b = sqrt(alpha*D(2,2));

ang1 = atan2(V(2,1),V(1,1));

theta = (0:360)/180*pi;
x = a*cos(theta);
y = b*sin(theta);
xn = x*cos(ang1)-y*sin(ang1)+meanval(1);
yn = x*sin(ang1)+y*cos(ang1)+meanval(2);

plot(xn,yn,color,'LineWidth',2);