├── README.md ├── getMeasurement.m ├── moveParticle.m ├── resample.m ├── screenshot.png └── slam.m /README.md: -------------------------------------------------------------------------------- 1 | SimpleSLAM 2 | ========== 3 | The following is a simple reference implementation of Simultaneous Localization 4 | And Mapping written in MATLAB. The code simulates a robot cruising around in 5 | a 2D world which contains 5 uniquely identifiable landmarks at unknown 6 | locations. The robot is equipped with a sensor that can measure the range and 7 | angle to these landmarks, but each measurement is corrupted by noise. 8 | 9 | This implementation is based on Thrun et. al's FastSLAM 1.0 algorithm, though 10 | all optimizations have been removed for clarity. The algorithm works by 11 | representing the position of the robot via a Particle Filter. Each particle 12 | contains 5 completely independent Extended Kalman Filters, each of which is 13 | responsible for tracking a single landmark. After each measurement of 14 | a landmark, each particle is given a score based on how closely the latest 15 | landmark measurement matched its expectation. The list of particles is then 16 | resampled such that particles with high scores are likely to be replicated, and 17 | those with low scores are likely to be deleted. 18 | 19 | To run this demo, just checkout this project, and run slam.m 20 | 21 | ![](https://github.com/randvoorhies/SimpleSLAM/raw/master/screenshot.png) 22 | 23 | Learn More! 24 | ================== 25 | * [My introductory lecture on Particle Filters](http://ilab.usc.edu/~rand/LectureSlides/445Lectures/ParticleFiltering/index.html) 26 | * [My SimpleKalman project](https://github.com/randvoorhies/SimpleKalman) -------------------------------------------------------------------------------- /getMeasurement.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Copyright 2010 Randolph Voorhies 3 | % This program is free software: you can redistribute it and/or modify 4 | % it under the terms of the GNU General Public License as published by 5 | % the Free Software Foundation, either version 3 of the License, or 6 | % (at your option) any later version. 7 | % 8 | % This program is distributed in the hope that it will be useful, 9 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | % GNU General Public License for more details. 12 | % 13 | % You should have received a copy of the GNU General Public License 14 | % along with this program. If not, see . 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | function [z, H] = getMeasurement(pos, landmark_pos, observation_variance) 17 | % Given a landmark position and a robot position, this method will return a 18 | % "measurement" z that contains the distance and the angle to the landmark. 19 | % Gaussian random noise is added to both based on the variances given in the 20 | % diagonal of the observation_variance matrix. Note that this method is used 21 | % both to take a "real" measurement in the simulation, as well as to assess 22 | % what kind of measurement each of our hypothetical particles would take. 23 | % This method also computes the Jacobian of the measurent function for use 24 | % in an extended Kalman filter. 25 | 26 | 27 | % Compute the distance from the current position to the landmark, and add 28 | % some Gaussian noise to make things interesting. Note that we are using 29 | % a smaller variance in this Gaussian distribution, as the algorithm seems 30 | % to work better when it underestimates the quality of the sensor. 31 | vector_to_landmark = [landmark_pos(1) - pos(1); landmark_pos(2) - pos(2)]; 32 | landmark_distance = norm(vector_to_landmark); 33 | landmark_distance = landmark_distance + normrnd(0, observation_variance(1)*.25); 34 | 35 | % Compute the angle from the given pos to the landmark 36 | landmark_angle = atan2(vector_to_landmark(2), vector_to_landmark(1)); 37 | landmark_angle = landmark_angle + normrnd(0, observation_variance(2)*.25); 38 | 39 | 40 | % Compute the Jacobian of this measurement function 41 | q = landmark_distance^2.0; 42 | H = [-(landmark_pos(1) - pos(1))/sqrt(q), -(landmark_pos(2) - pos(2))/sqrt(q), 0.0; 43 | (landmark_pos(2) - pos(2))/q, -(landmark_pos(1) - pos(1))/q, -1.0; 44 | 0.0, 0.0, 1.0]; 45 | 46 | z = [landmark_distance; 47 | landmark_angle; 48 | 0]; 49 | end 50 | -------------------------------------------------------------------------------- /moveParticle.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Copyright 2010 Randolph Voorhies 3 | % This program is free software: you can redistribute it and/or modify 4 | % it under the terms of the GNU General Public License as published by 5 | % the Free Software Foundation, either version 3 of the License, or 6 | % (at your option) any later version. 7 | % 8 | % This program is distributed in the hope that it will be useful, 9 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | % GNU General Public License for more details. 12 | % 13 | % You should have received a copy of the GNU General Public License 14 | % along with this program. If not, see . 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | function newpos = updateMovement(pos, movement, variance) 17 | % Compute how the robot should move from "pos" given the requested movement and 18 | % some Gaussian random noise using a very simple motion model. This method is 19 | % used to move the simulated robot as well as each of the hypothetical 20 | % particles. 21 | 22 | % Add some Gaussian random noise to the movement. Note that we are using 23 | % a smaller variance in this Gaussian distribution, as the algorithm seems 24 | % to work better when it underestimates the quality of the robot plant. 25 | speed = normrnd(movement(1), variance(1)*.25); 26 | rotation = normrnd(movement(2), variance(2)*.25); 27 | 28 | delta = zeros(3,1); 29 | delta(1,1) = cos(pos(3)+rotation)*speed; 30 | delta(2,1) = sin(pos(3)+rotation)*speed; 31 | delta(3,1) = rotation; 32 | 33 | newpos = pos+delta; 34 | end 35 | -------------------------------------------------------------------------------- /resample.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Copyright 2010 Randolph Voorhies 3 | % This program is free software: you can redistribute it and/or modify 4 | % it under the terms of the GNU General Public License as published by 5 | % the Free Software Foundation, either version 3 of the License, or 6 | % (at your option) any later version. 7 | % 8 | % This program is distributed in the hope that it will be useful, 9 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | % GNU General Public License for more details. 12 | % 13 | % You should have received a copy of the GNU General Public License 14 | % along with this program. If not, see . 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | function [newParticles] = resample(oldParticles) 17 | % Resample a group of particles such that those with higher weights have a 18 | % higher chance of being replicated, and those with low weights have a high 19 | % chance of disappearing 20 | 21 | weightSum = 0; 22 | for i=1:length(oldParticles) 23 | weightSum = weightSum + oldParticles(i).w; 24 | end 25 | for i=1:length(oldParticles) 26 | oldParticles(i).w = oldParticles(i).w/weightSum; 27 | end 28 | 29 | M = length(oldParticles); 30 | 31 | newParticles = []; 32 | 33 | r = rand / M; 34 | 35 | c = oldParticles(1).w; 36 | 37 | i = 1; 38 | 39 | for m=1:M 40 | U = r + (m-1) * M^(-1); 41 | 42 | while U > c 43 | i = i+1; 44 | c = c+oldParticles(i).w; 45 | end 46 | newParticles = [newParticles, oldParticles(i)]; 47 | end 48 | 49 | for i=1:length(newParticles) 50 | newParticles(i).w = 1.0/length(newParticles); 51 | end 52 | 53 | end 54 | -------------------------------------------------------------------------------- /screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/randvoorhies/SimpleSLAM/726385fe98d1d9fcf506ec6029e2167ea701830e/screenshot.png -------------------------------------------------------------------------------- /slam.m: -------------------------------------------------------------------------------- 1 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 2 | % Copyright 2010 Randolph Voorhies 3 | % This program is free software: you can redistribute it and/or modify 4 | % it under the terms of the GNU General Public License as published by 5 | % the Free Software Foundation, either version 3 of the License, or 6 | % (at your option) any later version. 7 | % 8 | % This program is distributed in the hope that it will be useful, 9 | % but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | % GNU General Public License for more details. 12 | % 13 | % You should have received a copy of the GNU General Public License 14 | % along with this program. If not, see . 15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 16 | 17 | 18 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 19 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 20 | % PARAMETERS 21 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 22 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 23 | 24 | % The number of timesteps for the simulation 25 | timesteps = 200; 26 | 27 | % The maximum distance from which our sensor can sense a landmark 28 | max_read_distance = 1.5; 29 | 30 | % The actual positions of the landmarks (each column is a separate landmark) 31 | real_landmarks = [1.0, 2.0, 0.0, 0.0, 1.0; % x 32 | 3.0, 2.5 3.4, 1.5, 3.5; % y 33 | 0.0, 0.0 0.0, 0.0, 0.0]; % Nothing 34 | 35 | % The initial starting position of the robot 36 | real_position = [0.0; % x 37 | -1.0; % y 38 | pi/3.0]; % rotation 39 | 40 | % The movement command given tot he robot at each timestep 41 | movement_command = [.05; % Distance 42 | .01]; % Rotation 43 | 44 | % The Gaussian variance of the movement commands 45 | movement_variance = [.1; % Distance 46 | .05]; % Rotation 47 | M = [movement_variance(1), 0.0; 48 | 0.0, movement_variance(2)]; 49 | 50 | % The Gaussian variance of our sensor readings 51 | measurement_variance = [0.1; % Distance 52 | 0.01; % Angle 53 | .0001]; % Landmark Identity 54 | R = [measurement_variance(1), 0.0, 0.0; 55 | 0.0, measurement_variance(2), 0.0; 56 | 0.0, 0.0, measurement_variance(3)]; 57 | 58 | % Create the particles and initialize them all to be in the same initial 59 | % position. 60 | particles = []; 61 | num_particles = 100; 62 | for i = 1:num_particles 63 | particles(i).w = 1.0/num_particles; 64 | particles(i).position = real_position; 65 | for lIdx=1:size(real_landmarks,2) 66 | particles(i).landmarks(lIdx).seen = false; 67 | end 68 | end 69 | 70 | pos_history = []; 71 | 72 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 74 | % SIMULATION 75 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 76 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 77 | 78 | for timestep = 1:timesteps 79 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 80 | % Move the actual robot 81 | real_position = moveParticle(real_position, movement_command, movement_variance); 82 | pos_history = [pos_history, real_position]; 83 | 84 | % Move the actual particles 85 | for pIdx = 1:num_particles 86 | particles(pIdx).position = moveParticle( ... 87 | particles(pIdx).position, movement_command, movement_variance); 88 | particles(pIdx).position(3) = real_position(3); 89 | end 90 | 91 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 92 | % Try to take a reading from each landmark 93 | doResample = false; 94 | for lIdx = 1:size(real_landmarks,2) 95 | real_landmark = real_landmarks(:, lIdx); 96 | 97 | % Take a real (noisy) measurement from the robot to the landmark 98 | [z_real, G] = getMeasurement(real_position, real_landmark, measurement_variance); 99 | read_distance(lIdx) = z_real(1); 100 | read_angle(lIdx) = z_real(2); 101 | 102 | % If the landmark is close enough, then we can spot it 103 | if(read_distance(lIdx) < max_read_distance) 104 | doResample = true; 105 | 106 | for pIdx = 1:num_particles 107 | 108 | if(particles(pIdx).landmarks(lIdx).seen == false) 109 | 110 | % If we have never seen this landmark, then we need to initialize it. 111 | % We'll just use whatever first reading we recieved. 112 | particles(pIdx).landmarks(lIdx).pos = [particles(pIdx).position(1) + cos(read_angle(lIdx))*read_distance(lIdx); 113 | particles(pIdx).position(2) + sin(read_angle(lIdx))*read_distance(lIdx); 114 | 0]; 115 | % Initialize the landmark position covariance 116 | particles(pIdx).landmarks(lIdx).E = inv(G) * R * inv(G)'; 117 | 118 | particles(pIdx).landmarks(lIdx).seen = true; 119 | 120 | else 121 | % Get an ideal reading to our believed landmark position (note 0 variance here). 122 | [z_p, Gp] = getMeasurement(particles(pIdx).position, particles(pIdx).landmarks(lIdx).pos, [0;0]); 123 | residual = z_real - z_p; 124 | 125 | %Calculate the Kalman gain 126 | Q = G' * particles(pIdx).landmarks(lIdx).E * G + R; 127 | K = particles(pIdx).landmarks(lIdx).E * G * inv(Q); 128 | 129 | % Mix the ideal reading, and our actual reading using the Kalman gain, and use the result 130 | % to predict a new landmark position 131 | particles(pIdx).landmarks(lIdx).pos = particles(pIdx).landmarks(lIdx).pos + K*(residual); 132 | 133 | % Update the covariance of this landmark 134 | particles(pIdx).landmarks(lIdx).E = (eye(size(K)) - K*G')*particles(pIdx).landmarks(lIdx).E; 135 | 136 | % Update the weight of the particle 137 | particles(pIdx).w = particles(pIdx).w * norm(2*pi*Q).^(-1/2)*exp(-1/2*(residual)'*inv(Q)*(residual)); 138 | end %else 139 | end %pIdx 140 | end %distance 141 | 142 | 143 | end %for landmark 144 | % Resample all particles based on their weights 145 | if(doResample) 146 | particles = resample(particles); 147 | end 148 | 149 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 150 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 151 | % PLOTTING 152 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 153 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 154 | clf; 155 | hold on; 156 | 157 | % Plot the landmarks 158 | for lIdx=1:size(real_landmarks,2) 159 | plot(real_landmarks(1,lIdx), real_landmarks(2,lIdx), 'b*'); 160 | end 161 | 162 | for lIdx = 1:size(real_landmarks,2) 163 | if(particles(1).landmarks(lIdx).seen) 164 | avg_landmark_guess =[0;0;0]; 165 | for pIdx = 1:length(particles) 166 | avg_landmark_guess = avg_landmark_guess + particles(pIdx).landmarks(lIdx).pos; 167 | end 168 | avg_landmark_guess = avg_landmark_guess / length(particles); 169 | plot(avg_landmark_guess(1), avg_landmark_guess(2), 'ko'); 170 | end 171 | end 172 | 173 | % Plot the particles 174 | particles_pos = [particles.position]; 175 | plot(particles_pos(1,:), particles_pos(2,:), 'r.'); 176 | 177 | % Plot the real robot 178 | plot(pos_history(1,:), pos_history(2,:), 'r'); 179 | w = .1; 180 | l = .3; 181 | x = real_position(1); 182 | y = real_position(2); 183 | t = real_position(3); 184 | plot(real_position(1), real_position(2), 'mo', ... 185 | 'LineWidth',1.5, ... 186 | 'MarkerEdgeColor','k', ... 187 | 'MarkerFaceColor',[0 1 0], ... 188 | 'MarkerSize',10); 189 | 190 | % Show the sensor measurement as an arrow 191 | for lIdx=1:size(real_landmarks,2) 192 | real_landmark = real_landmarks(:, lIdx); 193 | if(read_distance(lIdx) < max_read_distance) 194 | line([real_position(1), real_position(1)+cos(read_angle(lIdx))*read_distance(lIdx)], ... 195 | [real_position(2), real_position(2)+sin(read_angle(lIdx))*read_distance(lIdx)]); 196 | end 197 | end 198 | 199 | axis([-5, 5, -4, 7]); 200 | pause(.01); 201 | end 202 | 203 | 204 | 205 | --------------------------------------------------------------------------------