From 9b559b362009e9a99c0cba42922cd7463b19aa3d Mon Sep 17 00:00:00 2001
From: Frank Dellaert <frank@skyd.io>
Date: Tue, 22 Dec 2015 19:09:05 -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..4e15e00a1 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>(0, 0), cov.block<3, 3>(0, 3),  //
+        cov.block<3, 3>(3, 0), cov.block<3, 3>(3, 3);
     return poseCov;
   }