set biases

1 files changed, 11 insertions, 11 deletions

diff --git a/src/main.cpp b/src/main.cpp
index fb86f52..a6b66bc 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -41,12 +41,12 @@ double ransac_li_threshold = 1.8e-6;
 double ransac_hi_threshold = 0.25;
 double covbias = 2e-5;
 
-double angbiasx;
-double angbiasy;
-double angbiasz;
-double accbiasx;
-double accbiasy;
-double accbiasz;
+double angbiasx =  -0.0020490;
+double angbiasy =  0.0050130;
+double angbiasz = 0.0019250;
+double accbiasx = -0.007870;
+double accbiasy = 0.0179085;
+double accbiasz = -0.006310;
 
 using std::cout;
 using std::endl;
@@ -451,9 +451,6 @@ main(int argc, char **argv)
     Quaterniond qbw;
     attitude_t att;
     // bias in FRD coordinates
-    Vector3d acc_bias;
-    acc_bias << accbiasx, accbiasy, accbiasz;
-    mu.accelerometer_bias(acc_bias);
     timestamp dt{0,0};
     timestamp t_old{0,0};
 
@@ -476,7 +473,6 @@ 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);
@@ -490,14 +486,18 @@ main(int argc, char **argv)
     fscanf(pfin,"%lf",&ransac_li_threshold);
     fscanf(pfin,"%lf",&ransac_hi_threshold);
     fscanf(pfin,"%lf",&covbias);
-    */
+    /*
     fscanf(pfin,"%lf",&angbiasx);
     fscanf(pfin,"%lf",&angbiasy);
     fscanf(pfin,"%lf",&angbiasz);
     fscanf(pfin,"%lf",&accbiasx);
     fscanf(pfin,"%lf",&accbiasy);
     fscanf(pfin,"%lf",&accbiasz);
+    */
     fclose(pfin);
+    Vector3d acc_bias;
+    acc_bias << accbiasx, accbiasy, accbiasz;
+    mu.accelerometer_bias(acc_bias);
     // Read sensors from file
     int i=0;
     char line[MAXLINE];