From 6c03b6e316d72d2bf8cf35a5b7890c646719ff48 Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Mon, 26 Jun 2017 17:12:54 -0500
Subject: 10pct mean error

---
 src/body.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src/body.cpp')

diff --git a/src/body.cpp b/src/body.cpp
index 885b2ad..018075b 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -125,11 +125,12 @@ Body::Q (double dt)
     Gb.block<3,3>(0,0) = -0.5*dt*dt*Matrix3d::Identity();
     Gb.block<3,3>(3,0) = -dt*Matrix3d::Identity();
     Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
+    Gb.block<3,3>(STATESIZE-3,0) = dt*Matrix3d::Identity();
 
     Matrix<double,STATESIZE,3> Gw;
     Gw.block<3,3>(0,0) = 0.5*dt*dt*skewSymmetric(vel());
     Gw.block<3,3>(3,0) = dt*skewSymmetric(vel());
-    Gb.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
+    Gw.block<STATESIZE-6,3>(6,0) = Matrix<double,STATESIZE-6,3>::Zero();
 
 #if STATESIZE==13
     Matrix<double,STATESIZE,3> Gq;
-- 
cgit v1.1