[paparazzi-commits] [6280] fixed the initialization a bit

[Martin Dieblich]

Revision: 6280
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6280
Author:   mdieblich
Date:     2010-10-27 13:10:20 +0000 (Wed, 27 Oct 2010)
Log Message:
-----------
fixed the initialization a bit

Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
    paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp     
2010-10-27 12:55:50 UTC (rev 6279)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp     
2010-10-27 13:10:20 UTC (rev 6280)
@@ -11,7 +11,7 @@
 
 //useless initialization (I hate C++)
 static basic_ins_qkf ins = basic_ins_qkf(Vector3d::Zero(), 0, 0, 0,
-                                        gyroscope_noise,gyroscope_noise*0.1, 
accelerometer_noise);
+                                        gyro_stability_noise,gyroscope_noise, 
accelerometer_noise);
 
 // import most common Eigen types 
 USING_PART_OF_NAMESPACE_EIGEN
@@ -129,7 +129,7 @@
     }
     if(BARO_READY(e.message.valid_sensors)){
       baro_measurements++;
-      printf("baro_0_height before: %7f\n", baro_0_height);
+      // TODO: Fix it!
       //NEW_MEAN(baro_0_height, 
BARO_FLOAT_OF_BFP(e.message.pressure_absolute), baro_measurements);
       baro_0_height = 
(baro_0_height*(baro_measurements-1)+BARO_FLOAT_OF_BFP(e.message.pressure_absolute))/baro_measurements;
     }
@@ -183,9 +183,7 @@
   q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6, sigmaB, 
&sigma_q);
        orientation_0 = ecef2body_from_pprz_ned2body(pos_0_ecef,q_ned2body);
   
-  printf("baro_0_height before: %7f\n", baro_0_height);
   baro_0_height += pos_0_ecef.norm();
-  printf("baro_0_height after:  %7f\n", baro_0_height);
   
   struct DoubleEulers sigma_eu = sigma_euler_from_sigma_q(q_ned2body, sigma_q);
   orientation_cov_0 = EULER_AS_VECTOR3D(sigma_eu);
@@ -207,10 +205,10 @@
       printf("%6.2fs %6i\n", e.time, entry_counter);
       t += 10;
     }
-    //if ((e.time<22.58)||(e.time>22.6)){
+    if ((e.time<68.48)||(e.time>68.51)){
+      print_estimator_state(e.time);
       main_run_ins(e.message.valid_sensors);
-      print_estimator_state(e.time);
-    //} 
+    } 
     e = read_raw_log_entry(file_descriptor, &read_ok);
   }
 }
@@ -288,7 +286,7 @@
               Vector3d::Ones() *  angle_cov  *  angle_cov ,
                                                        Vector3d::Ones() *  
pos_cov_0  *  pos_cov_0 ,
                                                        Vector3d::Ones() * 
speed_cov_0 * speed_cov_0;*/
-  diag_cov << 0.1 * gyroscope_noise,
+  diag_cov << gyro_stability_noise,
               orientation_cov_0,
                                                        pos_cov_0,
                                                        speed_cov_0;
@@ -329,10 +327,14 @@
   
   #ifdef EKNAV_FROM_LOG_DEBUG
   printf("Right after initialization:\n");
-  DISPLAY_FLOAT_QUAT("\t ned2body quaternion:", q_ned2body);
-  DISPLAY_FLOAT_QUAT("\tecef2enu  quaternion:", q_ecef2enu);
-  DISPLAY_FLOAT_QUAT("\tecef2ned  quaternion:", q_ecef2ned);
-  DISPLAY_FLOAT_QUAT("\tecef2body quaternion:", q_ecef2body);
+  DISPLAY_DOUBLE_QUAT("\t ned2body quaternion:", q_ned2body);
+  DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ned2body);
+  DISPLAY_DOUBLE_QUAT("\tecef2enu  quaternion:", q_ecef2enu);
+  DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2enu);
+  DISPLAY_DOUBLE_QUAT("\tecef2ned  quaternion:", q_ecef2ned);
+  DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2ned);
+  DISPLAY_DOUBLE_QUAT("\tecef2body quaternion:", q_ecef2body);
+  DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG("\t\t\t", q_ecef2body);
   #endif
   
   return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body);

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp     
2010-10-27 12:55:50 UTC (rev 6279)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp     
2010-10-27 13:10:20 UTC (rev 6280)
@@ -90,6 +90,7 @@
 // NOTE: Measured during hovering in the air. Movement in the range of a 1 m³ 
cube with (approx.) max. 0.2 m/s speed.
 /// IMU
 const Vector3d gyroscope_noise      ( 1.0449e-1,  1.1191e-1,  4.5906e-2 );
+const Vector3d gyro_stability_noise ( 1.0000e-6,  1.0000e-6,  1.0000e-6 );
 const Vector3d accelerometer_noise  ( 2.5457e+0,  1.8242e+0,  1.5660e+0 );
 const unsigned short imu_frequency  = 512;
 
@@ -105,7 +106,8 @@
 //const Vector3d gps_pos_noise        = Vector3d::Ones() *10  *10  ;
 
 const double baro_noise             =  0.25;
-#define BARO_SCALING                  10.0
+// measured with GPS while climbing approximately 80 m
+#define BARO_SCALING                  10.17
 
 //const double   mag_error            = 2.536e-3;
 //const Vector3d gyro_white_noise     (  1.1328*1.1328e-4,    
0.9192*0.9192e-4,    1.2291*1.2291e-4);
@@ -193,7 +195,17 @@
           mat.m[0], mat.m[1], mat.m[2], mat.m[3], mat.m[4], mat.m[5],  \
           mat.m[6], mat.m[7], mat.m[8]);                               \
   }
-#define DISPLAY_FLOAT_QUAT(text, quat) {                               \
+#define DISPLAY_DOUBLE_QUAT(text, quat) {                              \
     double quat_norm = NORM_VECT4(quat);                               \
     printf("%s %f %f %f %f (%f)\n",text, quat.qi, quat.qx, quat.qy, quat.qz, 
quat_norm); \
   }
+#define DISPLAY_DOUBLE_EULERS_DEG(text, _e) {                          \
+  printf("%s %f %f %f\n",text,  DegOfRad((_e).phi),                    \
+  DegOfRad((_e).theta), DegOfRad((_e).psi));                   \
+}
+
+#define DISPLAY_DOUBLE_QUAT_AS_EULERS_DEG(text, quat) {                        
\
+    struct DoubleEulers _fe;                                           \
+    DOUBLE_EULERS_OF_QUAT(_fe, quat);                                  \
+    DISPLAY_DOUBLE_EULERS_DEG(text, _fe);                              \
+  }

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h      
2010-10-27 12:55:50 UTC (rev 6279)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h      
2010-10-27 13:10:20 UTC (rev 6280)
@@ -123,10 +123,10 @@
   }
   
 #define QUAT_ENU_FROM_TO_NED(from, to){        \
-       to.qi =  from.qx + from.qy;                                             
\
-  to.qx = -from.qi - from.qz;                                          \
-  to.qy = -from.qi + from.qz;                                          \
-  to.qz =  from.qx - from.qy;                                          \
+       to.qi = - from.qx - from.qy;                                    \
+  to.qy = + from.qi + from.qz;                                 \
+  to.qx = + from.qi - from.qz;                                 \
+  to.qz = - from.qx + from.qy;                                 \
   QUAT_SMUL(to, to, M_SQRT1_2);                                        \
 }