params for body noise

3 files changed, 22 insertions, 10 deletions

diff --git a/src/body.cpp b/src/body.cpp
index 018075b..5579e4d 100644
--- a/src/body.cpp
+++ b/src/body.cpp
@@ -101,7 +101,7 @@ Matrix<double,1,1>
 Body::R()
 {
     Matrix<double,1,1> R;
-    R << R_HEIGHT;
+    R << r_height;
     return R;
 }
 
@@ -142,12 +142,12 @@ Body::Q (double dt)
 #endif
     Matrix<double,STATESIZE,STATESIZE> Q;
     Q = Matrix<double,STATESIZE,STATESIZE>::Zero();
-    Q = ACC_STD*ACC_STD*Ga*Ga.transpose();
-    Q += ACC_BIAS_STD*ACC_BIAS_STD*Gb*Gb.transpose();
-    Q += ANG_STD*ANG_STD * Gw * Gw.transpose();
+    Q = acc_std*acc_std*Ga*Ga.transpose();
+    Q += acc_bias_std*acc_bias_std*Gb*Gb.transpose();
+    Q += ang_std*ang_std * Gw * Gw.transpose();
 
 #if STATESIZE==13
-    Q += ANG_STD*ANG_STD *Gq * Gq.transpose();
+    Q += ang_std*ang_std *Gq * Gq.transpose();
 #endif
 
 
diff --git a/src/main.cpp b/src/main.cpp
index c1d2a27..81b7df7 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -23,7 +23,14 @@
 bool seenutm=false;
 bool seenpva=false;
 bool seencov=false;
-double feature_noise,view_noise,initial_view_noise,reflection_view_noise;
+
+double feature_noise = 3.5;
+double view_noise = 0.0006;
+double initial_view_noise = 0.0001;
+double reflection_view_noise = 0.0006;
+
+double r_height,acc_std,ang_std,acc_bias_std;
+
 using std::cout;
 using std::endl;
 using std::cerr;
@@ -452,10 +459,10 @@ main(int argc, char **argv)
     // read params file
     FILE *pfin;
     pfin = fopen(argv[2], "r");
-    fscanf(pfin,"%lf",&feature_noise);
-    fscanf(pfin,"%lf",&view_noise);
-    fscanf(pfin,"%lf",&initial_view_noise);
-    fscanf(pfin,"%lf",&reflection_view_noise);
+    fscanf(pfin,"%lf",&r_height);
+    fscanf(pfin,"%lf",&acc_std);
+    fscanf(pfin,"%lf",&ang_std);
+    fscanf(pfin,"%lf",&acc_bias_std);
     fclose(pfin);
     // Read sensors from file
     int i=0;
diff --git a/src/types.h b/src/types.h
index e7fa82f..c0e5833 100644
--- a/src/types.h
+++ b/src/types.h
@@ -21,6 +21,11 @@ extern double view_noise;
 extern double initial_view_noise;
 extern double reflection_view_noise;
 
+extern double r_height;
+extern double acc_std;
+extern double ang_std;
+extern double acc_bias_std;
+
 typedef Eigen::Matrix<double,2,1,Eigen::DontAlign> UVector2d;
 // A struct for storing measurements.
 typedef enum {BOTH,REFLECTION,MONO,HEIGHT} measurement_type;