estimatemap.m

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

%ESTIMATEMAP Apply Kalman filter on map.
%   G = ESTIMATEMAP(G,NU,R,H) updates the map G by application of
%   the extended Kalman filter (EKF). The function implements a
%   stacked version of the EKF with the following input arguments:
%   The stacked measurement Jacobian H of size n x p, where p is
%   the number of measurements (which in turn is the number of
%   observed features times their number of parameters), and n 
%   the size of the map state vector. The stacked p x 1 innova-
%   tion vector NU, and the stacked p x p observation covariance
%   matrix R.
%
%   See also SLAM, MEANWM.

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


function G = estimatemap(G,nu,R,H);

if length(nu) > 0,
  
  % get state vector and covariance
  [x,C] = getstate(G);
    
  % EKF
  S = H*C*H' + R;
  K = C*H'*inv(S);
  x = x + K*nu;
  C = C - K*H*C;

  % put back updated state vector and covariance
  G = setstate(G,x,C);
  
  if det(C) < 0,
    disp('--> estimatemap: cov negative definite after!');
  end;
  
end;