![]() |
UpdateLoopPURPOSE
Adds a new loop to a TRO filter.
SYNOPSIS
function FF=UpdateLoop(FF,linked,dS,RS)
DESCRIPTION
Adds a new loop to a TRO filter. Updates the estimated trajectory introducing a loop closure. Parameters: FF: 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 (ceel array). RS: The noise for the sensors (cell array). See also FF_Update. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function FF=UpdateLoop(FF,linked,dS,RS) 0002 % Adds a new loop to a TRO filter. 0003 % 0004 % Updates the estimated trajectory introducing a loop closure. 0005 % Parameters: 0006 % FF: 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 (ceel array). 0009 % RS: The noise for the sensors (cell array). 0010 % 0011 % See also FF_Update. 0012 0013 l=Step2State(FF.P_Filter,linked); 0014 if (l<1) 0015 error('Trying to form close loop with a removed pose'); 0016 end 0017 0018 ns=size(RS,2); 0019 G=SensorFusion(1:ns,dS,RS); 0020 0021 FF=TRO_Update(FF,l,Pose(get(G,'mean')),get(G,'covariance')); 0022 0023 % Call the UpdateLoop for the generic filter 0024 FF.P_Filter=UpdateLoop(FF.P_Filter,linked,dS,RS); |