1.2 m error on 1112-1-3

1 files changed, 2 insertions, 1 deletions

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);
         }