![]() |
GetDistance2PosePURPOSE
Distance from the current pose to a previous pose (estimated as a Gaussian).
SYNOPSIS
function Gd=GetDistance2Pose(F,step)
DESCRIPTION
Distance from the current pose to a previous pose (estimated as a Gaussian). 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 GetRelativeDistance2Pose but here the pose displacement is computed in the global frame. See also GetRelativeDistance2Pose, GetGeneralDistance2Pose. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function Gd=GetDistance2Pose(F,step) 0002 % Distance from the current pose to a previous pose (estimated as a Gaussian). 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 GetRelativeDistance2Pose but here the 0012 % pose displacement is computed in the global frame. 0013 % 0014 % See also GetRelativeDistance2Pose, GetGeneralDistance2Pose. 0015 0016 0017 PoseN=Pose(GetPoseMean(F,F.nSteps); 0018 PoseI=Pose(GetPoseMean(F,step)); 0019 0020 % First order moment of the estimation of the relative squared distance from 0021 % pn to pi in global coordinates 0022 m=double(PoseN-PoseI); 0023 0024 MarginalSigma=GetMarginalCovariance(F,step); 0025 0026 H=[eye(F.dim) -eye(F.dim)]; 0027 0028 % This is shared with GetRelativeDistane2Pose and it can get 0029 % intervals as inputs. 0030 Gd=GetGeneralDistance2Pose(F.dim,m,MarginalSigma,H); |