diff --git a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m new file mode 100644 index 0000000..0c4a773 --- /dev/null +++ b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m @@ -0,0 +1,232 @@ + +%% FastSLAM and FastSLAM v.2 example +clear;clc;close all + +%% Create AVI object +makemovie = 0; +if(makemovie) + vidObj = VideoWriter('fastSLAM.mp4'); + vidObj.Quality = 100; + vidObj.FrameRate = 2; + open(vidObj); +end + + +%% Select an example to run +% Example=1: robot travelling in a circular path between 2 lines of obstacles +% Example=2: robot travelling back and forth between 2 lines of obstacles +% Example=3: robot travelling in a circular path. 6 obstacles form a small circle +% Example=4: robot travelling in a circular path. 6 obstacles form a large circle +% Example=5: robot travelling in a circular path. 10 randomly placed +% features +Example=1; + + +% The number of particles to use in the simulation +numParticles = 1000; + +slamVersion = 2; +% Select the fast slam implementation to use (func_fastSLAM or func_fastSLAM2) +switch slamVersion + case 1 + fastSlamFn = @func_fastSLAM; + case 2 + fastSlamFn = @func_fastSLAM2; + otherwise + fastSlamFn = @func_fastSLAM; +end + + +%% Main Code +% Time +Tf = 25; +dt = 0.5; +T = 0:dt:Tf; + +% Initial Robot State: (x,y,heading) +x0 = [0 0 0]'; + +% Generate control inputs and feature map based on example selection +switch Example + case 1 + u = ones(2, length(T)); + u(2,:)=0.3*u(2,:); + map = [-5:1:5 5:-1:-5; -2*ones(1,11) 8*ones(1,11)]; + axisDim = [ -6 6 -3 9 ]; + case 2 + turns=5; % number of 180 deg turns to make + u=ones(2,length(T)); + u(2,:)=0; + for n=1:turns + u(2,floor(length(T)./(turns+1)*n))=pi/dt; + end + map=[-1:1:8,8:-1:-1; 2*ones(1,10),-2*ones(1,10)]; + axisDim = [ -2 9 -3 3 ]; + case 3 + u = ones(2, length(T)); + u(2,:)=0.3*u(2,:); + N=6; %number of features + Radius=6; + center=[1;3]; + theta=linspace(2*pi./N,2*pi,N); + map=[center(1)+Radius.*cos(theta); + center(2)+Radius.*sin(theta)]; + axisDim = [ -6 8 -4 10 ]; + case 4 + u = ones(2, length(T)); + u(2,:)=0.3*u(2,:); + N=6; %number of features + Radius=12; + center=[1;3]; + theta=linspace(2*pi./N,2*pi,N); + map=[center(1)+Radius.*cos(theta); + center(2)+Radius.*sin(theta)]; + axisDim = [ -12 14 -10 16 ]; + case 5 + u = ones(2, length(T)); + u(2,:)=0.3*u(2,:); + N=16; %number of features + map = 10*rand(2,N); + map(1,:) = map(1,:)-5; + map(2,:) = map(2,:)-2; + axisDim = [ -6 6 -3 9 ]; + +end +M = length(map); + +% Motion Disturbance model +R = [0.01 0 0; + 0 0.01 0; + 0 0 0.001]; +[RE, Re] = eig(R); + +% Prior over robot state +mu0r = [0 0 0]'; % mean (mu) + +% Prior over feature map +newFeatures = zeros(M,1); + +%Measurement model +rMax = 20; % Max range +halfThetaMax = pi/4; % 1/2 Field of view + +% Measurement noise +Qi = 0.01 * [0.02 0; + 0 0.02]; +[QiE, Qie] = eig(Qi); + +% pre-compute noise multipliers +motionNoiseFactor = RE * sqrt(Re); +measurementNoiseFactor = QiE * sqrt(Qie); + + +% Simulation Initializations +n = size(x0,1); % Number of vehicle states +xr = zeros(n,length(T)); % Vehicle states +xr(:,1) = x0; +m = length(Qi(:,1)); % Number of measurements per feature +y = zeros(m*M,length(T)); % Measurements + +particles(1:numParticles) = struct( ... + 'mu', { zeros(size(x0,1),1) }, ... + 'f_mu', { zeros(2, M) }, ... + 'f_cov', { zeros(2, 2 * M) }, ... + 'w', { 0.00001 } ); + +mu_S = zeros(n,length(T)); % Belief + +count=0; % count of detected features in the map + +%% Plot initial step +t=1; +% //figure(1);clf; + + +if (makemovie) + writeVideo(vidObj, getframe(gcf)); +end + +%% Main loop +tic; +for t=2:length(T) + %% Simulation + + % Select a motion disturbance + e = motionNoiseFactor*randn(n,1); + % Update robot state + xr(:,t) = simpleRobotMotionModel(xr(:,t-1), u(:,t), dt)+e; + + % Find any features that are visible... (Each particle should have its own independent field of view...) + % and collect their measurements (noisy of course) + foundFeatures = searchForFeatures( xr(:,t), map, rMax, halfThetaMax, measurementNoiseFactor ); + + [particles,newFeatures] = fastSlamFn(particles, foundFeatures, u(:,t), R, Qi, newFeatures, dt); + + % calculate the new belief by averaging the particles + mu = sum( [particles.mu], 2 ) / numParticles; + + % Store results + mu_S(:,t) = mu; + + %% Plot results + figure(1);clf; hold on; + axis equal + axis(axisDim) + + %% Color Map Intialization + cmap = colormap('jet'); + cmap = cmap(1:3:end,:); + cn = length(cmap(:,1)); + + title('FastSLAM with Range & Bearing Measurements'); + %% Plot the Map + for j = 1:M + plot(map(1,j),map(2,j),'o','Color', cmap(mod(j,cn)+1,:), 'MarkerSize',10,'LineWidth',2); + end + + s = max(1,t - 30); + % plot the true motion + plot(xr(1,s:t), xr(2,s:t), 'ro'); + + % plot the lines of view + for j=1:size(foundFeatures,2) + fp = locateFeature( xr(:,t), foundFeatures(1:2,j) ); + plot([xr(1,t) fp(1)], [xr(2,t) fp(2)], 'Color', cmap(mod(j,cn)+1,:) ); + end + + % plot the particles + step = max(7,floor(numParticles) * 0.1); + for p = 1:step:numParticles + plot(particles(p).mu(1),particles(p).mu(2), 'b.'); + for i = 1:length(map) + if newFeatures(i) == 1 + plot(particles(p).f_mu(1,i), particles(p).f_mu(2,i), 'b.', 'Color', cmap(mod(i,cn)+1,:)); + end + end + end + + % plot the belief (center of the particles) + plot(mu_S(1,s:t),mu_S(2,s:t), 'k*') + + if (makemovie) + writeVideo(vidObj, getframe(gcf)); + end +end +toc + +if (makemovie) + close(vidObj); +end + +%Error Between true pose and fast slam prediction +err = xr - mu_S; +figure(2); +subplot(3,1,1); +plot(err(1,:));hold on;title('Error Plots');xlabel('time');ylabel('X_{err}'); +subplot(3,1,2); +plot(err(2,:));hold on;title('Error Plots');xlabel('time');ylabel('Y_{err}'); +subplot(3,1,3); +plot(err(3,:));hold on;title('Error Plots');xlabel('time');ylabel('Theta_{err}'); +fprintf('RMS error in X_pos: %f \n',rms(err(1,:))); +fprintf('RMS error in Y_pos: %f \n',rms(err(2,:))); +fprintf('RMS error in Orientation: %f \n',rms(err(3,:))); diff --git a/matlab_simulation/06-mapping/fastSLAM/addFeature.m b/matlab_simulation/06-mapping/fastSLAM/addFeature.m deleted file mode 100644 index 3746996..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/addFeature.m +++ /dev/null @@ -1,19 +0,0 @@ -%% A FUNCTION TO ADD UNOBSERVED FEATURES TO THE PARTICLES -% INPUTS:(in order of call) -% ------- -% The prior mean of the feature -% The observation that led to the feature's discovery -% Uncertainty in that Measurement -% Jacobian Matrix of the Observation with respect to the current particle -% OUTPUTS:(in order of call) -% -------- -% Predicted Mean of newly observed feature -% Predicted Covariance of the newly Observed Feature -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [meanFeatP,covaFeatP]=addFeature(prior,obsFeat,H_feat,measUncertainty) - meanFeatP(1,1) = prior(1,1)+obsFeat(1,1)*cos(obsFeat(2,1)+prior(3,1)); - meanFeatP(2,1) = prior(2,1)+obsFeat(1,1)*sin(obsFeat(2,1)+prior(3,1)); - covaFeatP= H_feat\measUncertainty*inv(H_feat)';%calculate cov - end \ No newline at end of file diff --git a/matlab_simulation/06-mapping/fastSLAM/applyMotionModel.m b/matlab_simulation/06-mapping/fastSLAM/applyMotionModel.m new file mode 100644 index 0000000..153b089 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM/applyMotionModel.m @@ -0,0 +1,7 @@ +function xNew = applyMotionModel( x, u, noiseFactor, dt ) + %% applyMotionModel( X, u, noiseFactor ) + e = noiseFactor * randn( size(x,1), 1 ); + xNew = [ x(1) + u(1) * cos( x(3) ) * dt; + x(2) + u(1) * sin( x(3) ) * dt; + x(3) + u(2) * dt ] + e; +end diff --git a/matlab_simulation/06-mapping/fastSLAM/calculateFeatureMeasurement.m b/matlab_simulation/06-mapping/fastSLAM/calculateFeatureMeasurement.m new file mode 100644 index 0000000..0e90bb8 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM/calculateFeatureMeasurement.m @@ -0,0 +1,17 @@ +function [y, H] = calculateFeatureMeasurement( f_mu, p_mup ) + %% calculateFeatureMeasurement( f_mu, r_mu ) + % Using the predicted particle position and the previous belief + % of the feature, calculate what the measurement should be and + % the linearized measurement model, H. + dx = f_mu(1) - p_mup(1); + dy = f_mu(2) - p_mup(2); + r_sq = dx*dx + dy*dy; + r = sqrt(r_sq); + + b = bearing( f_mu(1), f_mu(2), p_mup(1), p_mup(2), p_mup(3) ); + + y = [ r; b ]; + H = [ dx/r, dy/r; + -dy/r_sq, dx/r_sq ]; +end + diff --git a/matlab_simulation/06-mapping/fastSLAM/calculateMeasurementAndJacobians.m b/matlab_simulation/06-mapping/fastSLAM/calculateMeasurementAndJacobians.m new file mode 100644 index 0000000..dba28dc --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM/calculateMeasurementAndJacobians.m @@ -0,0 +1,19 @@ +function [y, Hm, Hx] = calculateMeasurementAndJacobians( f_mu, p_mup ) + %% calculateMeasurementJacobians( f_mu, p_mup ) + % Using the predicted particle position and the previous belief + % of the feature, calculate the Jacobians with respect to X, and M. + dx = f_mu(1) - p_mup(1); + dy = f_mu(2) - p_mup(2); + r_sq = dx*dx + dy*dy; + r = sqrt(r_sq); + + b = bearing( f_mu(1), f_mu(2), p_mup(1), p_mup(2), p_mup(3) ); + + y = [ r; b ]; + + Hm = [ dx/r, dy/r; + -dy/r_sq, dx/r_sq ]; + + Hx = [ -dx/r, -dy/r, 0; + dy/r_sq, -dx/r_sq, -1]; +end diff --git a/matlab_simulation/06-mapping/fastSLAM/ekf_correct.m b/matlab_simulation/06-mapping/fastSLAM/ekf_correct.m deleted file mode 100644 index 0e60876..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/ekf_correct.m +++ /dev/null @@ -1,25 +0,0 @@ -%% A function for the correction step of the EKF -% NOTE: THE prediction step has already been called, Meas Jac Calculated -% and new feature initialized, if at all. This is the next correction step -% -% INPUTS:(in order of call) -% ------- -% Mean of all observed Features -% covariance of all Observed Features -% Jacobian of the Measurement with respect to the features -% Predicted Observation from the prediction step of EKF -% Measurement Uncertainty -% OUTPUTS:(in order of call) -% -------- -% Preicted mean of The feature -% Predicted covariance of the feature -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [meanFeatP,covaFeatP]=ekf_correct(muFeat,covFeat,obsFeat,H_feat,pred_obs,measUncertainty) - I = obsFeat(:,1)-pred_obs; - Q2 = H_feat*covFeat*H_feat' + measUncertainty; - K = covFeat*H_feat'/Q2; - meanFeatP = muFeat + K*I;%new mean - covaFeatP = (eye(2)-K*H_feat)*covFeat;% New covariance - end \ No newline at end of file diff --git a/matlab_simulation/06-mapping/fastSLAM/featureSearch_fs.m b/matlab_simulation/06-mapping/fastSLAM/featureSearch_fs.m deleted file mode 100644 index b4a2beb..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/featureSearch_fs.m +++ /dev/null @@ -1,36 +0,0 @@ -%% FUNC to search of features in Field of VIEW -% INPUTS:(in order of call) -% ------- -% No of features -% Map of the Environment -% Robot's pose at the current time step -% Maximum Range -% Maximum FOV -% OUTPUTS:(in order of call) -% -------- -% A vector containing all the measured indexs -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% Feature Search - function meas_ind=featureSearch_fs(noFeatures,map,robot_pose,rmax,thmax) - meas_ind = []; - for i=1:noFeatures - if(inview(map(:,i),robot_pose,rmax,thmax)) - meas_ind=[meas_ind,i];%concatenate the indices - end - end - function yes = inview(f,x, rmax, thmax) - % Checks for features currently in robot's view - yes = 0; - dx = f(1)-x(1); - dy = f(2)-x(2); - r = sqrt(dx^2+dy^2); - th = mod(atan2(dy,dx)-x(3),2*pi); - if (th > pi) - th = th-2*pi; - end - if ((r seed,1); + Pnew(i).mu = P(cur).mu; + Pnew(i).f_mu = P(cur).f_mu; + Pnew(i).f_cov = P(cur).f_cov; + Pnew(i).w = P(cur).w; + end + +end diff --git a/matlab_simulation/06-mapping/fastSLAM/func_fastSLAM2.m b/matlab_simulation/06-mapping/fastSLAM/func_fastSLAM2.m index 1a7f824..94d5928 100644 --- a/matlab_simulation/06-mapping/fastSLAM/func_fastSLAM2.m +++ b/matlab_simulation/06-mapping/fastSLAM/func_fastSLAM2.m @@ -1,81 +1,122 @@ -%% Fast SLAM2.0 function -% INPUTS:(in order of call) -% ------- -% Robot's Current Pose -% Control Input -% Configurations -% Map -% Old particle Set -% OUTPUTS:(in order of call) -% -------- -% Predictd Pose of the Robot -% Measurements -% new ParticleSet -% Updated vector with flags for newly observed features -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%NOTE: The difference with FASTSLAM 1.0 is the sample_distribution before -%the observations and the way the weights are update. - -function[predPose,measurements,particles_new,newfeature]... - =func_fastSLAM2(oldPose,u,config,map,particles_old) - - R=config.MeasNoise;Q=config.MotNoise;noFeatures=size(map,2); - rmax=config.maxRange;thmax=config.maxFOV;dt=config.timeInterval; - totalParticles=config.totalParticles;newfeature=config.newFeat; +function [Pnew, featuresSeen] = func_fastSLAM2( P, z, u, R, Q, featureState, dt ) + %% fastSLAM( P, z, u, R, Q, dt ) + % perform one iteration of the fastSLAM algorithm. + % + % P -- particle vector - each particle has the following features: + % -- mu -- current estimated position (should be a distribution...) + % -- f_mu -- matrix of all estimated feature positions + % -- f_cov -- matrix of all feature covariances + % z -- measurement info for available features (range, bearing, id) + % u -- motion input + % R -- motion noise + % Q -- measurement noise + % featureState -- a vector of the features that have been seen or not + % (0 == unseen) + % rMax -- max + % dt -- time interval of motion + % + + countParticles = size(P,2); + w = zeros(countParticles,1); + + featuresSeen = featureState; + + [eV,ev] = eig(R); + noiseFactor = eV * sqrt(ev); - %Move the robot forward.This Motion has noise Q - predPose=motionUpdate_fs(oldPose,u,Q,dt); - %Look for features that are in FOV - meas_ind=featureSearch_fs(noFeatures,map,predPose,rmax,thmax); - %Take Measurements and accumulate them in a observation matrix - y=[]; - for feature_index = meas_ind - y_new=measurementUpdate_fs(map(:,feature_index),predPose,R); - y=[y,y_new]; - end + Rinv = inv(R); - measurements.indices=meas_ind;measurements.value=y; - %Loop through all particles - for particle=1:totalParticles - %Move the Particles forward - particles_old(particle).pose=motionUpdate_fs(particles_old(particle).pose,u,Q,dt); - clear yw hmuw Qw; - %Run Individual EKFs for each particles to predict and correct features - for j=1:length(meas_ind) - i = meas_ind(j); - -% %Sample Proposal (new in 2.0) - [mu_pose,cov_pose,H_pose,H_feat,pred_obs]=sample_proposal(... - particles_old(particle).meanFeat(:,i),... - particles_old(particle).covFeat(:,:,i),... - particles_old(particle).pose,y(:,j),R,Q); - %CORRECTION OF FEATURES OR Addition of new features - if(newfeature(i)==1) - [mu,cov]=addFeature(particles_old(particle).pose,y(:,j),H_feat,R); - else - [mu,cov]=ekf_correct(particles_old(particle).meanFeat(:,i),... - particles_old(particle).covFeat(:,:,i),... - y(:,j),H_feat,pred_obs,R); - end - particles_old(particle).meanFeat(:,i)=mu; - particles_old(particle).covFeat(:,:,i)=cov; - %Accumulate the weights for the Particle Filter Update - yw(2*(j-1)+1:2*j) = y(:,j);% target distribution - hmuw(2*(j-1)+1:2*j) = pred_obs;% proposal distribution Mean - %proposal distribution Covariance (new in 2.0) - Qw(2*(j-1)+1:2*j,2*(j-1)+1:2*j) =H_pose*Q*H_pose'+... - H_feat*particles_old(particle).covFeat(:,:,i)*H_feat'+R; - end - - %Calculate the Weights and Ensure that they are not too low. - if (exist('Qw1')) - Qw1=round(Qw,6);% To avoid error in cholcov in mvnpdf. - w(particle) = max(0.000001,mvnpdf(yw,hmuw,Qw1)); - end + for p = 1:countParticles + + % move the particle forward in time + mup = applyMotionModel( P(p).mu, u, noiseFactor, dt ); + + countFeatures = size(z,2); + if countFeatures == 0 + % no feature information -- use the original prediction + P(p).mu = mup; + else + % Allocate enough space to store the calculated co-variance, as well + % as both the measured and predicted measurements + zMeas = zeros(2 * size(z,2), 1); + zPred = zeros(2 * size(z,2), 1); + multiQm = zeros(2 * size(z,2), 2 * size(z,2)); + + mup_f = [0;0;0]; + for f = 1:countFeatures + + % for each feature, perform an EKF update + featureIndex = z(3,f); + + covStart = (featureIndex - 1) * 2 + 1; + covEnd = covStart + 1; + + if featuresSeen( featureIndex ) == 0 + % need an initial value -- use the predicted position + P(p).f_mu(:,featureIndex) = locateFeature( mup, z(1:2,f)); + [zp, Hm, Hx] = calculateMeasurementAndJacobians( P(p).f_mu(:,featureIndex), mup ); + HmInv = inv(Hm); + P(p).f_cov(:,covStart:covEnd) = HmInv * Q * HmInv'; + else + [zp, Hm, Hx] = calculateMeasurementAndJacobians( P(p).f_mu(:,featureIndex), P(p).mu ); + end + + cov = P(p).f_cov(:,covStart:covEnd); + Qm = Hm*cov*Hm'+Q; % Qt(k) + QmInv = inv(Qm); + + % calculate the measurement error + zErr = z(1:2,f) - zp; + + % calculate a possible belief for the particle's position + covXt = inv(Hx' * QmInv * Hx + Rinv); + + % update the particle's belief for position + mup_f = mup_f + covXt * Hx' * QmInv * zErr; + + % update the Kalman gain and calculate the new feature + % position and covariance. + K = cov * Hm' * QmInv; + P(p).f_mu(:,featureIndex) = P(p).f_mu(:,featureIndex) + K*zErr; + P(p).f_cov(:,covStart:covEnd) = (eye(2)-K*Hm)*cov; + + % Store the co-variance calculated, and the two related values of Z. They'll be used + % later to calculate the overall weight for this particle. + zStart = 2 * (f-1) + 1; + zEnd = zStart + 1; + + % Calculate the covariance for the importance weighting + covWt = Hx * R * Hx' + Hm * cov * Hm' + Q; + + multiQm( zStart:zEnd, zStart:zEnd ) = round( covWt, 6 ); % prevent cholcov error + zMeas( zStart:zEnd, 1 ) = z(1:2,f); + zPred( zStart:zEnd, 1 ) = zp(1:2,1); + end + + % if we saw anything, recalculate the final weight for the + % particle + P(p).w = max( 0.00001, mvnpdf(zPred, zMeas, multiQm)); + P(p).mu = mup + mup_f / countFeatures; + end + w(p) = P(p).w; + end + + if exist( 'z', 'var' ) && size(z,1) == 3 + % mark features as seen +% featuresSeen(z(3,:)) = 1; end - %Take note for all observed features.0:observed 1:New - newfeature(meas_ind) = 0; - %Run the RaoBlackwellization resampling step - [particles_new]=raoBlackwell_fs(totalParticles,particles_old); -end \ No newline at end of file + + + % draw a new set of particles + W = cumsum(w); + % Pnew = zeros( ... ) % preallocate new structure space + for i = 1:countParticles + seed = W(end) * rand(1); + cur = find(W > seed,1); + Pnew(i).mu = P(cur).mu; + Pnew(i).f_mu = P(cur).f_mu; + Pnew(i).f_cov = P(cur).f_cov; + Pnew(i).w = P(cur).w; + end + +end diff --git a/matlab_simulation/06-mapping/fastSLAM/locateFeature.m b/matlab_simulation/06-mapping/fastSLAM/locateFeature.m new file mode 100644 index 0000000..4d91fc8 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM/locateFeature.m @@ -0,0 +1,11 @@ +function x = locateFeature( p_mu, f_y ) + %% locateFeature( p_mu, f_y ) + % Calculate an (x,y) coordinate for a feature given a bearing and range measurement + r = f_y(1); + b = f_y(2); + + % Note that we have to add angle of the robot body here as we are + % moving from the World frame to the Robot frame. + x = [ p_mu(1) + r * cos(b+p_mu(3)); + p_mu(2) + r * sin(b+p_mu(3));]; +end diff --git a/matlab_simulation/06-mapping/fastSLAM/measurementUpdate_fs.m b/matlab_simulation/06-mapping/fastSLAM/measurementUpdate_fs.m deleted file mode 100644 index 3e39130..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/measurementUpdate_fs.m +++ /dev/null @@ -1,21 +0,0 @@ -%% Measurement Model -% INPUTS:(in order of call) -% ------- -% Environment Map -% Robot's Current Pose -% Measurement Noise -% OUTPUTS:(in order of call) -% -------- -% Measurement Matrix at the current time step -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% Measurement Model - function obsFeat=measurementUpdate_fs(map,poseRobot,MeasNoise) - [QiE, Qie] = eig(MeasNoise); - m = length(MeasNoise(:,1)); % Number of measurements per feature - delta = QiE*sqrt(Qie)*randn(m,1); - obsFeat=[sqrt((map(1,1)-poseRobot(1,1))^2 + (map(2,1)-poseRobot(2,1))^2); - mod(atan2(map(2,1)-poseRobot(2,1),map(1,1)-poseRobot(1,1))-poseRobot(3,1)+pi,2*pi)-pi] + delta; - - end \ No newline at end of file diff --git a/matlab_simulation/06-mapping/fastSLAM/motionUpdate_fs.m b/matlab_simulation/06-mapping/fastSLAM/motionUpdate_fs.m deleted file mode 100644 index 73f334b..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/motionUpdate_fs.m +++ /dev/null @@ -1,22 +0,0 @@ -%% Motion Model -% INPUTS:(in order of call) -% ------- -% Robot's Old Pose at last time step -% Control Input -% Motion Noise -% Time interval -% OUTPUTS:(in order of call) -% -------- -% New Pose of the Robot -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -%% Motion Model - function newPose=motionUpdate_fs(oldPose1,controlInput,MotionNoise,dt) - [QE, Qe] = eig(MotionNoise);n = length(MotionNoise(:,1)); - e = QE*sqrt(Qe)*randn(n,1); - % Update robot state - newPose = [oldPose1(1,1)+controlInput(1,1)*cos(oldPose1(3,1))*dt;% This means U is just a transla1ion and rotation - oldPose1(2,1)+controlInput(1,1)*sin(oldPose1(3,1))*dt; - oldPose1(3,1)+controlInput(2,1)*dt] + e; - end \ No newline at end of file diff --git a/matlab_simulation/06-mapping/fastSLAM/plot_fs.m b/matlab_simulation/06-mapping/fastSLAM/plot_fs.m deleted file mode 100644 index 2bff5d5..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/plot_fs.m +++ /dev/null @@ -1,79 +0,0 @@ -%% Plot function for FASTSLAM -% INPUTS: -% ------- -% None -% OUTPUTS:(in order of call) -% None -% -------- -%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% Record movie if set so. -if(config.makemovie) - vidObj = VideoWriter('fastslam.avi'); - vidObj.Quality = 100; - vidObj.FrameRate = 5; - open(vidObj); -end -%% Color Map Intialization -cmap = colormap('jet'); -cmap = cmap(1:3:end,:); -cn = length(cmap(:,1)); -figure(1); hold on; - -%% Define Axes -axis equal -axis([-10 10 -3 10]) -title('FastSLAM with Range & Bearing Measurements'); -%% Plot the Map -for j = 1:config.noFeatures - LM=plot(map(1,j),map(2,j),'o','Color', cmap(mod(j,cn)+1,:), 'MarkerSize',10,'LineWidth',2); -end - -%% Loop Through the DataSet and plot the results -for t=2:config.finalTime/config.timeInterval - %Plot the Robot Pose - L1=plot(pose(1,t),pose(2,t), 'ro--'); - %Plot the Robot True Pose - L2=plot(truePose(1,t),truePose(2,t),'k^'); hold on - % Draw the Heading - L3=plot([pose(1,t) pose(1,t)+1*cos(pose(3,t))],... - [pose(2,t) pose(2,t)+1*sin(pose(3,t))], 'r-'); - %Differentiate the currently Observed Features by drawing a line - for j=1:length(measurements(t).indices) - L4(j)=plot([pose(1,t) pose(1,t)+measurements(t).value(1,j)*cos(measurements(t).value(2,j)+pose(3,t))],... - [pose(2,t) pose(2,t)+measurements(t).value(1,j)*sin(measurements(t).value(2,j)+pose(3,t))],... - 'Color', cmap(mod(measurements(t).indices(j),cn)+1,:) ); - end - %Plot centroid of Particles - L5=plot(centroid(1,t),centroid(2,t), 'g*'); - %Draw the Particles - for d=1:config.totalParticles - L6(d)=plot(particleSet(t).p(d).pose(1,1),particleSet(t).p(d).pose(2,1),'b.'); - for j = measurements(t).indices - L7(d,j)= plot(particleSet(t).p(d).meanFeat(1,j),particleSet(t).p(d).meanFeat(2,j),'.','Color', cmap(mod(j,cn)+1,:)); - end - end - - legend([L1,L2,L5],'Actual','No Noise','CentroidPose','Location','southoutside','Orientation','horizontal'); - if (config.makemovie) writeVideo(vidObj, getframe(gca)); end - pause(0.001); - %Deletes the old measurements and particles from the plot - delete(L3) - delete(L4) - delete(L6) - delete(L7) -end -if (config.makemovie) close(vidObj); end - -%Plot Errors -err_p=centroid-pose; -%Error Between true pose and fast slam prediction -figure(2); -subplot(3,1,1); -plot(err_p(1,:));hold on;title('Error Plots');xlabel('time');ylabel('X_{err}'); -subplot(3,1,2); -plot(err_p(2,:));hold on;xlabel('time');ylabel('Y_{err}'); -subplot(3,1,3); -plot(err_p(3,:));hold on;xlabel('time');ylabel('\theta_{err}'); -fprintf('RMS error in X_pos: %f \n',rms(err_p(1,:))); -fprintf('RMS error in Y_pos: %f \n',rms(err_p(2,:))); -fprintf('RMS error in Orientation: %f \n',rms(err_p(3,:))); diff --git a/matlab_simulation/06-mapping/fastSLAM/predict_features.m b/matlab_simulation/06-mapping/fastSLAM/predict_features.m deleted file mode 100644 index 4bf5444..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/predict_features.m +++ /dev/null @@ -1,20 +0,0 @@ -%% Prediction Step of the observed features. -% INPUTS:(in order of call) -% ------- -% Mean of the Observed Features -% Robot's Current Pose -% OUTPUTS:(in order of call) -% -------- -% predicted observation matrix -% jacobian of the measurement wrt features -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - -function [H_feat,pred_obs]=predict_features(muFeatOld,rob_pose) - dx = muFeatOld(1,1)-rob_pose(1,1); - dy = muFeatOld(2,1)-rob_pose(2,1); - rp = sqrt((dx)^2+(dy)^2); - pred_obs=[rp; mod(atan2(dy,dx)-rob_pose(3,1)+pi,2*pi)-pi]; - H_feat = [ dx/rp dy/rp; -dy/rp^2 dx/rp^2];% Calculate Jacobian wrt features - end \ No newline at end of file diff --git a/matlab_simulation/06-mapping/fastSLAM/raoBlackwell_fs.m b/matlab_simulation/06-mapping/fastSLAM/raoBlackwell_fs.m deleted file mode 100644 index 36723ac..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/raoBlackwell_fs.m +++ /dev/null @@ -1,37 +0,0 @@ -%% Rao Blackwellization Function for the Particle FILTER -% INPUTS:(in order of call) -% ------- -% total no of Particles -% Weights calculated from Importance Sampling -% Old particle Set -% Mean of the Particle Set -% Covariance of the Particle Set -% OUTPUTS:(in order of call) -% -------- -% new Particle Set after -% Mean of the Particle set after resampling -% Covariance of the Resampled particle set -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - - -%% Rao Blackwellization - function [particles_new]=raoBlackwell_fs(totalParticles,particles_old) - W(1)=0; - for i=2:totalParticles - W(i) = W(i-1)+particles_old(i).weight;% Form the PDF and calculate the final cumulitive sum of all weights. - end - % Resample and copy all data to new particle set - for particle=1:totalParticles - seed = W(end)*rand(1); - cur = find(W>seed,1); - particles_new(particle).pose = particles_old(cur).pose; - particles_new(particle).meanFeat = particles_old(cur).meanFeat; - particles_new(particle).covFeat = particles_old(cur).covFeat; - end - for i=1:totalParticles - particles_new(i).weight=particles_old(i).weight; - end - - - end \ No newline at end of file diff --git a/matlab_simulation/06-mapping/fastSLAM/run_fastSLAM.m b/matlab_simulation/06-mapping/fastSLAM/run_fastSLAM.m deleted file mode 100644 index cebafdb..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/run_fastSLAM.m +++ /dev/null @@ -1,106 +0,0 @@ -%% RUN THE FAST SLAM FUNCTION -% INPUTS:(in order of call) -% ------- -% None -% OUTPUTS:(in order of call) -% -------- -% None -% INSTRUCTIONS: -%------------- -% 1.Settings can be changed in the 'config' structure -% 2.Change function name to fastSLAM2 to invoke fastSLAM2. -% 3.Set randMapFlag to 0 to load the static map.(staticMap.m)must -% exist.This is useful for comparision of fastSLAM1.0 and 2.0. Setting this -% value to 1 randomizes the map in each iteration. -% 4. Set the makemovie flag to 1 to record a movie -% -% NOTE: Advantages of the fastSLAM2.0 can be observed by reducing the no of -% features, increassing the motion noise, increasing total features -% COMMENTS: -% 1.TruePose(BLACK DELTA): here refers to the pose,we want the robot to undertake.But it -% reality it is corrupted by motion noise(RED).This is just for reference. -% The robot in reality executes the red trajectory which we have to track -% using fastSLAM. -% 2. The movie making is currently disabled.But can be added. -% 3. Q=Motion Noise and R=Measurement Noise, unlike the slides(flipped). -% 4. Error is plotted after the simulation is complete and RMS is displayed -% in prompt. -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% -%% Initializations -clc; clear all; close all; -% Initialize Configuration Structure -config=struct('MotNoise',{[0.001 0 0; - 0 0.001 0; - 0 0 0.0001]},... - 'MeasNoise',{[0.02 0; - 0 0.02]},... - 'finalTime',{20},... - 'timeInterval',{0.1},... - 'noFeatures',{20},... - 'totalParticles',{100},... - 'maxRange',{10},... - 'maxFOV',{pi/4},... - 'randMapFlag',{0},... - 'makemovie',{1});% Set=1:to randomize Map. 0:to load static map.(staticMap.m must exist) -config.newFeat=ones(config.noFeatures,1); -Q=config.MotNoise; -R=config.MeasNoise; - -% Primary Initializations -pose(:,1)=[0,0,0]';%initial Pose -truePose(:,1)=pose(:,1); -u=[1;0.3]; -%Map Initializations -if(config.randMapFlag==1) -map = 10*rand(2,noFeatures); -map(1,:) = map(1,:)-5; -map(2,:) = map(2,:)-2; -else - load('staticMap.mat'); -end - -% Define the Particle Structure -particles_old(1:config.totalParticles)=struct('pose',{zeros(size(Q,1),1)},... - 'meanFeat',{zeros(size(R,1),config.noFeatures)},... - 'covFeat',{zeros(size(R,1),size(R,1),config.noFeatures)},... - 'weight',{1/config.totalParticles}); -measurements=struct('indices',{},... - 'value',{}); -particleSet.p=particles_old; -particles_new=particles_old; - - -%Initialize ProgessBar -h = waitbar(0,'Initializing waitbar...'); -%% Fast SLAM1.0 LOOP - -for t=2:config.finalTime/config.timeInterval - oldPose=pose(:,t-1); - [newPose,measurements(t),particles_new,newfeature]... - =func_fastSLAM2(oldPose,u,config,map,particles_old); - %STORE the Pose,Centroid and timestamp - pose(:,t)=newPose; - particleSet(t).p=particles_new; - %calculate the centroid of particles - centroid(:,t)=[0;0;0]; - for i=1:config.totalParticles - centroid(:,t)=centroid(:,t)+particles_new(i).pose; - end - centroid(:,t)=centroid(:,t)./config.totalParticles; - - %Updates the true pose of the robot - truePose(:,t)=motionUpdate_fs(truePose(:,t-1),u,zeros(3),config.timeInterval); - - % Swap Particles - particles_old=particles_new; - config.newFeat=newfeature; - -%Show Progess -perC=t/(config.finalTime/config.timeInterval); -waitbar(perC,h,sprintf('Computing...%3.1f%% completed.',perC*100)); -end -close(h);%Close the waitbar - -%Plot the results -plot_fs; diff --git a/matlab_simulation/06-mapping/fastSLAM/sample_proposal.m b/matlab_simulation/06-mapping/fastSLAM/sample_proposal.m deleted file mode 100644 index 2d7dcb1..0000000 --- a/matlab_simulation/06-mapping/fastSLAM/sample_proposal.m +++ /dev/null @@ -1,31 +0,0 @@ -%% Sampling of the Proposal Distribution based on current measurement -%(unique to FAST SLAM2.0) -% INPUTS:(in order of call) -% ------- -% Mean of the observed features -% Covariance of the observed features -% Robot's Current Pose -% Observations at the current timestep -% Measurement Noise -% Motion Noise -% OUTPUTS:(in order of call) -% -------- -% Estimated Mean of the Pose of the robot represented by the curr particle -% Estimated Covariance of the pose of the robot of i-th particle -% Jacobian of the measurement wrt to robot's pose -% Jacobian of the measurement wrt to observed features -% Predicted Observation -% % AUTHOR: BISMAYA SAHOO, EMAIL:bsahoo@uwaterloo.ca -%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% - -function [mu_pose,cov_pose,H_pose,H_feat,pred_obs]=sample_proposal(muFeatOld,covFeatOld,rob_pose,act_obs,R,Q) - dx = muFeatOld(1,1)-rob_pose(1,1); - dy = muFeatOld(2,1)-rob_pose(2,1); - rp = sqrt((dx)^2+(dy)^2); - pred_obs=[rp; mod(atan2(dy,dx)-rob_pose(3,1)+pi,2*pi)-pi]; - H_feat = [ dx/rp dy/rp; -dy/rp^2 dx/rp^2];% Calculate Jacobian wrt features - H_pose = [-dx/rp -dy/rp 0; -dy/rp^2 -dx/rp^2 -1];%Jacobian wrt Pose - Q_mat=(R+H_feat*covFeatOld*H_feat'); - cov_pose=(H_pose'*((Q_mat)^-1)*H_pose+Q)^-1; - mu_pose=cov_pose*H_pose'*(Q_mat^-1)*(act_obs-pred_obs)+rob_pose; - end \ No newline at end of file diff --git a/matlab_simulation/06-mapping/fastSLAM/searchForFeatures.m b/matlab_simulation/06-mapping/fastSLAM/searchForFeatures.m new file mode 100644 index 0000000..e33db62 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM/searchForFeatures.m @@ -0,0 +1,22 @@ +function features = searchForFeatures( X, map, rMax, thetaMax, measurementNoiseFactor ) + %% searchForFeatures( X, map, rMax, thetaMax, measurementNoiseFactor ) + % given a position on a map and a list of features (map), return + % the index, and measurements to any features that are within range + % + + features = []; + + countFeatures = size(map,2); + for f = 1:countFeatures + rf = sqrt( sum( (X(1:2) - map(:,f)).^2 ) ); + bf = bearing( map(1,f), map(2,f), X(1), X(2), X(3) ); + + if (rf <= rMax) && (thetaMax >= abs(bf)) + % feature is in view -- calculate a noisy measurement + fm = [ rf; bf ] + measurementNoiseFactor * randn(2, 1); + features = [features [fm; f]]; + end + end +end + + diff --git a/matlab_simulation/06-mapping/fastSLAM/staticMap.mat b/matlab_simulation/06-mapping/fastSLAM/staticMap.mat deleted file mode 100644 index c19127c..0000000 Binary files a/matlab_simulation/06-mapping/fastSLAM/staticMap.mat and /dev/null differ