12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed bug

release/4.3a0
Luca 2014-04-07 11:56:22 -04:00
parent dfda7fad89
commit ed6788fff4
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
View File

@ -85,11 +85,12 @@ for k=1:numMonteCarloRuns
currentPoseKey = symbol('x', i-1); currentPoseKey = symbol('x', i-1);
gtPosition = gtValues.at(currentPoseKey).translation.vector; gtPosition = gtValues.at(currentPoseKey).translation.vector;
estPosition = estimate.at(currentPoseKey).translation.vector; estPosition = estimate.at(currentPoseKey).translation.vector;
estR = estimate.at(currentPoseKey).rotation.matrix;
errPosition = estPosition - gtPosition; errPosition = estPosition - gtPosition;
% compute covariances: % compute covariances:
cov = marginals.marginalCovariance(currentPoseKey); cov = marginals.marginalCovariance(currentPoseKey);
covPosition = cov(4:6,4:6); covPosition = estR * cov(4:6,4:6) * estR';
% compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances
NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof