✨ Completion of assignment 8 of SLAM Course

Topic: Fast SLAM

s8_fastslam/data/world.dat

@@ -0,0 +1,9 @@
+1 2 1
+2 0 4
+3 2 7
+4 9 2
+5 10 5
+6 9 8
+7 5 5
+8 5 3
+9 5 9

s8_fastslam/octave/correction_step.m

@@ -0,0 +1,65 @@
+function particles = correction_step(particles, z)
+
+% Weight the particles according to the current map of the particle
+% and the landmark observations z.
+% z: struct array containing the landmark observations.
+% Each observation z(j) has an id z(j).id, a range z(j).range, and a bearing z(j).bearing
+% The vector observedLandmarks indicates which landmarks have been observed
+% at some point by the robot.
+
+% Number of particles
+numParticles = length(particles);
+
+% Number of measurements in this time step
+m = size(z, 2);
+
+% TODO: Construct the sensor noise matrix Q_t (2 x 2)
+
+% process each particle
+for i = 1:numParticles
+ robot = particles(i).pose;
+ % process each measurement
+ for j = 1:m
+ % Get the id of the landmark corresponding to the j-th observation
+ % particles(i).landmarks(l) is the EKF for this landmark
+ l = z(j).id;
+
+ % The (2x2) EKF of the landmark is given by
+ % its mean particles(i).landmarks(l).mu
+ % and by its covariance particles(i).landmarks(l).sigma
+
+ % If the landmark is observed for the first time:
+ if (particles(i).landmarks(l).observed == false)
+
+ % TODO: Initialize its position based on the measurement and the current robot pose:
+
+ % get the Jacobian with respect to the landmark position
+ [h, H] = measurement_model(particles(i), z(j));
+
+ % TODO: initialize the EKF for this landmark
+
+ % Indicate that this landmark has been observed
+ particles(i).landmarks(l).observed = true;
+
+ else
+
+ % get the expected measurement
+ [expectedZ, H] = measurement_model(particles(i), z(j));
+
+ % TODO: compute the measurement covariance
+
+ % TODO: calculate the Kalman gain
+
+ % TODO: compute the error between the z and expectedZ (remember to normalize the angle)
+
+ % TODO: update the mean and covariance of the EKF for this landmark
+
+ % TODO: compute the likelihood of this observation, multiply with the former weight
+ % to account for observing several features in one time step
+
+ end
+
+ end % measurement loop
+end % particle loop
+
+end

s8_fastslam/octave/fastslam.m

@@ -0,0 +1,67 @@
+% This is the main FastSLAM loop. This script calls all the required
+% functions in the correct order.
+%
+% You can disable the plotting or change the number of steps the filter
+% runs for to ease the debugging. You should however not change the order
+% or calls of any of the other lines, as it might break the framework.
+%
+% If you are unsure about the input and return values of functions you
+% should read their documentation which tells you the expected dimensions.
+
+% Turn off pagination:
+more off;
+
+clear all;
+close all;
+
+% Make tools available
+addpath('tools');
+
+% Read world data, i.e. landmarks. The true landmark positions are not given to the robot
+landmarks = read_world('../data/world.dat');
+% Read sensor readings, i.e. odometry and range-bearing sensor
+data = read_data('../data/sensor_data.dat');
+
+% Get the number of landmarks in the map
+N = size(landmarks,2);
+
+noise = [0.005, 0.01, 0.005]';
+
+% how many particles
+numParticles = 100;
+
+% initialize the particles array
+particles = struct;
+for i = 1:numParticles
+ particles(i).weight = 1. / numParticles;
+ particles(i).pose = zeros(3,1);
+ particles(i).history = cell();
+ for l = 1:N % initialize the landmarks aka the map
+ particles(i).landmarks(l).observed = false;
+ particles(i).landmarks(l).mu = zeros(2,1); % 2D position of the landmark
+ particles(i).landmarks(l).sigma = zeros(2,2); % covariance of the landmark
+ end
+end
+
+% toogle the visualization type
+%showGui = true; % show a window while the algorithm runs
+showGui = false; % plot to files instead
+
+% Perform filter update for each odometry-observation pair read from the
+% data file.
+for t = 1:size(data.timestep, 2)
+%for t = 1:50
+ printf('timestep = %d\n', t);
+
+ % Perform the prediction step of the particle filter
+ particles = prediction_step(particles, data.timestep(t).odometry, noise);
+
+ % Perform the correction step of the particle filter
+ particles = correction_step(particles, data.timestep(t).sensor);
+
+ % Generate visualization plots of the current state of the filter
+ plot_state(particles, landmarks, t, data.timestep(t).sensor, showGui);
+
+ % Resample the particle set
+ particles = resample(particles);
+end

