From 8d73fd6f27ff524be88c2a2e06a67e22f47d28b9 Mon Sep 17 00:00:00 2001
From: Miller
Date: Fri, 7 Jul 2017 04:34:34 -0500
Subject: fix param and P0 bugs

---
 src/state.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index 8b1ff3f..b521eff 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -21,7 +21,7 @@ State::State ( )
 {
     body = new Body;
     P = Matrix<double,Dynamic,Dynamic>::Zero(STATESIZE,STATESIZE);
-    P.block<3,3>(6,6) = covbias*Matrix3d::Identity();
+    P.block<3,3>(STATESIZE-3,STATESIZE-3) = covbias*Matrix3d::Identity();
     return ;
 }		/* -----  end of method State::State  ----- */
 
-- 
cgit v1.1