Institut de Robòtica i Informàtica Industrial

TRO_Prediction

PURPOSE ^

Prediction Step.

SYNOPSIS ^

function FF=TRO_Prediction(FF,d,Q,overwritePose)

DESCRIPTION ^

 Prediction Step.

 This is used when extending the trajectory (i.e., the state), with a
 new pose.
 It updates the information matrix, the information vector, the mean 
 (necessary as a linearization point for all filter operations) and
 updates F, the accumulation of linearizations of the displacement
 function (see Relative2AbsoluteJacobian) that is latter used to compute
 cross covariances of poses w.r.t. the new robot's pose.
  
 Parameters
   FF: 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'.
   overwritePose: If true, the last pose in the filter is removed and
                  replaced by the new one. In this way the number of
                  estimated elements actually does not grow and we end
                  up with less states than simulation steps.
 Outputs:
   FF: The updated filter with the new pose.

  See also EKF_Prediction, EKF_Update, Update, Relative2AbsoluteJacobian.

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.
  • PoseData PoseData constructor.
  • get Get function for PoseData objects.
  • set Set functino 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 a TRO filter.

SOURCE CODE ^

0001 function FF=TRO_Prediction(FF,d,Q,overwritePose)
0002 % Prediction Step.
0003 %
0004 % This is used when extending the trajectory (i.e., the state), with a
0005 % new pose.
0006 % It updates the information matrix, the information vector, the mean
0007 % (necessary as a linearization point for all filter operations) and
0008 % updates F, the accumulation of linearizations of the displacement
0009 % function (see Relative2AbsoluteJacobian) that is latter used to compute
0010 % cross covariances of poses w.r.t. the new robot's pose.
0011 %
0012 % Parameters
0013 %   FF: The filter to update
0014 %   d: The sensor reading providing the position of the new robot's pose
0015 %      from its predecesor pose.
0016 %   Q: The noise of the sensor providing 'd'.
0017 %   overwritePose: If true, the last pose in the filter is removed and
0018 %                  replaced by the new one. In this way the number of
0019 %                  estimated elements actually does not grow and we end
0020 %                  up with less states than simulation steps.
0021 % Outputs:
0022 %   FF: The updated filter with the new pose.
0023 %
0024 %  See also EKF_Prediction, EKF_Update, Update, Relative2AbsoluteJacobian.
0025 
0026   ne=size(FF);
0027   dim=get(FF.P_Filter,'dim'); % dimensionality of the pose space
0028   nStates=get(FF.P_Filter,'nStates');
0029   step=get(FF.P_Filter,'nSteps');
0030   
0031   if overwritePose
0032     lastState=nStates-1; % Last valid state in the filter (in this case last
0033     % state minus one)
0034     newState=nStates;    % Index where to store the new state (in this case)
0035     % we re-use last state)
0036     
0037     last=(lastState-1)*dim+1:lastState*dim;
0038     new=(newState-1)*dim+1:newState*dim;
0039     
0040     % First we undo the effect of the last _Prediction
0041     iQw12=FF.Lambda(new,new);
0042     Qw12=iQw12\eye(dim);
0043     Jp12=-Qw12*FF.Lambda(new,last);
0044     
0045     FF.Lambda(last,last)=sparse2(FF.Lambda(last,last)-sparse2(Jp12'*iQw12*Jp12));
0046     
0047     etaN=FF.eta(new);
0048     FF.eta(last)=FF.eta(last)+Jp12'*etaN;
0049     
0050     FF.F=FF.F*(Jp12\eye(dim))';
0051     
0052     % Now we define the data necessary to execute the prediction
0053     % below
0054     % Qw: Noise in global coordinates
0055     % P1d:  last pose in the trajectory that we keep (as doubles).
0056     % Jp: Jacobian of Relative2Absolute w.r.t. the last pose in the
0057     %     trajectory that we keep.
0058     % newPose: New robot position (mean).
0059     P1d=FF.mu(last); % Last know pose that we will keep
0060     P1=Pose(P1d);
0061     
0062     P2=Pose(FF.mu(new));% Pose that will be removed
0063     
0064     P3=Relative2Absolute(P2,d); % Pose to connect directly with P1
0065     newPos=double(P3);
0066     
0067     [Jp23 Jd23]=Relative2AbsoluteJacobian(P2,d);
0068     
0069     Jp=Jp23*Jp12;
0070     
0071     Qw=Jp23*Qw12*Jp23'+Jd23*Q*Jd23';
0072   else
0073     lastState=nStates; % Last valid state in the filter
0074     newState=nStates+1; %  Index where to store the new state
0075     
0076     last=(lastState-1)*dim+1:lastState*dim;
0077     new=(newState-1)*dim+1:newState*dim;
0078     
0079     P1d=FF.mu(last);
0080     P1=Pose(P1d); % mean for the previous pose (used as a linealization
0081     % point
0082     
0083     % Jacobians used to linarly propagate the error from previous poses
0084     % to the new one
0085     [Jp Jd]=Relative2AbsoluteJacobian(P1,d);
0086     
0087     Qw=Jd*Q*Jd'; % displacement noise in global coordinates
0088     
0089     newPos=double(Relative2Absolute(P1,d));
0090     
0091     % make room for the new data
0092     Zk=zeros(dim,1);
0093     FF.eta=[FF.eta ; Zk];
0094     FF.mu=[FF.mu; Zk];
0095     
0096     Znk=sparse2(ne,dim);
0097     Zkk=sparse2(dim,dim);
0098     FF.Lambda=sparse2([FF.Lambda Znk ; Znk' Zkk ]);
0099   end
0100   
0101   v=newPos-Jp*P1d;
0102   
0103   iQw=Qw\eye(dim);
0104   etaN=iQw*v;
0105   
0106   FF.eta(last)=FF.eta(last)-Jp'*etaN;
0107   FF.eta(new)=etaN;
0108   
0109   FF.Lambda(last,last)=sparse2(FF.Lambda(last,last)+sparse2(Jp'*iQw*Jp));
0110   
0111   FF.Lambda(last,new)=sparse2(-Jp'*iQw);
0112   FF.Lambda(new,last)=sparse2(-iQw*Jp);
0113   FF.Lambda(new,new)=sparse2(iQw);
0114   
0115   FF.mu(new)=newPos;
0116   
0117   Sp=get(FF.PoseData{lastState},'covariance'); % Estimation for the previous pose
0118   FF.PoseData{lastState}=set(FF.PoseData{lastState},'crossCovarianceData',Sp*(FF.F\eye(dim)));
0119   
0120   FF.PoseData{newState}=PoseData(newPos,Jp*Sp*Jp'+Qw,eye(dim),step+1);
0121   
0122   FF.F=FF.F*Jp';
0123   
0124


Institut de Robòtica i Informàtica Industrial

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