Institut de Robòtica i Informàtica Industrial

TRO_Update

PURPOSE ^

Filter update step.

SYNOPSIS ^

function FF=TRO_Update(FF,i,d,R)

DESCRIPTION ^

 Filter update step.

 This is used when establishing a loop between the current robot pose
 and one of the previous poses, i. This can be the previous pose or one of
 the poses before it. In the first case we are basically forming a local
 loop with limited effects (only affects the current robot pose) and
 in the second we have a loop that can affect a significant part of the
 trajectory.
 Besides the update of the mean (it is the linealization point used in
 all EIF operations) and cross covariances of all the poses w.r.t. the
 last robot's pose and the marginal covariances of all poses w.r.t the
 current one.
 All together can be expensive but this is compensated if we are
 cautious with the loop formation and we only form reliable and
 informative loops. The cost of closing a loop is then amortized over 
 long periods without loop formations.

 Parameters
    FF: The filter to update.
    i; The pose (state) to be linked with the current robot's pose to
       forma a loop. 
    d: Displacement from pose 'i' to the current robot's pose 'n'
       according to the sensors readings.
    R: Noise of the sensor
 Outputs
    FF: The updated filter.

 See also EKF_Prediction, EKF_Update, UpdateLoop.

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.
  • GetMarginalCovariance Marginal covariance between the current pose and a previous one.
  • State2Step Returns the step corresponding to a state.
  • 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.
  • 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.
  • Absolute2Relative Relative displacement between two poses.
  • Absolute2RelativeJacobian Jacobian of the relative displacement between two poses.
  • 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:

SOURCE CODE ^

