Institut de Robòtica i Informàtica Industrial

UpdateLoop

PURPOSE ^

Adds a new loop to an EKF filter.

SYNOPSIS ^

function EKF=UpdateLoop(EKF,linked,dS,RS)

DESCRIPTION ^

 Adds a new loop to an EKF filter.
 
 Updates the estimated trajectory introducing a loop closure.
 Parameters:
     EKF: The filter with the estimated trajectory
     linked: The pose to be linked with the current robot's pose.
     dS: The sensor readings linking the two poses.
     RS: The noise for the sensors.

 See also EKF_Update.

CROSS-REFERENCE INFORMATION ^

This function calls:
  • size Number of poses in a trajectory.
  • UpdateLoop Adds a new loop to an EKF filter.
  • EKF_Update EKF Update step.
  • Step2State Returns the state correspoinding to a step.
  • UpdateLoop Adds a loop to the filter.
  • size Size of the state estimated in the filter
  • UpdateLoop Adds a new loop to a TRO filter.
  • size Size (rows/columns) of an interval matrix.
  • size Number of parameters of the pose.
  • size Number of readings stored in the Sensor.
This function is called by:

SOURCE CODE ^

0001 function EKF=UpdateLoop(EKF,linked,dS,RS)
0002 % Adds a new loop to an EKF filter.
0003 %
0004 % Updates the estimated trajectory introducing a loop closure.
0005 % Parameters:
0006 %     EKF: The filter with the estimated trajectory
0007 %     linked: The pose to be linked with the current robot's pose.
0008 %     dS: The sensor readings linking the two poses.
0009 %     RS: The noise for the sensors.
0010 %
0011 % See also EKF_Update.
0012 
0013 
0014   l=Step2State(EKF.P_Filter,linked);
0015   if (l<1)
0016     error('Trying to form close loop with a removed pose');
0017   end
0018   
0019   ns=size(RS,2);
0020   for i=1:ns
0021     EKF=EKF_Update(EKF,l,dS{i},RS{i});
0022   end
0023   
0024   % Call the UpdateLoop for the generic filter
0025   EKF.P_Filter=UpdateLoop(EKF.P_Filter,linked,dS,RS);


Institut de Robòtica i Informàtica Industrial

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