Institut de Robòtica i Informàtica Industrial

Update

PURPOSE ^

Adds a new pose to an EKF filter.

SYNOPSIS ^

function EKF=Update(EKF,step,SR,Sn,overwritePose)

DESCRIPTION ^

 Adds a new pose to an EKF filter.
 
 Updates an 'Extended Kalman Filter' adding a new pose to the estimated
 trajectory. It updates the mean and the covariance matrix.

 Parameters:
     EKF: The filter.
     step: Time slice.
     SR: The sensor readings.
     Sn: The noise of the sensors.
     overwritePose: Not used in the 'Extended Kalman Filter'.
 Returns:
     EKF: The updated 'Extended Kalman Filter'.

 The first sensor reading is used to extend the state (i.e., the
 trajectory) using EKF_Prediction, and the remaining ones are used to
 improve the estimation of the current robot's pose using EKF_Update.

 See also EKF_Prediction, EKF_Update

CROSS-REFERENCE INFORMATION ^

This function calls:
  • get Get function for robots.
  • get Get function for trajectories.
  • size Number of poses in a trajectory.
  • Update Adds a new pose to an EKF filter.
  • get Get function for EKF filters.
  • EKF_Prediction EKF Prediction step.
  • EKF_Update EKF Update step.
  • Update Adds a pose to the filter.
  • get Generic get function for filters.
  • size Size of the state estimated in the filter
  • Update Adds a new pose to a TRO filter.
  • get Get function for TRO filters.
  • get Get function for BTrees.
  • get Get function for PoseData objects.
  • get Get function for Gaussians.
  • size Size (rows/columns) of an interval matrix.
  • NullPose Checks is a pose is null.
  • get Get function for poses.
  • size Number of parameters of the pose.
  • 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.
  • Simulation Simulates a robot performing Pose SLAM.
  • Update Adds a new pose to a TRO filter.

SOURCE CODE ^

0001 function EKF=Update(EKF,step,SR,Sn,overwritePose)
0002 % Adds a new pose to an EKF filter.
0003 %
0004 % Updates an 'Extended Kalman Filter' adding a new pose to the estimated
0005 % trajectory. It updates the mean and the covariance matrix.
0006 %
0007 % Parameters:
0008 %     EKF: The filter.
0009 %     step: Time slice.
0010 %     SR: The sensor readings.
0011 %     Sn: The noise of the sensors.
0012 %     overwritePose: Not used in the 'Extended Kalman Filter'.
0013 % Returns:
0014 %     EKF: The updated 'Extended Kalman Filter'.
0015 %
0016 % The first sensor reading is used to extend the state (i.e., the
0017 % trajectory) using EKF_Prediction, and the remaining ones are used to
0018 % improve the estimation of the current robot's pose using EKF_Update.
0019 %
0020 % See also EKF_Prediction, EKF_Update
0021 
0022   % overwritePose is not implemented in EkF. We keep the parameter
0023   % for compatibility with P_TRO filter .
0024   
0025   ns=size(SR,2); % number of sensor readings
0026   
0027   % First valid sensor reading is used for prediction (i.e., to extend the
0028   % state), and the rest of valid sensor readings for update.
0029   first=true;
0030   for i=1:ns
0031     d=SR{i};
0032     if ~NullPose(d)
0033       if first
0034         EKF=EKF_Prediction(EKF,d,Sn{i});
0035         
0036         EKF.P_Filter=Update(EKF.P_Filter,step,false);
0037         
0038         nStates=get(EKF.P_Filter,'nStates');
0039         first=false;
0040       else
0041         EKF=EKF_Update(EKF,nStates-1,d,Sn{i});
0042       end
0043     end
0044   end
0045   if first
0046     error('EKF step without any valid sensor reading');
0047   end


Institut de Robòtica i Informàtica Industrial

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