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/feature.h | 10 +++++----- src/state.cpp | 3 ++- src/state.h | 1 + 3 files changed, 8 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/feature.h b/src/feature.h index 29c8098..07fe5ef 100644 --- a/src/feature.h +++ b/src/feature.h @@ -5,11 +5,11 @@ #include "types.h" #define FEATURE_NOISE 1e-3 /* Feature process noise */ -#define VIEW_NOISE 1e-3 /* */ -#define INITIAL_VIEW_NOISE 1e-3 /* */ -#define REFLECTION_VIEW_NOISE 1e-2 /* */ -#define FEATURECOVX 5e-5 /* */ -#define FEATURECOVY 5e-5 /* */ +#define VIEW_NOISE 0.35 /* */ +#define INITIAL_VIEW_NOISE 1. /* */ +#define REFLECTION_VIEW_NOISE .35 /* */ +#define FEATURECOVX 1. /* */ +#define FEATURECOVY 1. /* */ #define FEATURECOVRHO 5e-3 /* */ #define FEATURECOVRHO_MONO 0.5 /* */ #define RHO_0 1./10. /* */ 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::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 &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); } diff --git a/src/state.h b/src/state.h index 11bfa46..92a83ab 100644 --- a/src/state.h +++ b/src/state.h @@ -10,6 +10,7 @@ #include "types.h" #define MAXFEATURES 50 +#define COVBIAS 1e-7 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define FULLS // Comment out to treat each measurement independently. //#define BLOCKSI -- cgit v1.1