setstate.m

来自「it is a matlab file foe develop SLAM loc」· M 代码 · 共 33 行

%SETSTATE Set map state vector and state covariance matrix.
%   M = SETSTATE(M,X,C) overwrites the map state vector and the state
%   covariance matrix by X and C respectively. The size of X is nx1, 
%   the size of C is nxn where n equals the number of features in M
%   times their number of parameters plus three from the robot.
%
%   See also MAP/GETSTATE, MAP/SET.

% v.1.0, Nov. 2003, Kai Arras, CAS-KTH

function m = setstate(m,X,C);

nF = length(m.X);
  
% recast C into blockwise structure
iacc = 1;
for i = 1:nF,
  jacc = 1;
  for j = 1:nF,
    [nr,nc] = size(m.C(i,j).C);
    m.C(i,j).C = C(iacc:iacc+nr-1,jacc:jacc+nc-1);
    jacc = jacc + nc;
  end;
  iacc = iacc + nr;
end;

% recast x into blockwise structure and establish consistency between C and features
iacc = 1;
for i = 1:nF,
  nr = length(get(m.X{i},'x'));
  m.X{i} = set(m.X{i},'x',X(iacc:iacc+nr-1),'c',m.C(i,i).C);
  iacc = iacc + nr;
end;