![]() |
UpdateLoopPURPOSE
Adds a new loop to an EKF filter.
SYNOPSIS
function EKF=UpdateLoop(EKF,linked,dS,RS)
DESCRIPTION
Adds a new loop to an EKF filter. Updates the estimated trajectory introducing a loop closure. Parameters: EKF: The filter with the estimated trajectory linked: The pose to be linked with the current robot's pose. dS: The sensor readings linking the two poses. RS: The noise for the sensors. See also EKF_Update. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function EKF=UpdateLoop(EKF,linked,dS,RS) 0002 % Adds a new loop to an EKF filter. 0003 % 0004 % Updates the estimated trajectory introducing a loop closure. 0005 % Parameters: 0006 % EKF: The filter with the estimated trajectory 0007 % linked: The pose to be linked with the current robot's pose. 0008 % dS: The sensor readings linking the two poses. 0009 % RS: The noise for the sensors. 0010 % 0011 % See also EKF_Update. 0012 0013 0014 l=Step2State(EKF.P_Filter,linked); 0015 if (l<1) 0016 error('Trying to form close loop with a removed pose'); 0017 end 0018 0019 ns=size(RS,2); 0020 for i=1:ns 0021 EKF=EKF_Update(EKF,l,dS{i},RS{i}); 0022 end 0023 0024 % Call the UpdateLoop for the generic filter 0025 EKF.P_Filter=UpdateLoop(EKF.P_Filter,linked,dS,RS); |