Poem Analysis: SLAM Technim With Emlation And Graphics

% SLAM2D A 2D EKF?SLAM algorithm with simulation and graphics.
% SIMULATOR
W = cloister(-4,4,-4,4,7); % set of external landmarks
N = size(W,2); % N: number of landmarks
R = [0;-2;0]; % R: robot pose [x ; y ; alpha]
U = [0.1; 0.05]; % fixing advance and turn increments creates a circle
Y = zeros(2, N); % Y: measurements of all landmarks
% ESTIMATOR x = zeros(numel(R)+numel(W), 1); % x: state vector 's mean
P = zeros(numel(x),numel(x)); % P: state vector 's covariances matrix q = [.01;.02]; % amplitude or standard deviation of system noise
Q = diag(q.^2); % covariances matrix s = [.1;1*pi/180]; % amplitude or standard deviation-Noise measurement
S = diag(s.^2); % covariances matrix mapspace = false(1,numel(x)); % Map management landmarks = …show more content…
'color ', 'b ',... 'xdata ',[ ],... 'ydata ',[ ]);
% Estimated landmark ellipses, green leG = zeros(1,N); for i = 1:numel(leG) leG(i) = line(... 'linestyle ', '- ',... 'marker ', 'none ',... 'color ', 'g ',... 'xdata ',[ ],... 'ydata ',[ ]); end for t = 1:200 % LOOP n = q .* randn(2,1); % perturbation vector
R = move(R, U, zeros(2,1) ); % we will perturb the estimator for i = 1:N % i: landmark index v = s .* randn(2,1); % measurement noise
Y(:,i) = observe(R, W(:,i)) + v; end m = landmarks(landmarks ~= 0) '; % all pointers to landmarks rm = [r , m]; % all used states: robot and landmarks
[x(r), R_r, R_n] = move(x(r), U, n); % Estimator perturbed with n
P(r,m) = R_r * P(r,m); % See PDF notes 'SLAM course.pdf '
P(m,r) = P(r,m) ';
P(r,r) = R_r * P(r,r) * R_r ' + R_n * Q * R_n '; lids = find( landmarks(1,:) ); % returns all indices of existing landmarks for i = lids l = landmarks(:, i) '; % landmark pointer
[e, E_r, E_l] = observe(x(r), x(l) ); % this is h(x) in EKF rl = [r , l]; % pointers to robot and lmk.
E_rl = [E_r , E_l]; % expectation Jacobian
E = E_rl * P(rl, rl) * E_rl ';
Yi = Y(:, i); % measurement of landmark i z = Yi-e; % this is z = y-h(x) in EKF if z(2) > pi % we need values around zero for …show more content…
if z ' * Z^-1 * z < 9 % Kalman gain K = P(rm, rl) * E_rl ' * Z^-1; % this is K = P*H '*Zˆ-1 in EKF x(rm) = x(rm) + K*z; % map update using pointer rm P(rm,rm) = P(rm,rm)-K*Z*K '; end end lids = find(landmarks(1,:)==0); % all non?initialized landmarks if -isempty(lids) % there are still landmarks to initialize i = lids(randi(numel(lids))); % pick one landmark randomly, its index is i l = find(mapspace==false, 2); % pointer of the new landmark in the map if -isempty(l) % there is still space in the map mapspace(l) = true; % block map space landmarks(:,i) = l; % store landmark pointers
Yi = Y(:,i); % measurement
[x(l), L_r, L_y] = invObserve(x(r), Yi); % initialization
P(l,rm) = L_r * P(r,rm);
P(rm,l) = P(l,rm) ';
P(l,l) = L_r * P(r,r) * L_r ' + L_y * S * L_y '; end end
Rshape = fromFrame(R, Rshape0); % GRAPHICS Simulated robot set(RG, 'xdata ', Rshape(1,:), 'ydata ', Rshape(2,:));
Rshape = fromFrame(x(r), Rshape0); % Estimated robot set(rG, 'xdata ', Rshape(1,:), 'ydata ', Rshape(2,:)); re = x(r(1:2)); % robot position mean
RE = P(r(1:2),r(1:2)); % robot position covariance
[xx,yy] = cov2elli(re,RE,3,16); % x and y coordinates of contour set(reG, 'xdata ', xx, 'ydata ', yy); lids = find(landmarks(1,:)); % all indices of mapped