Institut de Robòtica i Informàtica Industrial

EKF_Prediction

PURPOSE ^

EKF Prediction step.

SYNOPSIS ^

function EKF=EKF_Prediction(EKF,d,Q)

DESCRIPTION ^

 EKF Prediction step.

   Predicton step in a 'Extended Kalman Filter'.
   This is used when extending the trajectory (i.e., the state), with a
   new pose.
   It updates the mean and the covariance matrix.

   Parameters
     EKF: The filter to update
     d: The sensor reading providing the position of the new robot's pose
        from its predecesor pose.
     Q: The noise of the sensor providing 'd'.
   Outputs:
     EKF: The updated filter with the new pose.

   See also EKF_Update, 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.
  • Relative2Absolute Adds a displacement to a pose.
  • Relative2AbsoluteJacobian Jacobian of the displacement function.
  • 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.

SOURCE CODE ^

0001 function EKF=EKF_Prediction(EKF,d,Q)
0002 % EKF Prediction step.
0003 %
0004 %   Predicton step in a 'Extended Kalman Filter'.
0005 %   This is used when extending the trajectory (i.e., the state), with a
0006 %   new pose.
0007 %   It updates the mean and the covariance matrix.
0008 %
0009 %   Parameters
0010 %     EKF: The filter to update
0011 %     d: The sensor reading providing the position of the new robot's pose
0012 %        from its predecesor pose.
0013 %     Q: The noise of the sensor providing 'd'.
0014 %   Outputs:
0015 %     EKF: The updated filter with the new pose.
0016 %
0017 %   See also EKF_Update, TRO_Prediction, TRO_Update.
0018 
0019   % EKF prediction a la full SLAM, where the state is extended to estimate the
0020     % full trajectory.
0021   n=size(EKF.mu,1);
0022 
0023   k=get(EKF.P_Filter,'dim'); % dimensionality of the pose space
0024   last=n-k+1:n;
0025   
0026   P1=Pose(EKF.mu(last)); % mean for the previous pose
0027     
0028   EKF.mu=[EKF.mu ; double(Relative2Absolute(P1,d))];
0029   
0030   % Jacobians used to linarly propagate the error from previous poses
0031   % to the new one
0032   [Jp Jd]=Relative2AbsoluteJacobian(P1,d);
0033   
0034   Sp=EKF.Sigma(last,last); % Covariance of the previous pose
0035   Cp=EKF.Sigma(1:n,last); % Previous cross-covariance (last k columns)
0036   C=Cp*Jp';
0037   
0038   EKF.Sigma=[EKF.Sigma C ; C' Jp*Sp*Jp'+Jd*Q*Jd'];
0039   
0040


Institut de Robòtica i Informàtica Industrial

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