Institut de Robòtica i Informàtica Industrial

UpdateLoop

PURPOSE ^

Adds a new loop to a TRO filter.

SYNOPSIS ^

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

DESCRIPTION ^

 Adds a new loop to a TRO filter.

 Updates the estimated trajectory introducing a loop closure.
 Parameters:
   FF: 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 (ceel array).
   RS: The noise for the sensors (cell array).

 See also FF_Update.

CROSS-REFERENCE INFORMATION ^

This function calls:
  • get Get function for robots.
  • get Get function for trajectories.
  • size Number of poses in a trajectory.
  • UpdateLoop Adds a new loop to an EKF filter.
  • get Get function for EKF filters.
  • Step2State Returns the state correspoinding to a step.
  • UpdateLoop Adds a loop to the filter.
  • get Generic get function for filters.
  • size Size of the state estimated in the filter
  • UpdateLoop Adds a new loop to a TRO filter.
  • get Get function for TRO filters.
  • TRO_Update Filter update step.
  • get Get function for BTrees.
  • get Get function for PoseData objects.
  • get Get function for Gaussians.
  • SensorFusion Fusion of noisy sensor readings.
  • size Size (rows/columns) of an interval matrix.
  • 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:

SOURCE CODE ^

0001 function FF=UpdateLoop(FF,linked,dS,RS)
0002 % Adds a new loop to a TRO filter.
0003 %
0004 % Updates the estimated trajectory introducing a loop closure.
0005 % Parameters:
0006 %   FF: 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 (ceel array).
0009 %   RS: The noise for the sensors (cell array).
0010 %
0011 % See also FF_Update.
0012 
0013   l=Step2State(FF.P_Filter,linked);
0014   if (l<1)
0015     error('Trying to form close loop with a removed pose');
0016   end
0017   
0018   ns=size(RS,2);
0019   G=SensorFusion(1:ns,dS,RS);
0020   
0021   FF=TRO_Update(FF,l,Pose(get(G,'mean')),get(G,'covariance'));
0022   
0023   % Call the UpdateLoop for the generic filter
0024   FF.P_Filter=UpdateLoop(FF.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