0001 function FF=TRO_Update(FF,i,d,R)
0002 % Filter update step.
0003 %
0004 % This is used when establishing a loop between the current robot pose
0005 % and one of the previous poses, i. This can be the previous pose or one of
0006 % the poses before it. In the first case we are basically forming a local
0007 % loop with limited effects (only affects the current robot pose) and
0008 % in the second we have a loop that can affect a significant part of the
0009 % trajectory.
0010 % Besides the update of the mean (it is the linealization point used in
0011 % all EIF operations) and cross covariances of all the poses w.r.t. the
0012 % last robot's pose and the marginal covariances of all poses w.r.t the
0013 % current one.
0014 % All together can be expensive but this is compensated if we are
0015 % cautious with the loop formation and we only form reliable and
0016 % informative loops. The cost of closing a loop is then amortized over
0017 % long periods without loop formations.
0018 %
0019 % Parameters
0020 %    FF: The filter to update.
0021 %    i; The pose (state) to be linked with the current robot's pose to
0022 %       forma a loop.
0023 %    d: Displacement from pose 'i' to the current robot's pose 'n'
0024 %       according to the sensors readings.
0025 %    R: Noise of the sensor
0026 % Outputs
0027 %    FF: The updated filter.
0028 %
0029 % See also EKF_Prediction, EKF_Update, UpdateLoop.
0030 
0031   %  stateRecoveryMethod
0032   % 2 -> Our method
0033   % 1 -> Get whole Sigma
0034   % 0 -> Get Sigma column by column
0035   
0036     ck=cputime;
0037     
0038     nStates=get(FF.P_Filter,'nStates');
0039     
0040     if i==nStates
0041       error('Self loop in  Update')
0042     end
0043     
0044     dim=get(FF.P_Filter,'dim'); % Dimensionality of the pose space
0045     rangeI=(dim*(i-1)+1):(dim*i);
0046     rangeN=(dim*(nStates-1)+1):(dim*nStates);
0047     
0048     ne=size(FF);
0049     
0050     mI=FF.mu(rangeI);
0051     Pi=Pose(mI);
0052     
0053     mN=FF.mu(rangeN);
0054     Pn=Pose(mN);
0055     
0056     [Ji Jn]=Absolute2RelativeJacobian(Pi,Pn);
0057     
0058     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0059     if FF.stateRecoveryMethod==2
0060       % The state recovery proposed in the paper.
0061       
0062       % Compute the innovation and the inverse of its Cholesky factor.
0063       M=GetMarginalCovariance(FF,State2Step(FF,i));
0064       J=[Jn Ji];
0065       
0066       S=J*M*J'+R;
0067       S=(S+S')/2; % This enhances numerical stability
0068       L=chol(S,'lower'); % S=L*L'
0069       iL=L\eye(dim); % L is triangular -> this is easy.
0070       
0071       % Compute the i-th block column of the current covariance matrix
0072       z=sparse2(ne,dim);
0073       z(rangeI,:)=sparse2(1:dim,1:dim,1);
0074       
0075       [Si stats]=cholmod2(FF.Lambda,z); % the i-th column
0076       mb=stats(5);
0077       
0078       % Compute the n-th block column of the current covariance matrix
0079       Sn=zeros(ne,dim);
0080       ind=1:dim;
0081       for j=1:nStates-1
0082         Sn(ind,:)=get(FF.PoseData{j},'crossCovarianceData');
0083         ind=ind+dim;
0084       end
0085       Sn=Sn*FF.F;
0086       Sn(rangeN,:)=M(1:dim,1:dim);
0087       
0088       % Compute the block column used to update the covariance and the mean
0089       T=[Sn Si]*J'*iL';
0090       
0091       % Update the mean
0092       yt=double(d-Absolute2Relative(Pi,Pn));
0093       mu=FF.mu+T*(iL*yt);
0094       
0095       % Compute the last column of the new covariance matrix
0096       Tn=T(rangeN,:);
0097       nSn=Sn-T*Tn';
0098       
0099       % Compute the new diagonal of the covariance matrix
0100       ind=1:dim;
0101       for j=1:nStates-1
0102         Tj=T(ind,:);
0103         mu(ind)=double(Pose(mu(ind)));
0104         FF.PoseData{j}=set(FF.PoseData{j},'updateInfo',mu(ind),Tj*Tj',nSn(ind,:));
0105         ind=ind+dim;
0106       end
0107       mu(rangeN)=double(Pose(mu(rangeN)));
0108       FF.PoseData{nStates}=set(FF.PoseData{nStates},'updateInfo',mu(rangeN),Tn*Tn',eye(dim));
0109       
0110       clear Sn Si T nSn;
0111     end
0112     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0113     
0114     iR=sparse2(R\eye(dim));
0115     
0116     H=sparse2(dim,ne);
0117     H(:,rangeI)=Ji;
0118     H(:,rangeN)=Jn;
0119     
0120     FF.Lambda=sparse2(FF.Lambda+H'*iR*H);
0121     if FF.stateRecoveryMethod~=2
0122       FF.eta=FF.eta+H'*iR*(double(d-Absolute2Relative(Pi,Pn))+H*FF.mu);
0123     end
0124     
0125     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0126     if FF.stateRecoveryMethod==1
0127       % A single cholmod2
0128       
0129       [S stats]=cholmod2(FF.Lambda,sparse2(1:ne,1:ne,1));
0130       mb=stats(5);
0131       
0132       mu=S*FF.eta;
0133       
0134       ind=1:dim;
0135       for j=1:nStates
0136         mu(ind)=double(Pose(mu(ind)));
0137         FF.PoseData{j}=set(FF.PoseData{j},'info',mu(ind),S(ind,ind),S(ind,rangeN));
0138         ind=ind+dim;
0139       end
0140       
0141       clear S;
0142     end
0143     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0144     
0145     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0146     if FF.stateRecoveryMethod==0
0147       % one system per column
0148       
0149       z=sparse2(ne,dim+1);
0150       z(rangeN,1:dim)=sparse2(1:dim,1:dim,1);
0151       z(:,dim+1)=FF.eta;
0152       
0153       [Sn stats]=cholmod2(FF.Lambda,z); % the last column + mean
0154       mb=stats(5);
0155       
0156       mu=Sn(:,dim+1);
0157       mu(rangeN)=double(Pose(mu(rangeN)));
0158       FF.PoseData{nStates}=set(FF.PoseData{nStates},'info',mu(rangeN),Sn(rangeN,1:dim),eye(dim));
0159       
0160       % We can decompose once and solve many times but this is not much
0161       % faster than decomposing+solving 'n' times.
0162       % Cholmod2 is optimized so that systems
0163       %           y=L\z
0164       %           Sj=L'\y
0165       % can be solved while decomposing.
0166       %L=lchol(FF.Lambda);
0167       
0168       ind=1:dim;
0169       for j=1:nStates-1
0170         
0171         z=sparse2(ne,dim);
0172         z(ind,:)=sparse2(1:dim,1:dim,1);
0173         
0174         [Sj stats]=cholmod2(FF.Lambda,z); % the j-th column
0175         mb=max(mb,stats(5));
0176         
0177         %y=L\z;
0178         %Sj=L'\y;
0179         
0180         mu(ind)=double(Pose(mu(ind)));
0181         FF.PoseData{j}=set(FF.PoseData{j},'info',mu(ind),Sj(ind,:),Sn(ind,1:dim));
0182         
0183         clear Sj;
0184         
0185         ind=ind+dim;
0186       end
0187       
0188       clear Sn;
0189       
0190     end
0191     %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0192     
0193     
0194     FF.mu=mu;
0195     FF.eta=FF.Lambda*FF.mu;
0196     FF.F=eye(dim);
0197     
0198     FF.cm2ne=[FF.cm2ne ne];
0199     FF.cm2t= [FF.cm2t  cputime-ck];
0200     FF.cm2mb=[FF.cm2mb mb];
0201


Institut de Robòtica i Informàtica Industrial

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