![]() |
GetRelativeDistance2PosePURPOSE
Relative displacement between the current pose and a previous one.
SYNOPSIS
function Grd=GetRelativeDistance2Pose(F,step)
DESCRIPTION
Relative displacement between the current pose and a previous one. Returns a set of Gaussians representing the first order error approximation of the displacement from the current robot's pose to the pose at time 'step'. The output includes one Gaussian for each dimension of the displacement between poses. This function is very simiar to GetDistance2Pose but here the pose displacement is computed in the current frame of the robot. See also GetDistance2Pose, GetGeneralDistance2Pose. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function Grd=GetRelativeDistance2Pose(F,step) 0002 % Relative displacement between the current pose and a previous one. 0003 % 0004 % Returns a set of Gaussians representing the first order error 0005 % approximation of the displacement from the current robot's pose to the 0006 % pose at time 'step'. 0007 % 0008 % The output includes one Gaussian for each dimension of the displacement 0009 % between poses. 0010 % 0011 % This function is very simiar to GetDistance2Pose but here the 0012 % pose displacement is computed in the current frame of the robot. 0013 % 0014 % See also GetDistance2Pose, GetGeneralDistance2Pose. 0015 0016 PoseN=Pose(GetPoseMean(F,F.nSteps)); 0017 PoseI=Pose(GetPoseMean(F,step)); 0018 0019 % First order moment of the estimation of the relative distance from 0020 % pn to pi in the frame of Pn 0021 m=double(Absolute2Relative(PoseN,PoseI)); 0022 0023 MarginalSigma=GetMarginalCovariance(F,step); 0024 0025 [Hn Hi]=Absolute2RelativeJacobian(PoseN,PoseI); 0026 0027 % This is shared with GetDistane2Pose and it can get 0028 % intervals as inputs. 0029 Grd=GetGeneralDistance2Pose(F.dim,m,MarginalSigma,[Hn Hi]); |