Simultaneous Localization and Mapping
(SLAM)
Project for ECE 2008
Robotics and Automation
Submitted by
RAJA RISHYANT 16BEC0669
VIPIN S RAKARADDI 16BEC0653
HEMASAGAR RAJU 16BEC0514
To
Prof. VENUGOPAL P
B1 SLOT
VIT University, Vellore,
Tamilnadu – 632014
Simultaneous Localization and Mapping
ABSTRACT
Simultaneous localization and mapping (SLAM) consists in the concurrent
construction of a model of the environment (the map), and the estimation of the
state of the robot moving within it. It surveys the current state of SLAM and
considers future directions covering a broad set of topics including robustness
and scalability in long-term mapping, metric and semantic representations for
mapping, theoretical performance guarantees, active SLAM and exploration,
and other new frontiers.
INTRODUCTION
SLAM comprises the simultaneous estimation of the state of a robot equipped
with on-board sensors and the construction of a model (the map) of the
environment that the sensors are perceived. In simple instances, the robot state
is described by its pose (position and orientation), although other quantities may
be included in the state, such as robot velocity, sensor biases, and calibration
parameters. The map, on the other hand, is a representation of aspects of interest
(e.g., position of landmarks, obstacles) describing the environment in which the
robot operates.
We are entering in a third era for SLAM, the robust-perception age, which is
characterized by the following key requirements:
1) Robust Performance: the SLAM system operates with low failure rate for
an extended period of time in a broad set of environments; the system includes
failsafe mechanisms and has self-tuning capabilities in that it can adapt the
selection of the system parameters to the scenario.
2) High-Level Understanding: the SLAM system goes beyond basic
geometry reconstruction to obtain a high-level understanding of the
environment (e.g., high-level geometry, semantics, physics, and affordances).
3) Resource Awareness: the SLAM system is tailored to the available
sensing and computational resources, and provides means to adjust the
computation load depending on the available resources.
4) Task-Driven Perception: the SLAM system is able to select relevant
perceptual information and filter out irrelevant sensor data, in order to support
the task, the robot has to perform; moreover, the SLAM system produces
adaptive map representations, whose complexity may vary depending on the
task at hand.
SLAM PROBLEM
A robot is exploring an unknown, static environment.
Given:
The robot’s controls
Observation of nearby features Estimate:
Map of features
Path of the robot
SLAM is a technique used to build up a map within an unknown
environment or a known environment while at the same time keeping
track of the current location.
For the SLAM process there were many algorithms which came along with the
development, among them there’s an algorithm where it became the base for the
approach of mapping technique.
Extended Kalman Filter (EKF)
An EKF (Extended Kalman Filter) is the heart of the SLAM process.
o It is responsible for updating where the robot thinks it is based on the
Landmarks (features).
o The EKF keeps track of an estimate of the uncertainty in the robots
position and also the uncertainty in these landmarks it has seen in the
environment.
Overview
It takes the laser input and updates it’s memory according to the
landmark passed/memorized.
Laser & Odometry data
• Laser data is the reading obtained from the scan
• The goal of the odometry data is to provide an approximate position of
the robot
• The difficult part about is the odometry data and the laser data is to get
the timing right.
Landmarks
• Landmarks are features which can easily be re-observed and
distinguished from the environment.
• These are used by the robot to find out where it is (to localize itself).
The key points about suitable Landmarks
o Landmarks should be easily re-observable. o Individual landmarks
should be distinguishable from each other. o Landmarks should be
plentiful in the environment.
o Landmarks should be stationary.
OUTPUT:
MATLAB algorithm:
def slam(data, N, num_landmarks, motion_noise, measurement_noise):
#set the dimension of the filter
dim = 2* (N + num_landmarks)
# make the constraint information matrix and vector
Omega = matrix()
[Link](dim, dim)
[Link][0][0] = 1.0
[Link][1][1] = 1.0
Xi = matrix()
[Link](dim, 1)
[Link][0][0] = world_size/2.0 [Link][0][0] =
world_size/2.0
# process the data
for k in
range(len(data)):
# n is the index of the robot pose in the matrix/vector
n=k*2
measurement = data[k][0]
motion = data[k][1]
# integrate the measurements
for i in range(len(measurement)):
# m is the index of the landmark coordinate in the matrix/vector
m = 2 * (N+ measurement[i][0])
# update the information matrix/vector based on the
measurement for b in range(2):
[Link][n+b][n+b] += 1.0 / measurement_noise
[Link][m+b][m+b] += 1.0 / measurement_noise
[Link][n+b][m+b] += -1.0 / measurement_noise
[Link][m+b][n+b] += -1.0 / measurement_noise
[Link][n+b][0] += -
measurement[i][1+b]/measurement_noise
[Link][m+b][0] += -
measurement[i][1+b]/measurement_noise
#update the information matrix/vector based on the robot motion for
b in range(4):
[Link][n+b][n+b] += 1.0/motion_noise
for b in range(2):
[Link][n+b ][n+b+2] += -1.0/motion_noise
[Link][n+b+2][n+b ] += -1.0/motion_noise
[Link][n+b ][0] += -motion[b]/motion_noise
[Link][n+b+2][0] += motion[b]/motion_noise
# compute best estimate mu
= [Link]() * Xi # return
the result return mu
Code:-
function EKFSLAM()
close all; clear
all;
disp('EKFSLAM sample program start!!')
time = 0; endtime = 60; global dt; dt =
0.1; nSteps = ceil((endtime - time)/dt);
[Link]=[];
[Link]=[]; [Link]=[];
[Link]=[]; result.z=[];
[Link]=[]; result.u=[];
% State Vector [x y yaw]' xEst=[0 0 0]';
global PoseSize;PoseSize=length(xEst);
global LMSize;LMSize=2; % True State
xTrue=xEst;
% Dead Reckoning State xd=xTrue;
% Covariance Matrix for predict
R=diag([0.2 0.2 toRadian(1)]).^2;
% Covariance Matrix for observation global Q;
Q=diag([10 toRadian(30)]).^2;%range[m], Angle[rad]
% Simulation parameter global
Qsigma
Qsigma=diag([0.1 toRadian(20)]).^2; global
Rsigma
Rsigma=diag([0.1 toRadian(1)]).^2;
LM=[0 15;
10 0;
15 20];
MAX_RANGE=20;
alpha=1;
PEst = eye(3); initP=eye(2)*1000;
%movcount=0;
tic;
% Main loop for i=1 :
nSteps time = time +
dt; % Input
u=doControl(time);
% Observation
[z,xTrue,xd,u]=Observation(xTrue, xd, u, LM, MAX_RANGE);
% ------ EKF SLAM --------
% Predict xEst =
f(xEst, u);
[G,Fx]=jacobF(xEst,
u);
PEst= G'*PEst*G + Fx'*R*Fx;
% Update for iz=1:length(z(:,1))
zl=CalcLMPosiFromZ(xEst,z(iz,:));
xAug=[xEst;zl];
PAug=[PEst zeros(length(xEst),LMSize); zeros(LMSize,length(xEst))
initP];
mdist=[]; for
il=1:GetnLM(xAug) if
il==GetnLM(xAug)
mdist=[mdist alpha]; else
lm=xAug(4+2*(il-1):5+2*(il-1));
[y,S,H]=CalcInnovation(lm,xAug,PAug,z(iz,1:2),il);
mdist=[mdist y'*inv(S)*y]; end end
[C,I]=min(mdist);
if I==GetnLM(xAug)
%disp('New LM')
xEst=xAug;
PEst=PAug; end
lm=xEst(4+2*(I-1):5+2*(I-1));
[y,S,H]=CalcInnovation(lm,xEst,PEst,z(iz,1:2),I); K
= PEst*H'*inv(S); xEst = xEst + K*y;
PEst = (eye(size(xEst,1)) - K*H)*PEst; end
xEst(3)=PI2PI(xEst(3));
%Simulation Result [Link]=[[Link];
time]; [Link]=[[Link]; xTrue'];
[Link]=[[Link]; xd'];
[Link]=[[Link];xEst(1:3)'];
result.u=[result.u; u'];
if rem(i,5)==0
Animation(result,xTrue,LM,z,xEst,zl);
end
end toc
DrawGraph(result,xEst,LM);
function [y,S,H]=CalcInnovation(lm,xEst,PEst,z,LMId) global
Q; delta=lm-xEst(1:2); q=delta'*delta;
zangle=atan2(delta(2),delta(1))-xEst(3);
zp=[sqrt(q) PI2PI(zangle)]; y=(z-zp)';
H=jacobH(q,delta,xEst,LMId);
S=H*PEst*H'+Q;
function n=GetnLM(xEst) n=(length(xEst)-3)/2;
function zl=CalcLMPosiFromZ(x,z) zl=x(1:2)+[z(1)*cos(x(3)+z(2));z(1)*sin(x(3)+z(2))];
function Animation(result,xTrue,LM,z,xEst,zl) hold off;
plot([Link](:,1),[Link](:,2),'.b');hold on;
plot(LM(:,1),LM(:,2),'pk','MarkerSize',10);hold on;
if~isempty(z) for iz=1:length(z(:,1))
ray=[xTrue(1:2)';z(iz,3:4)]; plot(ray(:,1),ray(:,2),'-
r');hold on; end end
for il=1:GetnLM(xEst);
plot(xEst(4+2*(il-1)),xEst(5+2*(il-1)),'.c');hold on; end
plot(zl(1,:),zl(2,:),'.b');hold on;
plot([Link](:,1),[Link](:,2),'.k');hold on;
plot([Link](:,1),[Link](:,2),'.r');hold on;
arrow=0.5; x=[Link](end,:);
quiver(x(1),x(2),arrow*cos(x(3)),arrow*sin(x(3)),'ok');hold on; axis
equal; grid on; drawnow;
function x = f(x, u)
% Motion Model
global dt; global
PoseSize; global
LMSize;
F = horzcat(eye(PoseSize),zeros(PoseSize,LMSize*GetnLM(x)));
B = [dt*cos(x(3)) 0 dt*sin(x(3)) 0
0 dt];
x= x+F'*B*u; x(3)=PI2PI(x(3));
function [G,Fx]=jacobF(x, u)
global dt; global PoseSize; global
LMSize;
Fx = horzcat(eye(PoseSize),zeros(PoseSize,LMSize*GetnLM(x)));
jF=[0 0 -dt*u(1)*sin(x(3))
0 0 dt*u(1)*cos(x(3))
0 0 0];
G=eye(length(x))+Fx'*jF*Fx;
function H=jacobH(q,delta,x,i) sq=sqrt(q);
G=[-sq*delta(1) -sq*delta(2) 0 sq*delta(1) sq*delta(2); delta(2) -
delta(1) -1 -delta(2) delta(1)];
G=G/q;
F=[eye(3) zeros(3,2*GetnLM(x));
zeros(2,3) zeros(2,2*(i-1)) eye(2) zeros(2,2*GetnLM(x)-2*i)];
H=G*F;
function u = doControl(time)
%Calc Input Parameter
T=10; % [sec]
% [V yawrate] V=1.0; %
[m/s] yawrate = 5; %
[deg/s]
u =[ V*(1-exp(-time/T)) toRadian(yawrate)*(1-exp(-time/T))]'; function [z, x, xd, u] =
Observation(x, xd, u, LM ,MAX_RANGE)
%Calc Observation from noise prameter
global Qsigma; global Rsigma;
x=f(x, u);% Ground Truth
u=u+Qsigma*randn(2,1);%add Process Noise
xd=f(xd, u);% Dead Reckoning %Simulate
Observation z=[];
for iz=1:length(LM(:,1))
yaw=zeros(3,1); yaw(3)=-x(3);
localLM=HomogeneousTransformation2D(LM(iz,:)-x(1:2)',yaw');
d=norm(localLM); if d<MAX_RANGE
noise=Rsigma*randn(2,1);
z=[z;[d+noise(1) PI2PI(atan2(localLM(2),localLM(1))+noise(2))
LM(iz,:)]];
end end
function DrawGraph(result,xEst,LM)
%Plot Result figure(1);
hold off;
x=[ [Link](:,1:2) [Link](:,1:2)]; set(gca,
'fontsize', 16, 'fontname', 'times'); plot(x(:,1), x(:,2),'-
b','linewidth', 4); hold on;
plot([Link](:,1), [Link](:,2),'-k','linewidth', 4); hold on;
plot(x(:,3), x(:,4),'-r','linewidth', 4); hold on;
plot(LM(:,1),LM(:,2),'pk','MarkerSize',10);hold on; for
il=1:GetnLM(xEst);
plot(xEst(4+2*(il-1)),xEst(5+2*(il-1)),'.g');hold on; end
title('EKF SLAM Result', 'fontsize', 16, 'fontname', 'times'); xlabel('X
(m)', 'fontsize', 16, 'fontname', 'times'); ylabel('Y (m)', 'fontsize', 16,
'fontname', 'times');
legend('Ground Truth','Dead Reckoning','EKF SLAM','True LM','Estimated LM'); grid
on; axis equal;
function angle=PI2PI(angle)
angle = mod(angle, 2*pi);
i = find(angle>pi); angle(i) =
angle(i) - 2*pi; i =
find(angle<-pi); angle(i) =
angle(i) + 2*pi;
function out = HomogeneousTransformation2D(in, base, mode)
Rot=[cos(base(3)) sin(base(3)); -sin(base(3)) cos(base(3))];
Nin=size(in);
baseMat=repmat(base(1:2),Nin(1),1);
if Nin(2)>=3
inxy=in(:,1:2);
inOther=in(:,3:end);
in=inxy; end
if nargin==2 || mode==0
out=baseMat+in*Rot; else
out=(baseMat+in)*Rot; end
if Nin(2)>=3 out=[out
inOther]; end
function radian = toRadian(degree) %
degree to radian radian = degree/180*pi;
function degree = toDegree(radian) %
radian to degree degree = radian/pi*180;