s8_fastslam/octave/tools/drawellipse.m

@@ -0,0 +1,32 @@
+%DRAWELLIPSE Draw ellipse.
+% DRAWELLIPSE(X,A,B,COLOR) draws an ellipse at X = [x y theta]
+% with half axes A and B. Theta is the inclination angle of A,
+% regardless if A is smaller or greater than B. COLOR is a
+% [r g b]-vector or a color string such as 'r' or 'g'.
+%
+% H = DRAWELLIPSE(...) returns the graphic handle H.
+%
+% See also DRAWPROBELLIPSE.
+
+% v.1.0-v.1.1, Aug.97-Jan.03, Kai Arras, ASL-EPFL
+% v.1.2, 03.12.03, Kai Arras, CAS-KTH: (x,a,b) interface 
+
+
+function h = drawellipse(x,a,b,color);
+
+% Constants
+NPOINTS = 100; % point density or resolution
+
+% Compose point vector
+ivec = 0:2*pi/NPOINTS:2*pi; % index vector
+p(1,:) = a*cos(ivec); % 2 x n matrix which
+p(2,:) = b*sin(ivec); % hold ellipse points
+
+% Translate and rotate
+xo = x(1); yo = x(2); angle = x(3);
+R = [cos(angle) -sin(angle); sin(angle) cos(angle)];
+T = [xo; yo]*ones(1,length(ivec));
+p = R*p + T;
+
+% Plot
+h = plot(p(1,:),p(2,:),'Color',color, 'linewidth', 2);

s8_fastslam/octave/tools/drawprobellipse.m

@@ -0,0 +1,55 @@
+%DRAWPROBELLIPSE Draw elliptic probability region of a Gaussian in 2D.
+% DRAWPROBELLIPSE(X,C,ALPHA,COLOR) draws the elliptic iso-probabi-
+% lity contour of a Gaussian distributed bivariate random vector X
+% at the significance level ALPHA. The ellipse is centered at X =
+% [x; y] where C is the associated 2x2 covariance matrix. COLOR is
+% a [r g b]-vector or a color string such as 'r' or 'g'.
+%
+% X and C can also be of size 3x1 and 3x3 respectively.
+%
+% For proper scaling, the function CHI2INVTABLE is employed to
+% avoid the use of CHI2INV from the Matlab statistics toolbox.
+%
+% In case of a negative definite matrix C, the ellipse collapses
+% to a line which is drawn instead.
+%
+% H = DRAWPROBELLIPSE(...) returns the graphic handle H.
+%
+% See also DRAWELLIPSE, CHI2INVTABLE, CHI2INV.
+
+% v.1.0-v.1.3, 97-Jan.03, Kai Arras, ASL-EPFL
+% v.1.4, 03.12.03, Kai Arras, CAS-KTH: toolbox version
+
+
+function h = drawprobellipse(x,C,alpha,color);
+
+% Calculate unscaled half axes
+sxx = C(1,1); syy = C(2,2); sxy = C(1,2);
+a = sqrt(0.5*(sxx+syy+sqrt((sxx-syy)^2+4*sxy^2))); % always greater
+b = sqrt(0.5*(sxx+syy-sqrt((sxx-syy)^2+4*sxy^2))); % always smaller
+
+% Remove imaginary parts in case of neg. definite C
+if ~isreal(a), a = real(a); end;
+if ~isreal(b), b = real(b); end;
+
+% Scaling in order to reflect specified probability
+a = a*sqrt(chi2invtable(alpha,2));
+b = b*sqrt(chi2invtable(alpha,2));
+
+% Look where the greater half axis belongs to
+if sxx < syy, swap = a; a = b; b = swap; end;
+
+% Calculate inclination (numerically stable)
+if sxx ~= syy,
+ angle = 0.5*atan(2*sxy/(sxx-syy)); 
+elseif sxy == 0,
+ angle = 0; % angle doesn't matter 
+elseif sxy > 0,
+ angle = pi/4;
+elseif sxy < 0,
+ angle = -pi/4;
+end;
+x(3) = angle;
+
+% Draw ellipse
+h = drawellipse(x,a,b,color);

