addentity.m

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

%ADDENTITY Add map entity object to map.
%   M = ADDENTITY(M,E) appends map entity E to map M and
%   extends the map covariance matrix by filling in zeros
%   in the new row and column belonging to E.
%
%   See also ENTITY, MAP.

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

function m = addentity(m, e);

if isa(m,'map') & isa(e,'entity'),
  
  % extend state vector and matrix
  n = length(m.X);
  m.X{n+1} = e;
  Ce = get(e,'c');
  m.C(n+1,n+1).C = Ce;
  
  % fill up new block matrices with zeros
  dimc = length(Ce);
  for i = 1:n,
    dimr = length(get(m.X{i},'x'));
    m.C(i,n+1).C = zeros(dimr, dimc);
    m.C(n+1,i).C = m.C(i,n+1).C';
  end;
  
else
  disp('addentity: Wrong input. Check your arguments');
end;