slam_if_2d.m

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

if(Params.verbose)
    ShowLinkStrength(TheJournal.Lambda(TheJournal.II,TheJournal.II),0,Graphics.SecondFigure);
end;


% Note that, in the linear case, we don't need the mean to add the feature
% xf = g(xv,z) + w = (mu_v + z) + w
for i=1:size(Observations,2)
    id = Observations(1,i);
    
    II_new = TheJournal.II(end-1:end) + 2;
    
    %%%%%%%%%%%%%%%
    % The operation of adding features with the information form as described
    % in Eustice et al. (eustice05a) Section III.A.
    
    TheJournal.Lambda(1:2,II_new) = -G'*Params.Rinv;
    TheJournal.Lambda(II_new,1:2) = -Params.Rinv*G;
    TheJournal.Lambda(II_new,II_new) = Params.Rinv;
    TheJournal.Lambda(1:2,1:2) = TheJournal.Lambda(1:2,1:2) + G'*Params.Rinv*G;

    etaf_new = Params.Rinv*Observations(2:3,i);

    TheJournal.eta = [TheJournal.eta; etaf_new];
    TheJournal.eta(1:2) = TheJournal.eta(1:2) - G'*Params.Rinv*Observations(2:3,i);
    
    TheJournal.II = [TheJournal.II; II_new];
    %%%%%%%%%%%%%%%
    
    % Plotting stuff
    if(Params.PlotSwitch)
        xyf = GetMean(1:2) + Observations(2:3,i);
        figure(Graphics.MainFigure);
        subplot(1,2,1);
        thandle = plot(xyf(1),xyf(2),'rx');

        Graphics.Features = [Graphics.Features [id; thandle]];
    end;
        
    % Keep track of the shared information between the robot and feature
    TheJournal.Feature{end+1}.id = id;
    LambdaNorm = rhomatrix(TheJournal.Lambda(TheJournal.II,TheJournal.II));
    TheJournal.Feature{end}.Link = [TheJournal.LookUP(1,2); det(LambdaNorm(1:2,end-1:end))];
    
    % Now, update the LookUP table
    TheJournal.LookUP = [TheJournal.LookUP; id NaN*ones(1,size(TheJournal.LookUP,2)-1)];
    TheJournal.LookUP(end,end) = length(TheJournal.eta)-1;
end;
    

if(Params.verbose)
    fprintf(1,'\n\n');
    fprintf(1,'+++++++++++++++++++++++ \n\n');
    fprintf(1,'ADDING FEATURE(S): \n\n')
    fprintf(1,'    We have added features: ');
    disp(Observations(1,:));
    fprintf(1,'\n\n');
    fprintf(1,'    Notice that they share information only with the robot state\n\n');
    fprintf(1,'+++++++++++++++++++++++ \n\n');
    ShowLinkStrength(TheJournal.Lambda(TheJournal.II,TheJournal.II),0);
    pause;
end;


return;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function which pictoralizes links in the information matrix
function ShowLinkStrength(Lambda,twoposeflag,fhandle)

global TheJournal;
global Graphics

if(nargin <2)
    twoposeflag = 0;
end;

% We are intersted in the normalized information matrix
if(nargin==0)
    LambdaNorm = rhomatrix(TheJournal.Lambda(TheJournal.II,TheJournal.II));
else
    LambdaNorm = rhomatrix(Lambda);
end;

% The strength of each link is proportional to the determinant of the
% corresponding 2x2 submatrix of Lambda. In general, the determinant
% of the matrices along the diagonal of Lambda (which represent self-links)
% will be much larger and we "tone them down" for the sake of rendering.
ILinks = sparse(size(LambdaNorm,1)/2,size(LambdaNorm,2)/2);
for i=1:2:size(LambdaNorm,1)
    for j=(i):2:size(LambdaNorm,2)
        temp = det(LambdaNorm(i:(i+1),j:(j+1)));
        ILinks((i+1)/2,(j+1)/2) = temp;
        ILinks((j+1)/2,(i+1)/2) = temp;
    end;
end;

if(size(ILinks,1) > 2)
    %ILinks = spdiags(max(ILinks(:))*ones(size(ILinks,1),1),0,ILinks);
