slam_if_2d.m

这是移动机器人同时定位与地图创建的第三部分

%
% SLAM_IF_2D performs an interactive simulation of SLAM with information filter.
%   SLAM_IF_2D() is a two-dimensional linear simulation of Simultaneous 
%   Localization and Mapping (SLAM) with the information filter. It is meant
%   to serve as an introduction to the basic steps of IF SLAM and to explore
%   some important traits of feature-based SLAM information filters.
%
%   This code was written by Matthew R. Walter (Massachusetts Institute of
%   Technology) as part of a 2006 SLAM Summer School lab on the information
%   filter. Feel free to play with the parameter settings to see how they
%   affect the information filter.
%
%   For the most part, I have tried to be pretty verbose when it comes
%   to my comments and hope that they help you to understand what is going on.
%   Also, while there were not any obvious bugs upon my final writing, I can
%   not guarantee that the code is free of errors.
%
% ------------
% References:
%
%   R. Eustice, H. Singh, and J. Leonard. Exactly Sparse Delayed State
%   Filters. ICRA 2005.
%
%   M. Walter, R. Eustice, and J. Leonard. A Provably Consistent Method
%   for Imposing Exact Sparsity in Feature-based SLAM Information Filters.
%   ISRR 2005.
%
%-----------------------------------------------------------------
%    History:
%    Date            Who         What
%    -----------     -------     -----------------------------
%    08-10-2006      mrw         Created and written.

function slam_if_2d()


% Clear variables
clear global;
clear all;

global TheJournal;
global Truth;
global Graphics;
global Params;
global Data;


Params.verbose = input('Enter 1 for verbose output, else 0: ');





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Simulation Parameters

Params.PlotPeriod = 20;
Params.Verbose = 0;
Params.PlotSwitch = 1;

% System Parameters
Params.Q = [0.15 0.1; 0.1 0.15].^2;
Params.R = [0.2 0.1; 0.1 0.2].^2;


Params.MaxObsRange = 10;
Params.nObsPerIteration = 8;
Params.density = 0.05;      % density of features


% The information form utilizes the inverse of Q and R in sparse form
Params.Q = sparse(Params.Q);
Params.R = sparse(Params.R);
Params.Qinv = inv(Params.Q);
Params.Rinv = inv(Params.R);



Params.MapSize = 20;        % width and height of the map
Params.nRepeat = 3;         % number of times that the vehicle goes around the loop
Params.maxVelocity = 0.5;   

Params.numFeatures = floor(Params.density*Params.MapSize^2);
Params.FeatureSpacing = ((Params.MapSize^2)/Params.numFeatures)/5;


Params.ObsFrequency = 1;        % [0,1] likelihood of observations




%
% Filter initialization

Sigma = diag([0.001 0.001]);     % initial 2x2 covariance matrix
x = [0; 0];                      % initial state

