From e5e65d31eb86fc41ddc68e5bb09f3729f8c009ae Mon Sep 17 00:00:00 2001
From: Martin Miller
Date: Wed, 29 Mar 2017 16:04:02 -0500
Subject: 1.2 m error on 1112-1-3

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

(limited to 'src/state.cpp')

diff --git a/src/state.cpp b/src/state.cpp
index 4677595..94e8a5c 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -21,7 +21,7 @@ State::State ( )
 {
     body = new Body;
     P = Matrix<double,Dynamic,Dynamic>::Zero(9,9);
-    P.block<3,3>(6,6) = 1e-6*Matrix3d::Identity();
+    P.block<3,3>(6,6) = COVBIAS*Matrix3d::Identity();
     return ;
 }		/* -----  end of method State::State  ----- */
 
@@ -421,6 +421,7 @@ State::addFeatures ( std::vector<measurement_t> &F, const Quaterniond &q, double
         Feature *f;
         if (i->z_type==REFLECTION) {
             f = new Feature(i->id, i->source, i->reflection, pos, q, z);
+            //f = new Feature(i->id, i->source, pos, q);
         } else {
             f = new Feature(i->id, i->source, pos, q);
         }
-- 
cgit v1.1