end;

if(nargin < 3)
    figure(Graphics.MainFigure);
    subplot(1,2,2);
else
    figure(fhandle);
end;

cla;
imagesc(ILinks);
axis image;
hold on;
set(gca,'xtick',1.5:1:(size(ILinks,1)-0.5),'ytick',1.5:1:(size(ILinks,1)-0.5));

set(gca,'xtick',[],'ytick',[],'box','on');
xlim = get(gca,'xlim');
ylim = get(gca,'ylim');

[X,Y] = meshgrid(xlim(1):1:xlim(2),xlim(1):1:xlim(2));
plot(X,Y,'r-');
plot(Y,X,'r-');

temp = int2str(TheJournal.LookUP(3:end,1));
if(twoposeflag)
    temp = strvcat('v+','v-',temp);
else
    temp = strvcat('v+',temp);
end;

X = 1:size(ILinks,2);
Y = repmat(0.2,1,length(X));
text(X,Y,temp);
text(Y-0.05*X(end),X,temp)


[I,J] = find(ILinks);
plot(I,J,'k.');
xlabel('Dots denote nonzero entries','Fontsize',10);


return;





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% This function records data
function DoLogging(k)

global TheJournal;
global Params;
global Truth;


if(size(TheJournal.LookUP,1) == 2)
    return;
end;


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

for i=3:size(TheJournal.LookUP,1)
    xfi = TheJournal.LookUP(i,2);
    
    TheJournal.Feature{i-2}.Link(:,end+1) = [k; det(Lambda(1:2,xfi:(xfi+1)))];
end;



