Change hardcoded values to defines

1 files changed, 9 insertions, 6 deletions

diff --git a/src/feature.cpp b/src/feature.cpp
index 9dce1d1..fc08ee7 100644
--- a/src/feature.cpp
+++ b/src/feature.cpp
@@ -132,7 +132,7 @@ Feature::Q ( const double dt )
 {
     Matrix<double,3,3> Q;
     Q = Matrix<double,3,3>::Identity();
-    Q *= dt*dt;//*1e-1;
+    Q *= dt*dt*FEATURE_NOISE;//*1e-1;
     return Q;
 }		/* -----  end of method Feature::q  ----- */
 
@@ -141,7 +141,9 @@ Feature::R (  )
 {
     Matrix<double,6,6> R;
     R = Matrix<double,6,6>::Identity();
-    R *= 1e-4;
+    R.block<2,2>(0,0) *= VIEW_NOISE;
+    R.block<2,2>(2,2) *= INITIAL_VIEW_NOISE;
+    R.block<2,2>(4,4) *= REFLECTION_VIEW_NOISE;
     return R;
 }		/* -----  end of method Feature::R  ----- */
 
@@ -492,11 +494,12 @@ Matrix<double,3,3>
 Feature::P0 ( )
 {
     Matrix<double,3,3> P;
-    double p0 = 1e-4;
-    double p1 = 1e-4;
+    double p0 = FEATURECOVX;
+    double p1 = FEATURECOVY;
+    double p2 = FEATURECOVRHO; 
     P << p0, 0., 0., 
-         0., p0, 0.,
-         0., 0., p1;
+         0., p1, 0.,
+         0., 0., p2;
     return P;
 }		/* -----  end of method Feature::P0  ----- */