From 3fb87a31fdae539626261de1c88ad15ca6b414a4 Mon Sep 17 00:00:00 2001 From: Barry James Gilhuly Date: Thu, 22 Feb 2018 16:53:49 -0500 Subject: [PATCH 1/5] Completed implementations of fastSLAM and fastSLAM V2 --- .../mr-6-fastSlam/fastSLAM.m | 222 ++++++++++++++++++ .../fastSLAM_alt/applyMotionModel.m | 7 + .../calculateFeatureMeasurement.m | 17 ++ .../calculateMeasurementAndJacobians.m | 19 ++ .../06-mapping/fastSLAM_alt/fSLAM.m | 116 +++++++++ .../06-mapping/fastSLAM_alt/fSLAM_2.m | 122 ++++++++++ .../06-mapping/fastSLAM_alt/locateFeature.m | 11 + .../fastSLAM_alt/searchForFeatures.m | 22 ++ 8 files changed, 536 insertions(+) create mode 100644 matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m create mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/applyMotionModel.m create mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/calculateFeatureMeasurement.m create mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/calculateMeasurementAndJacobians.m create mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m create mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m create mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/locateFeature.m create mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/searchForFeatures.m 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..f0c9917 --- /dev/null +++ b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m @@ -0,0 +1,222 @@ + +% Extended Kalman Filter SLAM example +clear;clc;close all + +%% Create AVI object +makemovie = 0; +if(makemovie) + vidObj = VideoWriter('ekfSLAM.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; + +% Select the fast slam implementation to use (fSLAM or FSLAM_2) +fastSlamFn = @fSLAM_2; + +%% Main Code +% Time +Tf = 50; +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_alt/applyMotionModel.m b/matlab_simulation/06-mapping/fastSLAM_alt/applyMotionModel.m new file mode 100644 index 0000000..153b089 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM_alt/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_alt/calculateFeatureMeasurement.m b/matlab_simulation/06-mapping/fastSLAM_alt/calculateFeatureMeasurement.m new file mode 100644 index 0000000..0e90bb8 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM_alt/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_alt/calculateMeasurementAndJacobians.m b/matlab_simulation/06-mapping/fastSLAM_alt/calculateMeasurementAndJacobians.m new file mode 100644 index 0000000..dba28dc --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM_alt/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_alt/fSLAM.m b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m new file mode 100644 index 0000000..054eca3 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m @@ -0,0 +1,116 @@ +function [Pnew, featuresSeen] = fSLAM( 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); + + for p = 1:countParticles + + % move the particle forward in time + P(p).mu = applyMotionModel( P(p).mu, u, noiseFactor, dt ); + + if exist( 'z', 'var' ) + countFeatures = size(z,2); + + % 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)); + + 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 + P(p).f_mu(:,featureIndex) = locateFeature( P(p).mu, z(1:2,f)); + [zp, H] = calculateFeatureMeasurement( P(p).f_mu(:,featureIndex), P(p).mu ); + H_inv = inv(H); + P(p).f_cov(:,covStart:covEnd) = H_inv * Q * H_inv'; + else + [zp, H] = calculateFeatureMeasurement( P(p).f_mu(:,featureIndex), P(p).mu ); + end + + cov = P(p).f_cov(:,covStart:covEnd); + Qm = H*cov*H'+Q; + K = cov * H' * inv(Qm); + P(p).f_mu(:,featureIndex) = P(p).f_mu(:,featureIndex) + K*(z(1:2,f) - zp); + P(p).f_cov(:,covStart:covEnd) = (eye(2)-K*H)*cov; + + % Store the co-variance caluculated, and the two related values of Z. They'll be used + % later to calculate the overall weight for this particle. + qStart = size(Qm, 1) * (f - 1) + 1; + qEnd = qStart + size(Qm,1) - 1; + multiQm( qStart:qEnd, qStart:qEnd ) = round( Qm, 6 ); % prevent cholcov error + + zStart = 2 * (f-1) + 1; + zEnd = zStart + 1; + zMeas( zStart:zEnd, 1 ) = z(1:2,f); + zPred( zStart:zEnd, 1 ) = zp(1:2,1); + end + + if countFeatures > 0 + % if we saw anything, recalculate the final weight for the + % particle + P(p).w = max( 0.00001, mvnpdf(zPred,zMeas,multiQm)); + end + w(p) = P(p).w; + else + % no judgement info -- use the last weight + if length(P(p).w) ~= 1 + P(p).w = 1; + end + w(p) = P(p).w; + end + + end + + if exist( 'z', 'var' ) && size(z,1) == 3 + % mark features as seen + featuresSeen(z(3,:)) = 1; + end + + + % 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) = P(cur); + Pnew(i).mu = P(cur).mu; + Pnew(i).f_mu = P(cur).f_mu; + Pnew(i).f_cov = P(cur).f_cov; + if length(P(cur).w) ~= 1 + P(p).w = 1; + end + + Pnew(i).w = P(cur).w; + end + +end diff --git a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m new file mode 100644 index 0000000..2a3b403 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m @@ -0,0 +1,122 @@ +function [Pnew, featuresSeen] = fSLAM( 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); + + Rinv = inv(R); + + 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 ); + Hm_inv = inv(Hm); + P(p).f_cov(:,covStart:covEnd) = Hm_inv * Q * Hm_inv'; + 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) + Qm_inv = inv(Qm); + + % calculate the measurement error + zErr = z(1:2,f) - zp; + + % calculate a possible belief for the particle's position + covXt = inv(Hx' * Qm_inv * Hx + Rinv); + mup_f = mup_f + covXt * Hx' * Qm_inv * zErr + mup; + + % update the Kalman gain and calculate the new feature + % position and covariance. + K = cov * Hm' * Qm_inv; + 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 caluculated, and the two related values of Z. They'll be used + % later to calculate the overall weight for this particle. + qStart = size(Qm, 1) * (f - 1) + 1; + qEnd = qStart + size(Qm,1) - 1; + multiQm( qStart:qEnd, qStart:qEnd ) = round( Qm, 6 ); % prevent cholcov error + + zStart = 2 * (f-1) + 1; + zEnd = zStart + 1; + 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)); + + % true belief is the average of all the guesses + P(p).mu = 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 + + + % 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) = P(cur); + 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_alt/locateFeature.m b/matlab_simulation/06-mapping/fastSLAM_alt/locateFeature.m new file mode 100644 index 0000000..4d91fc8 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM_alt/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_alt/searchForFeatures.m b/matlab_simulation/06-mapping/fastSLAM_alt/searchForFeatures.m new file mode 100644 index 0000000..e33db62 --- /dev/null +++ b/matlab_simulation/06-mapping/fastSLAM_alt/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 + + From f4dbe6e1bab4af683566b1ec111ded34975c3e89 Mon Sep 17 00:00:00 2001 From: idlebear Date: Fri, 23 Feb 2018 08:56:50 -0500 Subject: [PATCH 2/5] Some minor code cleanup -- comment corrections and pruning test code --- .../mr-6-fastSlam/fastSLAM.m | 2 +- .../06-mapping/fastSLAM_alt/fSLAM.m | 34 ++++++------------- .../06-mapping/fastSLAM_alt/fSLAM_2.m | 16 ++++----- 3 files changed, 19 insertions(+), 33 deletions(-) diff --git a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m index f0c9917..723aad0 100644 --- a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m +++ b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m @@ -25,7 +25,7 @@ numParticles = 1000; % Select the fast slam implementation to use (fSLAM or FSLAM_2) -fastSlamFn = @fSLAM_2; +fastSlamFn = @fSLAM; %% Main Code % Time diff --git a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m index 054eca3..5c332e6 100644 --- a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m +++ b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m @@ -29,9 +29,11 @@ % move the particle forward in time P(p).mu = applyMotionModel( P(p).mu, u, noiseFactor, dt ); - if exist( 'z', 'var' ) - countFeatures = size(z,2); - + countFeatures = size(z,2); + if countFeatures == 0 + % no judgement info -- use the last weight + w(p) = P(p).w; + 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); @@ -62,29 +64,18 @@ P(p).f_mu(:,featureIndex) = P(p).f_mu(:,featureIndex) + K*(z(1:2,f) - zp); P(p).f_cov(:,covStart:covEnd) = (eye(2)-K*H)*cov; - % Store the co-variance caluculated, and the two related values of Z. They'll be used + % 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. - qStart = size(Qm, 1) * (f - 1) + 1; - qEnd = qStart + size(Qm,1) - 1; - multiQm( qStart:qEnd, qStart:qEnd ) = round( Qm, 6 ); % prevent cholcov error - zStart = 2 * (f-1) + 1; zEnd = zStart + 1; + multiQm( zStart:zEnd, zStart:zEnd ) = round( Qm, 6 ); % prevent cholcov error zMeas( zStart:zEnd, 1 ) = z(1:2,f); zPred( zStart:zEnd, 1 ) = zp(1:2,1); end - if countFeatures > 0 - % if we saw anything, recalculate the final weight for the - % particle - P(p).w = max( 0.00001, mvnpdf(zPred,zMeas,multiQm)); - end - w(p) = P(p).w; - else - % no judgement info -- use the last weight - if length(P(p).w) ~= 1 - P(p).w = 1; - end + % if we saw anything, recalculate the final weight for the + % particle + P(p).w = max( 0.00001, mvnpdf(zPred,zMeas,multiQm)); w(p) = P(p).w; end @@ -102,14 +93,9 @@ for i = 1:countParticles seed = W(end) * rand(1); cur = find(W > seed,1); - % Pnew(i) = P(cur); Pnew(i).mu = P(cur).mu; Pnew(i).f_mu = P(cur).f_mu; Pnew(i).f_cov = P(cur).f_cov; - if length(P(cur).w) ~= 1 - P(p).w = 1; - end - Pnew(i).w = P(cur).w; end diff --git a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m index 2a3b403..ae9bf95 100644 --- a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m +++ b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m @@ -71,28 +71,29 @@ % calculate a possible belief for the particle's position covXt = inv(Hx' * Qm_inv * Hx + Rinv); mup_f = mup_f + covXt * Hx' * Qm_inv * zErr + mup; - + % update the Kalman gain and calculate the new feature % position and covariance. K = cov * Hm' * Qm_inv; 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 caluculated, and the two related values of Z. They'll be used + % 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. - qStart = size(Qm, 1) * (f - 1) + 1; - qEnd = qStart + size(Qm,1) - 1; - multiQm( qStart:qEnd, qStart:qEnd ) = round( Qm, 6 ); % prevent cholcov error - 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).w = max( 0.00001, mvnpdf(zMeas, zPred, multiQm)); % true belief is the average of all the guesses P(p).mu = mup_f / countFeatures; @@ -112,7 +113,6 @@ for i = 1:countParticles seed = W(end) * rand(1); cur = find(W > seed,1); - % Pnew(i) = P(cur); Pnew(i).mu = P(cur).mu; Pnew(i).f_mu = P(cur).f_mu; Pnew(i).f_cov = P(cur).f_cov; From 2233f5eabe02b07512f1e8f9ae947e18c6c1f0bf Mon Sep 17 00:00:00 2001 From: idlebear Date: Mon, 26 Feb 2018 15:01:59 -0500 Subject: [PATCH 3/5] Minor changes --- .../mr-6-fastSlam/fastSLAM.m | 2 +- .../06-mapping/fastSLAM_alt/fSLAM_2.m | 22 +++++++++---------- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m index 723aad0..f0c9917 100644 --- a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m +++ b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m @@ -25,7 +25,7 @@ numParticles = 1000; % Select the fast slam implementation to use (fSLAM or FSLAM_2) -fastSlamFn = @fSLAM; +fastSlamFn = @fSLAM_2; %% Main Code % Time diff --git a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m index ae9bf95..4b8698c 100644 --- a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m +++ b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m @@ -55,26 +55,28 @@ % 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 ); - Hm_inv = inv(Hm); - P(p).f_cov(:,covStart:covEnd) = Hm_inv * Q * Hm_inv'; + 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) - Qm_inv = inv(Qm); + 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' * Qm_inv * Hx + Rinv); - mup_f = mup_f + covXt * Hx' * Qm_inv * zErr + mup; + 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' * Qm_inv; + 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; @@ -93,17 +95,15 @@ % if we saw anything, recalculate the final weight for the % particle - P(p).w = max( 0.00001, mvnpdf(zMeas, zPred, multiQm)); - - % true belief is the average of all the guesses - P(p).mu = mup_f / countFeatures; + 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; +% featuresSeen(z(3,:)) = 1; end From ecb70699373be68c3394c2e6121641fb8d0225bf Mon Sep 17 00:00:00 2001 From: idlebear Date: Mon, 26 Feb 2018 20:32:35 -0500 Subject: [PATCH 4/5] Some spelling errors crept in (carried over from EKF) --- .../01-examples_lecture/mr-6-fastSlam/fastSLAM.m | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m index f0c9917..30fda97 100644 --- a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m +++ b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m @@ -1,11 +1,11 @@ -% Extended Kalman Filter SLAM example +%% FastSLAM and FastSLAM v.2 example clear;clc;close all %% Create AVI object makemovie = 0; if(makemovie) - vidObj = VideoWriter('ekfSLAM.mp4'); + vidObj = VideoWriter('fastSLAM.mp4'); vidObj.Quality = 100; vidObj.FrameRate = 2; open(vidObj); @@ -25,7 +25,7 @@ numParticles = 1000; % Select the fast slam implementation to use (fSLAM or FSLAM_2) -fastSlamFn = @fSLAM_2; +fastSlamFn = @fSLAM; %% Main Code % Time From b7ab4f139a46a00ef99394eec6ec8e944c9a1a86 Mon Sep 17 00:00:00 2001 From: idlebear Date: Mon, 19 Mar 2018 21:02:45 -0400 Subject: [PATCH 5/5] Reorganized change to overwrite the existing fastSLAM implmentation instead of making a parallel version. --- .../mr-6-fastSlam/fastSLAM.m | 16 +- .../06-mapping/fastSLAM/addFeature.m | 19 -- .../applyMotionModel.m | 0 .../calculateFeatureMeasurement.m | 0 .../calculateMeasurementAndJacobians.m | 0 .../06-mapping/fastSLAM/ekf_correct.m | 25 --- .../06-mapping/fastSLAM/featureSearch_fs.m | 36 ---- .../06-mapping/fastSLAM/func_fastSLAM.m | 169 ++++++++------- .../06-mapping/fastSLAM/func_fastSLAM2.m | 197 +++++++++++------- .../locateFeature.m | 0 .../fastSLAM/measurementUpdate_fs.m | 21 -- .../06-mapping/fastSLAM/motionUpdate_fs.m | 22 -- .../06-mapping/fastSLAM/plot_fs.m | 79 ------- .../06-mapping/fastSLAM/predict_features.m | 20 -- .../06-mapping/fastSLAM/raoBlackwell_fs.m | 37 ---- .../06-mapping/fastSLAM/run_fastSLAM.m | 106 ---------- .../06-mapping/fastSLAM/sample_proposal.m | 31 --- .../searchForFeatures.m | 0 .../06-mapping/fastSLAM/staticMap.mat | Bin 519 -> 0 bytes .../06-mapping/fastSLAM_alt/fSLAM.m | 102 --------- .../06-mapping/fastSLAM_alt/fSLAM_2.m | 122 ----------- 21 files changed, 229 insertions(+), 773 deletions(-) delete mode 100644 matlab_simulation/06-mapping/fastSLAM/addFeature.m rename matlab_simulation/06-mapping/{fastSLAM_alt => fastSLAM}/applyMotionModel.m (100%) rename matlab_simulation/06-mapping/{fastSLAM_alt => fastSLAM}/calculateFeatureMeasurement.m (100%) rename matlab_simulation/06-mapping/{fastSLAM_alt => fastSLAM}/calculateMeasurementAndJacobians.m (100%) delete mode 100644 matlab_simulation/06-mapping/fastSLAM/ekf_correct.m delete mode 100644 matlab_simulation/06-mapping/fastSLAM/featureSearch_fs.m rename matlab_simulation/06-mapping/{fastSLAM_alt => fastSLAM}/locateFeature.m (100%) delete mode 100644 matlab_simulation/06-mapping/fastSLAM/measurementUpdate_fs.m delete mode 100644 matlab_simulation/06-mapping/fastSLAM/motionUpdate_fs.m delete mode 100644 matlab_simulation/06-mapping/fastSLAM/plot_fs.m delete mode 100644 matlab_simulation/06-mapping/fastSLAM/predict_features.m delete mode 100644 matlab_simulation/06-mapping/fastSLAM/raoBlackwell_fs.m delete mode 100644 matlab_simulation/06-mapping/fastSLAM/run_fastSLAM.m delete mode 100644 matlab_simulation/06-mapping/fastSLAM/sample_proposal.m rename matlab_simulation/06-mapping/{fastSLAM_alt => fastSLAM}/searchForFeatures.m (100%) delete mode 100644 matlab_simulation/06-mapping/fastSLAM/staticMap.mat delete mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m delete mode 100644 matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m diff --git a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m index 30fda97..0c4a773 100644 --- a/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m +++ b/matlab_simulation/01-examples_lecture/mr-6-fastSlam/fastSLAM.m @@ -21,15 +21,25 @@ % features Example=1; + % The number of particles to use in the simulation numParticles = 1000; -% Select the fast slam implementation to use (fSLAM or FSLAM_2) -fastSlamFn = @fSLAM; +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 = 50; +Tf = 25; dt = 0.5; T = 0:dt:Tf; 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_alt/applyMotionModel.m b/matlab_simulation/06-mapping/fastSLAM/applyMotionModel.m similarity index 100% rename from matlab_simulation/06-mapping/fastSLAM_alt/applyMotionModel.m rename to matlab_simulation/06-mapping/fastSLAM/applyMotionModel.m diff --git a/matlab_simulation/06-mapping/fastSLAM_alt/calculateFeatureMeasurement.m b/matlab_simulation/06-mapping/fastSLAM/calculateFeatureMeasurement.m similarity index 100% rename from matlab_simulation/06-mapping/fastSLAM_alt/calculateFeatureMeasurement.m rename to matlab_simulation/06-mapping/fastSLAM/calculateFeatureMeasurement.m diff --git a/matlab_simulation/06-mapping/fastSLAM_alt/calculateMeasurementAndJacobians.m b/matlab_simulation/06-mapping/fastSLAM/calculateMeasurementAndJacobians.m similarity index 100% rename from matlab_simulation/06-mapping/fastSLAM_alt/calculateMeasurementAndJacobians.m rename to matlab_simulation/06-mapping/fastSLAM/calculateMeasurementAndJacobians.m 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_alt/locateFeature.m b/matlab_simulation/06-mapping/fastSLAM/locateFeature.m similarity index 100% rename from matlab_simulation/06-mapping/fastSLAM_alt/locateFeature.m rename to matlab_simulation/06-mapping/fastSLAM/locateFeature.m 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_alt/searchForFeatures.m b/matlab_simulation/06-mapping/fastSLAM/searchForFeatures.m similarity index 100% rename from matlab_simulation/06-mapping/fastSLAM_alt/searchForFeatures.m rename to matlab_simulation/06-mapping/fastSLAM/searchForFeatures.m diff --git a/matlab_simulation/06-mapping/fastSLAM/staticMap.mat b/matlab_simulation/06-mapping/fastSLAM/staticMap.mat deleted file mode 100644 index c19127c90eecd2ef12d26d85b9b3f3f3b86cfa0b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 519 zcmeZu4DoSvQZUssQ1EpO(M`+DN!3vZ$Vn_o%P-2cQV4Jk_w+L}(NSyj5-`93qo*%FkX_Hnz)&&gadJX}MnXzL z0`nyq9swzqh=nz33Sx{&Zft37caAWz`6=9HToG42vE)Ij@H1wyuD6fQOFfk_5|fL| zd;7np{$!u}^+op$te!04oL9grQsR;Lc>YQ`&G!XM&OeBAKc8PcxpVoo8qSHO0UdVf zufqRV*593Y_i`G?zs>$1?tQ)e_a9^RRBhGQx8nBSlQ&g=@%5nHL1v*xr~7t_+}#>p zu`N`)=l7F;s{gLXeV<#u`M#Uu%b$M(&M|X)mAjaDov_~V$6))c*#}=1Bo`V*U*8n} zO@Ef1R$b!1z46ETR~MdMJ^!1$+P{tQU;cmoXEOI>r+E1(%QO5Hzmx9DUt_-M*w6Iq zQQh189Xd+`jNi}Rds4J`+LqTre8EZU^P*S$NdCXnb^1M#8~<+UCw$M+I(+o`w?m8{ zrTyc7Zm@5-UL4mZcDd1RyVh!B?HMvw_LZ~Ne-*X}*l_7v;6t0bcFA|FuM+k5-SB^S i%QpWkOJ=*Pu#&h~D5uqpJ$7doZ1y?D$RKLHU^)Or9^Kdg diff --git a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m deleted file mode 100644 index 5c332e6..0000000 --- a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM.m +++ /dev/null @@ -1,102 +0,0 @@ -function [Pnew, featuresSeen] = fSLAM( 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); - - for p = 1:countParticles - - % move the particle forward in time - P(p).mu = applyMotionModel( P(p).mu, u, noiseFactor, dt ); - - countFeatures = size(z,2); - if countFeatures == 0 - % no judgement info -- use the last weight - w(p) = P(p).w; - 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)); - - 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 - P(p).f_mu(:,featureIndex) = locateFeature( P(p).mu, z(1:2,f)); - [zp, H] = calculateFeatureMeasurement( P(p).f_mu(:,featureIndex), P(p).mu ); - H_inv = inv(H); - P(p).f_cov(:,covStart:covEnd) = H_inv * Q * H_inv'; - else - [zp, H] = calculateFeatureMeasurement( P(p).f_mu(:,featureIndex), P(p).mu ); - end - - cov = P(p).f_cov(:,covStart:covEnd); - Qm = H*cov*H'+Q; - K = cov * H' * inv(Qm); - P(p).f_mu(:,featureIndex) = P(p).f_mu(:,featureIndex) + K*(z(1:2,f) - zp); - P(p).f_cov(:,covStart:covEnd) = (eye(2)-K*H)*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; - multiQm( zStart:zEnd, zStart:zEnd ) = round( Qm, 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)); - w(p) = P(p).w; - end - - end - - if exist( 'z', 'var' ) && size(z,1) == 3 - % mark features as seen - featuresSeen(z(3,:)) = 1; - end - - - % 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_alt/fSLAM_2.m b/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m deleted file mode 100644 index 4b8698c..0000000 --- a/matlab_simulation/06-mapping/fastSLAM_alt/fSLAM_2.m +++ /dev/null @@ -1,122 +0,0 @@ -function [Pnew, featuresSeen] = fSLAM( 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); - - Rinv = inv(R); - - 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 - - - % 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