![]() |
EKF_UpdatePURPOSE
EKF Update step.
SYNOPSIS
function EKF=EKF_Update(EKF,i,d,R)
DESCRIPTION
EKF 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. Parameters EKF: The filter to update. i; The pose to be linked with the current robot's pose to form 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 EKF: The updated filter. See also EKF_Prediction, TRO_Prediction, TRO_Update. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function EKF=EKF_Update(EKF,i,d,R) 0002 % EKF 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 % 0011 % Parameters 0012 % EKF: The filter to update. 0013 % i; The pose to be linked with the current robot's pose to form a 0014 % loop. 0015 % d: Displacement from pose 'i' to the current robot's pose 'n' 0016 % according to the sensors readings. 0017 % R: Noise of the sensor 0018 % Outputs 0019 % EKF: The updated filter. 0020 % 0021 % See also EKF_Prediction, TRO_Prediction, TRO_Update. 0022 0023 % the dispalcement 'd' MUST be from 'i' to 'n' 0024 nStates=get(EKF.P_Filter,'nStates'); 0025 0026 if i==nStates 0027 error('Self loop in EKF Update') 0028 end 0029 0030 m=get(EKF.P_Filter,'dim'); % Dimensionality of the pose space 0031 rangeI=m*(i-1)+1:m*i; 0032 rangeN=m*(nStates-1)+1:m*nStates; 0033 0034 ne=size(EKF.mu,1); % number of elements in the state (m per pose) 0035 0036 mI=EKF.mu(rangeI); 0037 Pi=Pose(mI); 0038 0039 mN=EKF.mu(rangeN); 0040 Pn=Pose(mN); 0041 0042 y=double(d-Absolute2Relative(Pi,Pn)); 0043 0044 H=sparse(zeros(m,ne)); 0045 0046 [Ji Jn]=Absolute2RelativeJacobian(Pi,Pn); 0047 H(:,rangeI)=Ji; 0048 H(:,rangeN)=Jn; 0049 0050 SHt=EKF.Sigma*H'; 0051 0052 S=H*SHt+R; 0053 S=(S+S')/2; % This enforces symmetry and enhances numerical stability 0054 0055 L=chol(S); 0056 iL=inv(L); 0057 W1=SHt*iL; 0058 0059 EKF.mu=EKF.mu + W1*iL'*y; 0060 0061 EKF.Sigma=EKF.Sigma - W1*W1'; |