![]() |
UpdateLoopPURPOSE
Adds a loop to the filter.
SYNOPSIS
function F=UpdateLoop(F,linked,dS,RS)
DESCRIPTION
Adds a loop to the filter. Updates the the filter when a loop is closed (i.e., when we add a link between the current robot's pose and one of the previous poses). CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function F=UpdateLoop(F,linked,dS,RS) 0002 % Adds a loop to the filter. 0003 % 0004 % Updates the the filter when a loop is closed (i.e., when we add a link 0005 % between the current robot's pose and one of the previous poses). 0006 0007 loop.p1=F.nSteps; 0008 loop.p2=linked; 0009 0010 % More than one sensor can confirm the loop. Just store the fusion 0011 % of them 0012 ns=size(RS,2); 0013 G=SensorFusion(1:ns,dS,RS); 0014 loop.displacement=Pose(get(G,'mean')); 0015 loop.noise=get(G,'covariance'); 0016 0017 F.nLoops=F.nLoops+1; 0018 F.loops{F.nLoops}=loop; 0019 |