% Initial information matrix, Lambda = inv(Sigma), and information vector, eta = Lambda*x
% We store the information matrix in sparse form and allocate all the memory beforehand.
% As a result, size(Lambda,1) is proportional to the total number of features in the environment
% and not the size of the current state, which is 2. This speeds things up by allowing us to avoid
% some overhead in Matlab but requires that we carry around a list of indices of where stuff is in this
% large matrix. Right now we only have 2 states, so our index is [1; 2]
TheJournal.II = [1:2]';
TheJournal.Lambda = spalloc(2*(1+Params.numFeatures),2*(1+Params.numFeatures),(2*(1+Params.numFeatures))^2);
Temp = sparse(inv(Sigma));
Temp = (Temp + Temp')/2;
TheJournal.Lambda(1:2,1) = Temp(:,1);
TheJournal.Lambda(1:2,2) = Temp(:,2);
TheJournal.eta = TheJournal.Lambda(TheJournal.II,TheJournal.II)*x;


% A lookup table to keep track of feature id's and their corresponding location within the state vector
TheJournal.LookUP = [NaN 1; 1 1];


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%







if(Params.PlotSwitch)
    Graphics.MainFigure = figure(1);
    clf;
    subplot(1,2,1)
    hold on;
    axis square;
    set(gcf,'DoubleBuffer','on','Color',[1 1 1]);
    set(gca,'Box','on');
    
    subplot(1,2,1)
    cla;
    set(gcf,'ColorMap',colormap(flipud(colormap('gray'))));
    
    if(Params.verbose)
        Graphics.SecondFigure = figure(2);
        clf;
        Temp = get(gcf,'Position');
        Temp(3:4) = [300 300];
        set(gcf,'ColorMap',colormap(flipud(colormap('gray'))),'Position',Temp);
    end;
        
    figure(Graphics.MainFigure);
    
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Graphic settings
    Graphics.TrueColor = 'g';
    Graphics.Color = 'r';

    Graphics.curLaser = [];
    Graphics.TrueVehiclePose = [];  % handle for plot of true vehicle pose
    Graphics.VehiclePose = [];      % handle for plot of filter estimate for vehicle pose
    Graphics.VehicleEllipse = [];   %   "     "    "  "     "   pose estimate uncertainty ellipse
    Graphics.Features = [];         % handle for plot of filter estimates for feature locations
    Graphics.FeatureEllipse = [];   % handle for plot of feature uncertainty ellipse
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate the predicted and true motion (i.e. noise-corrupted) of the vehicle
MakeSurvey();
% Place the features in the world
MakeFeatures();
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Various logs for post-analysis
TheJournal.Feature = [];
TheJournal.ProjectionTime = [];
TheJournal.UpdateTime = [];


% The vehicle history log keeps track of the estimated vehicle pose error together with
% the diagonal of the Sigma_vehicle sub-block and the chi-square error.
% Each column corresponds to a different time
verror = x - Truth.VehicleTrajectory(2:3,1);
TheJournal.VehicleHistory = [1; verror; diag(Sigma); verror'*inv(Sigma)*verror];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%   NOW WE LOOP OVER THE SIMULATION AND IMPLEMENT THE FILTER


tstep = floor(0.1*Truth.VehicleTrajectory(1,end));  % time step simply for printing progress update


for k=2:size(Truth.VehicleTrajectory,2)

    if(~mod(k,tstep))
        fprintf(1,'Simulation is at k = %i, %.2f%% done \n',k,100*k/Truth.VehicleTrajectory(1,end));
    end;


    if(Params.PlotSwitch & ~isempty(Graphics.curLaser))
        set(Graphics.curLaser,'Visible','off');
    end;


    % Perform the motion update in the information form as the addition of the new pose
    % to the state followed by the marginalization of the past pose
    Information_DoProjection(k);


    % update the plot of the vehicle and map;
    if(Params.PlotSwitch)
        PlotState(k);
        title(['k = ' int2str(k) ' ( ' num2str(100*k/size(Truth.VehicleTrajectory,2),2) ' percent )']);
        drawnow;
    end;
    
    
    % Simulate feature observations
    Observations = [];
    if(rand <= Params.ObsFrequency)
        Observations = GetFeatureObservations(k);
    end;

    if(~isempty(Observations))

        % data association is given by the first row
        DA = Observations(1,:);

        % Differentiate between features that have already been mapped and those that are new
        if(size(TheJournal.LookUP,1) > 2)
            [C,IA,IB] = intersect(DA,TheJournal.LookUP(3:end,1));
            OldObs = Observations(:,IA);
            
            [C,IA] = setdiff(Observations(1,:),OldObs(1,:));
            NewObs = Observations(:,IA);
        else
            OldObs = [];
            NewObs = Observations;
        end;
        
        % Visualize observations
        if(Params.PlotSwitch)
            xv = GetMean(1:2);  % Estimate of the current vehicle pose
            [C,IA,IB] = intersect(Truth.Features(1,:),Observations(1,:));

            X = [repmat(xv(1),1,size(Observations,2)); Truth.Features(2,IA)];
            Y = [repmat(xv(2),1,size(Observations,2)); Truth.Features(3,IA)];
            
            figure(Graphics.MainFigure);
            subplot(1,2,1);
            Graphics.curLaser = plot(X,Y,'y');
            set(Graphics.curLaser,'Color',[0.268 0.686 1.00]);
        end;
            
        
        % If the vehicle has observed new features, add them
        if(~isempty(NewObs))
            Information_AddFeature(NewObs);
        end;
        
        % If we have reobserved known features, run an update step
        if(~isempty(OldObs))
            Information_DoUpdate(OldObs);
        end;
    end;


    % update the plot of the vehicle and map;
    if(Params.PlotSwitch)
        PlotState(k);
        title(['k = ' int2str(k) ' ( ' num2str(100*k/size(Truth.VehicleTrajectory,2),2) ' percent )']);
        drawnow;
        ShowLinkStrength();
    end;
   
    if(~mod(k,5))
        DoLogging(k)
    end;

    if(~issparse(TheJournal.Lambda))
        fprintf(1,'k = %i : Lambda is no longer sparse \n',k);
    end;

end;



% Plot the history for the shared information between the first 5 features and the robot.
figure(3);
clf;
hold on;
cmap = [colormap('hot'); colormap('jet'); colormap('gray')];
Temp = randperm(size(cmap,1));

Handles = [];
IDs = [];
for i=1:min(size(TheJournal.Feature,2),5)
    IDs = [IDs; TheJournal.Feature{i}.id];
    thandle = plot(TheJournal.Feature{i}.Link(1,:),(abs(TheJournal.Feature{i}.Link(2,:))),'k-');
    set(thandle,'Color',cmap(Temp(i),:));
    Handles(end+1) = thandle;
end;
    
legend(Handles,int2str(IDs));
xlabel('Time','FontSize',14);
ylabel('Link Strength','FontSize',14);
title('Shared Feature-Vehicle Information vs Time','FontSize',14);
axis tight;
set(gca,'box','on','yscale','log','ylim',[0 Truth.VehicleTrajectory(1,end)],'FontSize',14);


return;






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function executes the motion update for the information filter by forming it as the addition
% of the new pose to the state vector followed by the marginalization of the previous pose
function Information_DoProjection(k);

global Params;
global TheJournal;
global Data;
global Graphics;


% Motion model:
%   x_new = x_old + velocity_x * delta_time
%   y_new = y_old + velocity_y * delta_time

% Jacobian
F = speye(2);

% Velocity vector
u = Data.Velocity(2:3,find(Data.Velocity(1,:)==k));


LambdaOld = TheJournal.Lambda(TheJournal.II,TheJournal.II);


%%%%%%%%%%%%%%%
% The time projection operation for the information matrix and information vector
% as described in Eustice et al. (eustice05a) Section III.C.
tic;
Psi = inv(Params.Q + F*inv(TheJournal.Lambda(1:2,1:2))*F');
Omegainv = inv(TheJournal.Lambda(1:2,1:2) + F*Params.Qinv*F');

eta1 = Params.Qinv*F*Omegainv*TheJournal.eta(1:2) + Psi*u;
if(length(TheJournal.eta) > 2)
    eta2 = TheJournal.eta(3:end) - TheJournal.Lambda(3:TheJournal.II(end),1:2)*Omegainv*(TheJournal.eta(1:2)-F'*Params.Qinv*u);

    Lambda12 = Params.Qinv*F*Omegainv*TheJournal.Lambda(1:2,3:TheJournal.II(end));
    Lambda22 = TheJournal.Lambda(3:TheJournal.II(end),3:TheJournal.II(end)) - TheJournal.Lambda(3:TheJournal.II(end),1:2)*Omegainv*TheJournal.Lambda(1:2,3:TheJournal.II(end));

    TheJournal.eta = [eta1; eta2];
    TheJournal.Lambda(TheJournal.II,TheJournal.II) = [Psi Lambda12; Lambda12' Lambda22];
else
    TheJournal.eta = eta1;
    TheJournal.Lambda(TheJournal.II,TheJournal.II) = Psi;
end
comptime = toc;
TheJournal.ProjectionTime = [TheJournal.ProjectionTime [k; length(TheJournal.II); comptime]];
%%%%%%%%%%%%%%%


% Update the timestamp in the LookUP table
TheJournal.LookUP(1,2) = k;


if(Params.verbose)
    fprintf(1,'\n\n');
    fprintf(1,'----------------------- \n\n');
    fprintf(1,'TIME PROJECTION STEP: \n\n')
    fprintf(1,'    The IF has just performed the time prediction step. \n');
    fprintf(1,'    Compare the new information matrix in Figure 1 to the\n');
    fprintf(1,'    structure beforehand as shown in Figure 2.\n');
    ShowLinkStrength(TheJournal.Lambda(TheJournal.II,TheJournal.II),0);
    ShowLinkStrength(LambdaOld,0,Graphics.SecondFigure);
    pause;
    fprintf(1,'----------------------- \n\n');
end;



return;







%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function to perform update step for information filter
function Information_DoUpdate(Observations)

global TheJournal;
global Params;
global Graphics;


k = TheJournal.LookUP(1,end);
xi = GetStateIndex(1,k);


% Measurement model:
%   z_x = x_f - x_v
%   z_y = y_f - y_v
% Note: with a linear observation model, we don't need the mean

LambdaOld = TheJournal.Lambda(TheJournal.II,TheJournal.II);

% Loop over the measurement data, performing an individual update for each observation
H_I = []; H_J = []; H_S = [];
Rinv_I = []; Rinv_J = []; Rinv_S = [];
Z = [];
II = [1:2]';
for i=1:size(Observations,2)
    id = Observations(1,i);
    xfi = GetStateIndex(id,k);
        
    %%%%%%%%%%%%%%%
    % The update step for the information filter as described
    % in Eustice et al. (eustice05a) Section III.B.
    
    
    temp = 2*i - 1;
    H_I = [H_I; [temp; temp; temp+1; temp+1]];
    H_J = [H_J; [1; temp+2; 2; (temp+2)+1]];
    H_S = [H_S; [-1; 1; -1; 1]];
    
    II = [II; [xfi; xfi+1]];
    
    Rinv_I = [Rinv_I; [temp; temp; temp+1; temp+1]];
    Rinv_J = [Rinv_J; [temp; temp+1; temp; temp+1]];
    Rinv_S = [Rinv_S; Params.Rinv(:)];
    
    Z = [Z; Observations(2:3,i)];
    
    %%%%%%%%%%%%%%%
end;

H = sparse(H_I,H_J,H_S,max(H_I),max(H_J));
RINV = sparse(Rinv_I,Rinv_J,Rinv_S);

tic;
TheJournal.Lambda(II,II) = TheJournal.Lambda(II,II) + H'*RINV*H;
TheJournal.eta(II) = TheJournal.eta(II) + H'*RINV*Z;
comptime = toc;

TheJournal.UpdateTime = [TheJournal.UpdateTime [k; length(TheJournal.II); comptime]];


if(Params.verbose)
    fprintf(1,'\n\n');
    fprintf(1,'----------------------- \n\n');
    fprintf(1,'MEASUREMENT UPDATE STEP: \n\n')
    fprintf(1,'    The IF has just performed the update step based upon observations \n');
    fprintf(1,'    of features: ');
    disp(Observations(1,:)); fprintf(1,'\n');
    fprintf(1,'    Compare the new information matrix in Figure 1 to the\n');
    fprintf(1,'    structure beforehand as shown in Figure 2.\n');
    ShowLinkStrength(TheJournal.Lambda(TheJournal.II,TheJournal.II),0);
    ShowLinkStrength(LambdaOld,0,Graphics.SecondFigure);
    pause;
    fprintf(1,'----------------------- \n\n');
end;





return;


    
            


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function to add features
function Information_AddFeature(Observations)

global TheJournal;
global Params;
global Graphics;


% Measurement model:
%   z_x = x_f - x_v
%   z_y = y_f - y_v

% Jacobian
G = speye(2);

xi = GetStateIndex(1,TheJournal.LookUP(1,end));