s8_fastslam/octave/tools/drawrobot.m

@@ -0,0 +1,147 @@
+%DRAWROBOT Draw robot.
+% DRAWROBOT(X,COLOR) draws a robot at pose X = [x y theta] such
+% that the robot reference frame is attached to the center of
+% the wheelbase with the x-axis looking forward. COLOR is a
+% [r g b]-vector or a color string such as 'r' or 'g'.
+%
+% DRAWROBOT(X,COLOR,TYPE) draws a robot of type TYPE. Five
+% different models are implemented:
+% TYPE = 0 draws only a cross with orientation theta
+% TYPE = 1 is a differential drive robot without contour
+% TYPE = 2 is a differential drive robot with round shape
+% TYPE = 3 is a round shaped robot with a line at theta
+% TYPE = 4 is a differential drive robot with rectangular shape
+% TYPE = 5 is a rectangular shaped robot with a line at theta
+%
+% DRAWROBOT(X,COLOR,TYPE,W,L) draws a robot of type TYPE with
+% width W and length L in [m].
+%
+% H = DRAWROBOT(...) returns a column vector of handles to all
+% graphic objects of the robot drawing. Remember that not all
+% graphic properties apply to all types of graphic objects. Use
+% FINDOBJ to find and access the individual objects.
+%
+% See also DRAWRECT, DRAWARROW, FINDOBJ, PLOT.
+
+% v.1.0, 16.06.03, Kai Arras, ASL-EPFL
+% v.1.1, 12.10.03, Kai Arras, ASL-EPFL: uses drawrect
+% v.1.2, 03.12.03, Kai Arras, CAS-KTH : types implemented
+
+
+function h = drawrobot(varargin);
+
+% Constants
+DEFT = 2; % default robot type
+DEFB = 0.4; % default robot width in [m], defines y-dir. of {R}
+WT = 0.03; % wheel thickness in [m]
+DEFL = DEFB+0.2; % default robot length in [m]
+WD = 0.2; % wheel diameter in [m]
+RR = WT/2; % wheel roundness radius in [m]
+RRR = 0.04; % roundness radius for rectangular robots in [m]
+HL = 0.09; % arrow head length in [m]
+CS = 0.1; % cross size in [m], showing the {R} origin
+
+% Input argument check
+inputerr = 0;
+switch nargin,
+ case 2,
+ xvec = varargin{1};
+ color = varargin{2};
+ type = DEFT;
+ B = DEFB;
+ L = DEFL;
+ case 3;
+ xvec = varargin{1};
+ color = varargin{2};
+ type = varargin{3};
+ B = DEFB;
+ L = DEFL;
+ case 5;
+ xvec = varargin{1};
+ color = varargin{2};
+ type = varargin{3};
+ B = varargin{4};
+ L = varargin{5};
+ otherwise
+ inputerr = 1;
+end;
+
+% Main switch statement
+if ~inputerr,
+ x = xvec(1); y = xvec(2); theta = xvec(3);
+ T = [x; y];
+ R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
+
+ switch type
+ case 0,
+ % Draw origin cross
+ p = R*[CS, -CS, 0, 0; 0, 0, -CS, CS] + T*ones(1,4); % horiz. line
+ h = plot(p(1,1:2),p(2,1:2),'Color',color,p(1,3:4),p(2,3:4),'Color',color);
+
+ case 1,
+ % Draw wheel pair with axis and arrow
+ xlw = [x+B/2*cos(theta+pi/2); y+B/2*sin(theta+pi/2); theta];
+ h1 = drawrect(xlw,WD,WT,RR,1,color); % left wheel
+ xlw = [x-B/2*cos(theta+pi/2); y-B/2*sin(theta+pi/2); theta];
+ h2 = drawrect(xlw,WD,WT,RR,1,color); % right wheel
+ % Draw axis cross with arrow
+ p = R*[0, 0; -B/2+WT/2, B/2-WT/2] + T*ones(1,2);
+ h3 = plot(p(1,:),p(2,:),'Color',color);
+ p = R*[L/2; 0] + T;
+ h4 = drawarrow(T,p,1,HL,color);
+ h = cat(1,h1,h2,h3,h4);
+
+ case 2,
+ % Draw wheel pair with axis and arrow
+ xlw = [x+B/2*cos(theta+pi/2); y+B/2*sin(theta+pi/2); theta];
+ h1 = drawrect(xlw,WD,WT,RR,1,color); % left wheel
+ xlw = [x-B/2*cos(theta+pi/2); y-B/2*sin(theta+pi/2); theta];
+ h2 = drawrect(xlw,WD,WT,RR,1,color); % right wheel
+ % Draw axis cross with arrow
+ p = R*[0, 0; -B/2+WT/2, B/2-WT/2] + T*ones(1,2);
+ h3 = plot(p(1,:),p(2,:),'Color',color);
+ p = R*[(B+WT)/2; 0] + T;
+ h4 = drawarrow(T,p,1,HL,color);
+ % Draw circular contour
+ radius = (B+WT)/2;
+ h5 = drawellipse(xvec,radius,radius,color);
+ h = cat(1,h1,h2,h3,h4,h5);
+
+ case 3,
+ % Draw circular contour
+ radius = (B+WT)/2;
+ h1 = drawellipse(xvec,radius,radius,color);
+ % Draw line with orientation theta with length radius
+ p = R*[(B+WT)/2;0] + T;
+ h2 = plot([T(1) p(1)],[T(2) p(2)],'Color',color,'linewidth',2);
+ h = cat(1,h1,h2);
+
+ case 4,
+ % Draw wheel pair with axis and arrow
+ xlw = [x+B/2*cos(theta+pi/2); y+B/2*sin(theta+pi/2); theta];
+ h1 = drawrect(xlw,WD,WT,RR,1,color); % left wheel
+ xlw = [x-B/2*cos(theta+pi/2); y-B/2*sin(theta+pi/2); theta];
+ h2 = drawrect(xlw,WD,WT,RR,1,color); % right wheel
+ % Draw axis cross with arrow
+ p = R*[0, 0; -B/2+WT/2, B/2-WT/2] + T*ones(1,2);
+ h3 = plot(p(1,:),p(2,:),'Color',color);
+ p = R*[L/2; 0] + T;
+ h4 = drawarrow(T,p,1,HL,color);
+ % Draw rectangular contour
+ h5 = drawrect(xvec,L,B,RRR,0,color);
+ h = cat(1,h1,h2,h3,h4,h5);
+
+ case 5,
+ % Draw rectangular contour
+ h1 = drawrect(xvec,L,B,RRR,0,color);
+ % Draw line with orientation theta with length L
+ p = R*[L/2; 0] + T;
+ h2 = plot([T(1) p(1)],[T(2) p(2)],'Color',color,'linewidth',2);
+ h = cat(1,h1,h2);
+
+ otherwise
+ disp('drawrobot: Unsupported robot type'); h = [];
+ end;
+else
+ disp('drawrobot: Wrong number of input arguments'); h = [];
+end;