From bcdfea37d9d06ddfba2f66d820cf2e444cc43aac Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Tue, 22 Dec 2015 19:28:49 -0800
Subject: [PATCH] pick out correct blocks

---
 gtsam/navigation/ScenarioRunner.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/gtsam/navigation/ScenarioRunner.h b/gtsam/navigation/ScenarioRunner.h
index fa07b290f..310d96d6f 100644
--- a/gtsam/navigation/ScenarioRunner.h
+++ b/gtsam/navigation/ScenarioRunner.h
@@ -74,10 +74,10 @@ class ScenarioRunner {
   /// Return pose covariance by re-arranging pim.preintMeasCov() appropriately
   Matrix6 poseCovariance(
       const ImuFactor::PreintegratedMeasurements& pim) const {
-    Matrix9 cov = pim.preintMeasCov();  // _ position rotation
+    Matrix9 cov = pim.preintMeasCov();
     Matrix6 poseCov;
-    poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 3),  //
-        cov.block<3, 3>(3, 6), cov.block<3, 3>(3, 3);
+    poseCov << cov.block<3, 3>(6, 6), cov.block<3, 3>(6, 0),  //
+        cov.block<3, 3>(0, 6), cov.block<3, 3>(0, 0);
     return poseCov;
   }