diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/feature.h | 10 | ||||
| -rw-r--r-- | src/state.cpp | 3 | ||||
| -rw-r--r-- | src/state.h | 1 |
3 files changed, 8 insertions, 6 deletions
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<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); } 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 |