drawcorr.m

it is a matlab file foe develop SLAM localization this is a toolbox for develop develop realtime e

%DRAWCORR Display map correlation coefficients.
%   DRAWCORR(M,COLORMODE) displays the correlations of map M
%   in the current figure. If COLORMODE is 1, the correlations
%   are drawn as gray values, if COLORMODE is 0, they are 
%   drawn using the default colormap.
%
%   See also MAP/DRAW, COLORMAP.

% v.1.0, 01.11.03, Kai Arras, CAS-KTH

function drawcorr(M,colormode);

% Get state vector and covariance
[x,C] = getstate(M);
n = length(x);

% Determine correlation coefficients
for i = 1:n,
  for j = 1:n,
    denom = sqrt(C(i,i)*C(j,j));
    if (denom == 0) | ~isreal(denom),
      CC(i,j) = 0;
    else
      CC(i,j) = C(i,j)/denom;
    end;
    if abs(CC(i,j)) > 1,
      disp(['drawcorr: Warning: |corrcoeff| > 1! i = ',int2str(i),', j = ',...
            int2str(j),', cc = ',num2str(CC(i,j))]);
    end;
  end;
end;

% Draw using the imagesc command
imagesc(abs(CC));
axis square;

% Set up colormap
if colormode == 1,
  graymap = gray;
  colormap(graymap(length(graymap):-1:1,:));
end;