% Enable to log errors in the state estimates
if(0)
    % Calculate the covariance matrix and mean via brute-force inversion of the information matrix
    Sigma = inv(TheJournal.Lambda(TheJournal.II,TheJournal.II));
    Sigma = (Sigma + Sigma')/2;     % Ensures symmetry
    x = Sigma*TheJournal.eta;


    % Log vehicle data
    xv_truth = Truth.VehicleTrajectory(2:3,find(Truth.VehicleTrajectory(1,:)==k));
    verror = x(1:2) - xv_truth;
    chisqr_error = verror'*inv(Sigma(1:2,1:2))*verror;

    TheJournal.VehicleHistory = [TheJournal.VehicleHistory [k; verror; diag(Sigma(1:2,1:2)); chisqr_error]];



    if(0 & size(TheJournal.LookUP,1) > 2)

        TheJournal.MapError = [TheJournal.MapError [k; 0]];

        nFeatures = size(TheJournal.LookUP,1)-2;

        for i=3:size(TheJournal.LookUP,1)
            id = TheJournal.LookUP(i,1);
            xi = GetStateIndex(id,k);

            % Error in estimated feature position
            xf_truth = Truth.Features(2:3,find(Truth.Features(1,:)==id));
            feat_error = x(xi:xi+1)-xf_truth;

            %TheJournal.Feature{id}.History = [TheJournal.Feature{id}.History [k; feat_error; diag(Sigma(xi:xi+1,xi:xi+1)); feat_error'*inv(Sigma(xi:xi+1,xi:xi+1))*feat_error]];

            % Keep track of map convergence
            TheJournal.MapError(2,end) = TheJournal.MapError(2,end) + (feat_error'*feat_error)/nFeatures;
        end;

    end;
end;


return;




    


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function to extract mean state estimates
%
function x = GetMean(xi)

global TheJournal;


% Right now, fully invert the information matrix to get the mean
mu = TheJournal.Lambda(TheJournal.II,TheJournal.II)\TheJournal.eta;
x = mu(xi);
    
return;





%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function to generate feature observations
function Observations = GetFeatureObservations(k)

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

Observations = [];

xv = Truth.VehicleTrajectory(2:3,find(Truth.VehicleTrajectory(1,:)==k));
vvel = Data.Velocity(2:3,find(Data.Velocity(1,:)==k));

Temp = sqrt(sum((Truth.Features(2:3,:)-repmat(xv,1,size(Truth.Features,2))).^2));
Temp = [Truth.Features(1,:); Temp];
Temp = Temp(1,find(Temp(2,:) <= Params.MaxObsRange));

    
% Generate correlated measurement noise using the Cholesky Decomposition: R = X'*X
% for which: w = U*randn(2,1) ---> cov(w) = U*I*U' = U*U' = R;  (U*U'=R=X'X --> U=X')
U = chol(Params.R)';


if(length(Temp) >= Params.nObsPerIteration)
    while(size(Observations,2) < Params.nObsPerIteration)
        i = ceil(rand*length(Temp));
        fid = Temp(i);
        Temp(i) = [];
        
        xf = Truth.Features(2:3,find(Truth.Features(1,:)==fid));
        ObsNoise = (xf-xv) + U*randn(2,1);
        Observations = [Observations [fid; ObsNoise]];
    end;
else
    for i=1:length(Temp)
        fid = Temp(i);
        
        xf = Truth.Features(2:3,find(Truth.Features(1,:)==fid));
        ObsNoise = (xf-xv) + U*randn(2,1);
        Observations = [Observations [fid; ObsNoise]];
    end;
end;

return;
        



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function PlotState(k);

global TheJournal;
global Graphics;
global Truth;

figure(Graphics.MainFigure);
subplot(1,2,1)

% Ground truth vehicle plot
x = Truth.VehicleTrajectory(2:3,k);
if(isempty(Graphics.TrueVehiclePose))
    Graphics.TrueVehiclePose = plot(x(1),x(2),'ko','MarkerFaceColor','r','MarkerSize',6);
    set(Graphics.TrueVehiclePose,'Color',Graphics.TrueColor);
else
    set(Graphics.TrueVehiclePose,'xdata',x(1),'ydata',x(2));
end;


% Brute-force estimate of the covariance matrix and mean vector
Sigma = TheJournal.Lambda(TheJournal.II,TheJournal.II)\eye(length(TheJournal.II));
x = Sigma*TheJournal.eta;

if(isempty(Graphics.VehiclePose))
    Graphics.VehiclePose = plot(x(1),x(2),'k^','MarkerFaceColor','r','MarkerEdgeColor','k','MarkerSize',8);
    set(Graphics.VehiclePose,'Color',Graphics.Color);
else
    lastvx = get(Graphics.VehiclePose,'xdata');
    lastvy = get(Graphics.VehiclePose,'ydata');

    set(Graphics.VehiclePose,'xdata',x(1),'ydata',x(2));
end;


if(isempty(Graphics.VehicleEllipse))
    Graphics.VehicleEllipse = DrawEllipse([x(1); x(2)],Sigma(1:2,1:2),3,Graphics.Color);
else
    [Sigmax,Sigmay] = GetEllipse([x(1); x(2)],Sigma(1:2,1:2),3);
    set(Graphics.VehicleEllipse,'xdata',Sigmax,'ydata',Sigmay);
end;

for i=1:size(Graphics.Features,2)
    id = Graphics.Features(1,i);
    thandle = Graphics.Features(2,i);

    xfi = GetStateIndex(id,k);
    try
        set(thandle,'xdata',x(xfi),'ydata',x(xfi+1));
    catch
        keyboard;
    end;
    
    if(isempty(Graphics.FeatureEllipse) | isempty(find(Graphics.FeatureEllipse(1,:)==id)))
        thandle = DrawEllipse(x(xfi:xfi+1),Sigma(xfi:xfi+1,xfi:xfi+1),3,Graphics.Color);
        Graphics.FeatureEllipse = [Graphics.FeatureEllipse [id; thandle]];
    else
        [Sigmax,Sigmay] = GetEllipse(x(xfi:xfi+1),Sigma(xfi:xfi+1,xfi:xfi+1),3);
        thandle = Graphics.FeatureEllipse(2,find(Graphics.FeatureEllipse(1,:)==id));
        set(thandle,'xdata',Sigmax,'ydata',Sigmay);
    end;
end;



return;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function h = DrawEllipse(x,P,nSigma,RGBCol)

[V,D] = eig(P);
y = nSigma*[cos(0:0.1:2*pi);sin(0:0.1:2*pi)];
el = V*sqrtm(D)*y;
el = [el el(:,1)]+repmat(x,1,size(el,2)+1);

h = line(el(1,:),el(2,:));
set(h, 'markersize',0.5,'color',RGBCol);

return;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [XData,YData] = GetEllipse(x,P,nSigma)

[V,D] = eig(P);
y = nSigma*[cos(0:0.1:2*pi);sin(0:0.1:2*pi)];
el = V*sqrtm(D)*y;
el = [el el(:,1)]+repmat(x,1,size(el,2)+1);

XData = el(1,:);
YData = el(2,:);

return;







%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function which returns the index for a particular feature/pose
function xi = GetStateIndex(id,k)

global TheJournal;


iRow = find(TheJournal.LookUP(:,1) == id);
iColumn = find(TheJournal.LookUP(1,:) == k);

if(isempty(iRow) || isempty(iColumn))
    fprintf(1,'!!!!!!   Error determining index for vehicle/feature %i at time %i \n',id,k);
else
    xi = TheJournal.LookUP(iRow,iColumn); 
end;

return;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Function to generate point features
function MakeFeatures()

global Params;
global Truth;
global Graphics;

Truth.Features = [];

dist = Params.MaxObsRange/2;
minx = min(Truth.VehicleTrajectory(2,:)) - dist;
maxx = max(Truth.VehicleTrajectory(2,:)) + dist;
miny = min(Truth.VehicleTrajectory(3,:)) - dist;
maxy = max(Truth.VehicleTrajectory(3,:)) + dist;


Truth.Features = repmat([minx; miny],1,Params.numFeatures) + [(maxx-minx) 0; 0 (maxy-miny)]*rand(2,Params.numFeatures);
Truth.Features = [2:(Params.numFeatures+1); Truth.Features];

if(Params.PlotSwitch)
    figure(Graphics.MainFigure);
    subplot(1,2,1)
    plot(Truth.Features(2,:),Truth.Features(3,:),['k.']);
    text(Truth.Features(2,:),Truth.Features(3,:)-1.0,int2str(Truth.Features(1,:)'));
end;

return;

 






%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Make me a survey pattern for a vehicle with 2 uncoupled dof
function pattern = MakeSurvey()

global Params;
global Truth;
global Data;


% Generate the 4 waypoints that comprise the corners of the route (a box)
a = 0.8*Params.MapSize;
b = 0.8*Params.MapSize;
X0 = Params.MapSize*[0.1; 0.1];

Params.Path = repmat([0 a a  0; 0 0 b b],1,Params.nRepeat);
Params.Path = Params.Path + repmat(X0,1,size(Params.Path,2));


Data.Velocity = [1 0 0]';
xold = Params.Path(:,1);
for i=2:size(Params.Path,2)
    tlast = Data.Velocity(1,end);
    xold = Params.Path(:,i-1);
    xnew = Params.Path(:,i);
    xdiff = xnew(1)-xold(1);
    ydiff = xnew(2)-xold(2);
    if(abs(xdiff) > abs(ydiff))
        tend = tlast + ceil(abs(xdiff)/Params.maxVelocity);
        xvel = sign(xdiff)*Params.maxVelocity;
        yvel = ydiff/(tend-tlast);
    elseif(abs(ydiff) > abs(xdiff))
        tend = tlast + ceil(abs(ydiff)/Params.maxVelocity);
        yvel = sign(ydiff)*Params.maxVelocity;
        xvel = xdiff/(tend-tlast);
    end;

    XVel = repmat(xvel,1,tend-tlast);
    YVel = repmat(yvel,1,tend-tlast);
    Data.Velocity = [Data.Velocity [(tlast+1):tend; XVel; YVel]];
end;

% Generate the true, NOISE CORRUPTED, vehicle trajectory
%
% Generate correlated noise using the Cholesky Decomposition: Q = X'*X
% for which: v = U*randn(2,1) --> cov(v) = U*I*U' = U*U' = Q    (U*U' = X'*X --> U = X')
U = chol(Params.Q)';
Truth.VehicleTrajectory = [1; [0;0]];
for i=2:size(Data.Velocity,2)
    VelocityNoise = Data.Velocity(2:3,i) + U*randn(2,1);
    Truth.VehicleTrajectory = [Truth.VehicleTrajectory [i; Truth.VehicleTrajectory(2:3,end)+VelocityNoise]];
end;


return;