![]() |
TRO_UpdatePURPOSE
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:
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 |