From ed6788fff43f8457f1a029ec9be3b6d74c0dd630 Mon Sep 17 00:00:00 2001
From: Luca <luca.carlone@gatech.edu>
Date: Mon, 7 Apr 2014 11:56:22 -0400
Subject: [PATCH] fixed bug

---
 .../+imuSimulator/covarianceAnalysisBetween.m                  | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
index 1701de64b..bbd5dfff3 100644
--- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
+++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m
@@ -85,11 +85,12 @@ for k=1:numMonteCarloRuns
     currentPoseKey = symbol('x', i-1);
     gtPosition  = gtValues.at(currentPoseKey).translation.vector;
     estPosition = estimate.at(currentPoseKey).translation.vector;
+    estR = estimate.at(currentPoseKey).rotation.matrix;
     errPosition = estPosition - gtPosition;
     
     % compute covariances:
     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
     NEES(k,i) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof