![]() |
GetCrossCovarianceWithPosePURPOSE
Cross covariance of a given pose with the current one.
SYNOPSIS
function CC=GetCrossCovarianceWithPose(EKF,step)
DESCRIPTION
Cross covariance of a given pose with the current one. Returns the croos covariance of the pose at time slice 'step' with the current robot's pose. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function CC=GetCrossCovarianceWithPose(EKF,step) 0002 % Cross covariance of a given pose with the current one. 0003 % 0004 % Returns the croos covariance of the pose at time slice 'step' with the 0005 % current robot's pose. 0006 0007 if isa(step,'double') 0008 nStates=get(EKF.P_Filter,'nStates'); 0009 dim=get(EKF.P_Filter,'dim'); 0010 0011 i=Step2State(EKF.P_Filter,step); 0012 0013 ind=(i-1)*dim+1:i*dim; 0014 last=(nStates-1)*dim+1:nStates*dim; 0015 0016 CC=EKF.Sigma(ind,last); 0017 else 0018 if isa(step,'PoseData') 0019 CC=get(step,'crossCovarianceData'); 0020 else 0021 error('Unknown 2nd parameter type in GetCrossCovarianceWithPose (EKF)'); 0022 end 0023 end 0024 |