Institut de Robòtica i Informàtica Industrial

EKF_Update

PURPOSE ^

EKF Update step.

SYNOPSIS ^

function EKF=EKF_Update(EKF,i,d,R)

DESCRIPTION ^

 EKF Update step.

 This is used when establishing a loop between the current robot pose
 and one of the previous poses, i. This can be the previous pose or one of
 the poses before it. In the first case we are basically forming a local
 loop with limited effects (only affects the current robot pose) and
 in the second we have a loop that can affect a significant part of the
 trajectory.

 Parameters
      EKF: The filter to update.
      i; The pose to be linked with the current robot's pose to form a
         loop.
      d: Displacement from pose 'i' to the current robot's pose 'n'
         according to the sensors readings.
      R: Noise of the sensor
 Outputs
      EKF: The updated filter.

 See also EKF_Prediction, TRO_Prediction, TRO_Update.

CROSS-REFERENCE INFORMATION ^

This function calls:
  • get Get function for robots.
  • get Get function for trajectories.
  • size Number of poses in a trajectory.
  • get Get function for EKF filters.
  • get Generic get function for filters.
  • size Size of the state estimated in the filter
  • get Get function for TRO filters.
  • get Get function for BTrees.
  • get Get function for PoseData objects.
  • get Get function for Gaussians.
  • double Converts an interval to a double.
  • size Size (rows/columns) of an interval matrix.
  • Absolute2Relative Relative displacement between two poses.
  • Absolute2RelativeJacobian Jacobian of the relative displacement between two poses.
  • double Converts a double
  • get Get function for poses.
  • size Number of parameters of the pose.
  • Pose Generic pose constructor.
  • get Generic get for relative positioning sensors
  • get Generic get for sensors
  • size Number of readings stored in the Sensor.
This function is called by:
  • Update Adds a new pose to an EKF filter.
  • UpdateLoop Adds a new loop to an EKF filter.

SOURCE CODE ^

0001 function EKF=EKF_Update(EKF,i,d,R)
0002 % EKF Update step.
0003 %
0004 % This is used when establishing a loop between the current robot pose
0005 % and one of the previous poses, i. This can be the previous pose or one of
0006 % the poses before it. In the first case we are basically forming a local
0007 % loop with limited effects (only affects the current robot pose) and
0008 % in the second we have a loop that can affect a significant part of the
0009 % trajectory.
0010 %
0011 % Parameters
0012 %      EKF: The filter to update.
0013 %      i; The pose to be linked with the current robot's pose to form a
0014 %         loop.
0015 %      d: Displacement from pose 'i' to the current robot's pose 'n'
0016 %         according to the sensors readings.
0017 %      R: Noise of the sensor
0018 % Outputs
0019 %      EKF: The updated filter.
0020 %
0021 % See also EKF_Prediction, TRO_Prediction, TRO_Update.
0022 
0023   % the dispalcement 'd' MUST be from 'i' to 'n'
0024   nStates=get(EKF.P_Filter,'nStates');
0025   
0026   if i==nStates
0027     error('Self loop in EKF Update')
0028   end
0029   
0030   m=get(EKF.P_Filter,'dim'); % Dimensionality of the pose space
0031   rangeI=m*(i-1)+1:m*i;
0032   rangeN=m*(nStates-1)+1:m*nStates;
0033   
0034   ne=size(EKF.mu,1); % number of elements in the state (m per pose)
0035   
0036   mI=EKF.mu(rangeI);
0037   Pi=Pose(mI);
0038   
0039   mN=EKF.mu(rangeN);
0040   Pn=Pose(mN);
0041   
0042   y=double(d-Absolute2Relative(Pi,Pn));
0043   
0044   H=sparse(zeros(m,ne));
0045   
0046   [Ji Jn]=Absolute2RelativeJacobian(Pi,Pn);
0047   H(:,rangeI)=Ji;
0048   H(:,rangeN)=Jn;
0049   
0050   SHt=EKF.Sigma*H';
0051   
0052   S=H*SHt+R;
0053   S=(S+S')/2; % This enforces symmetry and enhances numerical stability
0054   
0055   L=chol(S);
0056   iL=inv(L);
0057   W1=SHt*iL;
0058   
0059   EKF.mu=EKF.mu + W1*iL'*y;
0060   
0061   EKF.Sigma=EKF.Sigma - W1*W1';


Institut de Robòtica i Informàtica Industrial

Generated on Fri 24-Jul-2009 12:32:50 by m2html © 2003