slam_if_1d.m

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

eta_b = eta(beta_indices);

TheJournal.Lambda = Lambda_aa - Lambda_ab*inv(Lambda_bb)*Lambda_ab';
TheJournal.eta = eta_a - Lambda_ab*inv(Lambda_bb)*eta_b;

% Update the schematic of the information matrix
if(Params.verbose)
    ShowLinkStrength();
    ShowLinkStrength(Lambda,1,Graphics.SecondFigure);
    fprintf(1,'\n\n');
    fprintf(1,'ii)  The final step is to marginalize over the old pose, x_v(t) \n\n');
    fprintf(1,'     By doing so, we have created links among all features that were \n');
    fprintf(1,'     linked to x_v(t). Over time, this will fill in the matrix\n\n');
    fprintf(1,'----------------------- \n\n');

    pause;
end;


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


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);

innov = [];
G = [];
Gr = [];
Zinv = [];

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



if(Params.verbose)
    fprintf(1,'\n\n');
    fprintf(1,'+++++++++++++++++++++++ \n\n');
    fprintf(1,'MEASUREMENT UPDATE: \n\n')
    ShowLinkStrength(TheJournal.Lambda,0,Graphics.SecondFigure);
end;

% Loop over the measurement data, performing an individual update for each observation
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.
    z = Observations(2,i);
    
    CT = [-1 sparse(1,size(TheJournal.Lambda,1)-1)];
    CT(:,xfi) = 1;
    
    TheJournal.Lambda = TheJournal.Lambda + CT'*Params.Rinv*CT;
    TheJournal.eta = TheJournal.eta + CT'*Params.Rinv*z;
    %%%%%%%%%%%%%%%

    
    if(Params.verbose)
        fprintf(1,'    We updated the filter based on an observation of feature %i. \n\n',id);
        fprintf(1,'    Notice how the shared information between this feature and the \n');
        fprintf(1,'    robot increases by comparing with the old matrix in Figure 2. \n\n')
        ShowLinkStrength(TheJournal.Lambda);
        pause;
    end;
end;

if(Params.verbose)
    fprintf(1,'\n');
    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

% Jacobian
F = 1;


if(Params.verbose)
    fprintf(1,'\n\n');
    fprintf(1,'\n\n');
    fprintf(1,'+++++++++++++++++++++++ \n\n');
    fprintf(1,'ADDING A FEATURE: \n\n')
    ShowLinkStrength(TheJournal.Lambda,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);
    
    
    %%%%%%%%%%%%%%%
    % The operation of adding features with the information form as described
    % in Eustice et al. (eustice05a) Section III.A.
    H12 = [-F'*Params.Rinv; sparse(size(TheJournal.Lambda,1)-1,1)];
    
    TheJournal.Lambda = [TheJournal.Lambda H12; H12' Params.Rinv];
    TheJournal.Lambda(1,1) = TheJournal.Lambda(1,1) + F'*Params.Rinv*F;

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

    TheJournal.eta = [TheJournal.eta; etaf_new];
    TheJournal.eta(1) = TheJournal.eta(1) - F'*Params.Rinv*Observations(2,i);
    %%%%%%%%%%%%%%%
    
    % Plotting stuff
    if(Params.PlotSwitch)
        xyf = GetMean(1) + Observations(2,i);
        figure(Graphics.MainFigure);
        subplot(Graphics.axes1);
        thandle = plot(xyf(1),0,'rx','MarkerSize',12);

        Graphics.Features = [Graphics.Features [id; thandle]];
    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);
    
    % Keep track of the shared information between the robot and feature
    TheJournal.Feature{end+1}.id = id;
    LambdaNorm = rhomatrix(TheJournal.Lambda);
    TheJournal.Feature{end}.Link = [TheJournal.LookUP(1,2); LambdaNorm(1,end)];
    
    
    if(Params.verbose)
        ShowLinkStrength(TheJournal.Lambda);

        fprintf(1,'\n\n');
        fprintf(1,'    We have added feature %i to the map. \n\n',id);
        fprintf(1,'    Figure 1 shows the new information matrix and Figure 2 the previous version. \n');
        fprintf(1,'    A new row and column have been added to the information matrix \n');
        fprintf(1,'    and the new feature is only linked to the robot pose. \n');
        pause;
    end;
        
end;

if(Params.verbose)
    fprintf(1,'\n');
    fprintf(1,'+++++++++++++++++++++++ \n\n');
end;


return;




%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function DoLogging(k)

global TheJournal;

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


Lambda = rhomatrix(TheJournal.Lambda);

for i=3:size(TheJournal.LookUP,1)
    xfi = TheJournal.LookUP(i,2);
    
    TheJournal.Feature{i-2}.Link(:,end+1) = [k; Lambda(1,xfi)];
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<1)
    ILinks = rhomatrix(TheJournal.Lambda);
else
    ILinks = rhomatrix(Lambda);
end;

if(nargin < 3)
    figure(Graphics.MainFigure);
    subplot(Graphics.axes2);
else
    figure(fhandle);
end;
cla;
imagesc(abs(ILinks),[0 1]);
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,X,temp)

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

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.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,find(Truth.VehicleTrajectory(1,:)==k));
Temp = Truth.Features(2,:) - repmat(xv,1,size(Truth.Features,2));
I = find(abs(Temp) <= Params.MaxObsRange);
Observations = [Truth.Features(1,I); Temp(I)];
[Y,I] = sort(abs(Observations(2,:)));
Observations = Observations(:,I(1:min(length(I),Params.nObsPerIteration)));


% 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)';


Observations(2,:) = Observations(2,:) + U*randn(1,size(Observations,2));


return;
        



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

global TheJournal;
global Graphics;
global Truth;

figure(Graphics.MainFigure);
subplot(Graphics.axes1);


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

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


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

    xfi = GetStateIndex(id,k);
    set(thandle,'xdata',x(xfi),'ydata',0);
end;



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;