[paparazzi-commits] [6162] Added initialisation for the filter

[Martin Dieblich]

Revision: 6162
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6162
Author:   mdieblich
Date:     2010-10-17 18:30:56 +0000 (Sun, 17 Oct 2010)
Log Message:
-----------
Added initialisation for the filter

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-17 00:10:51 UTC (rev 6161)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.cpp     
2010-10-17 18:30:56 UTC (rev 6162)
@@ -13,10 +13,8 @@
 
 int main(int, char *argv[]) {
 
-  std::cout << "test libeknav 3" << std::endl;
-  main_init();
+  printf("==============================\nRunning libeknav from 
File...\n==============================\n");
   
-  
   int raw_log_fd = open(argv[1], O_RDONLY); 
   
   if (raw_log_fd == -1) {
@@ -24,18 +22,13 @@
     return -1;
   }
   
-  while (1) {
-    struct raw_log_entry e;
-    ssize_t nb_read = read(raw_log_fd, &e, sizeof(e));
-    if (nb_read != sizeof(e)) break;
-    
-    COPY_RATES_ACCEL_TO_IMU_FLOAT(e);
-    COPY_MAG_TO_IMU_FLOAT(e);
-    COPY_GPS_TO_IMU(e);
-    main_run_ins(e.data_valid);
-    print_estimator_state(e.time);
-  }
+  printf("Initialisation...\n");
+  struct raw_log_entry e = first_entry_after_initialisation(raw_log_fd);
+  main_init();
+  printf("Running filter from file...\n");
+  main_run_from_file(raw_log_fd, next_GPS(raw_log_fd));
   
+  printf("Finished\n");
   return 0;
 
 }
@@ -58,6 +51,75 @@
 
 }
 
+static struct raw_log_entry first_entry_after_initialisation(int 
file_descriptor){
+  int        imu_measurements = 0,      // => Gyro + Accel
+    magnetometer_measurements = 0,
+             gps_measurements = 0;      // only the position
+  
+  struct DoubleMat33 attitude_profile_matrix;
+  struct Orientation_Measurement  gravity,
+                                  magneto,
+                                  fake;  
+  struct DoubleQuat q_ned2body;
+  
+  /* Prepare the attitude profile matrix */
+  FLOAT_MAT33_ZERO(attitude_profile_matrix);
+  
+  /* set the gravity measurement */
+  VECT3_ASSIGN(gravity.reference_direction, 0,0,1);
+  gravity.weight_of_the_measurement = 1;
+  
+  /* set the magneto - measurement */
+  EARTHS_GEOMAGNETIC_FIELD_NORMED(magneto.reference_direction);
+  magneto.weight_of_the_measurement = 1;
+    
+  uint8_t read_ok = 1;
+  struct raw_log_entry e = next_GPS(file_descriptor);
+  
+  while( (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, 
magnetometer_measurements, gps_measurements) ){
+    if(IMU_READY(e.data_valid)){
+      imu_measurements++;
+      
+      // update the estimated bias
+      bias_0 = NEW_MEAN(bias_0, RATES_AS_VECTOR3D(e.gyro), imu_measurements);
+      
+      // update the attitude profile matrix
+      VECT3_COPY(gravity.measured_direction,e.accel);
+      add_orientation_measurement(&attitude_profile_matrix, gravity);
+    }
+    if(MAG_READY(e.data_valid)){
+      magnetometer_measurements++;
+      // update the attitude profile matrix
+      VECT3_COPY(magneto.measured_direction,e.mag);
+      add_orientation_measurement(&attitude_profile_matrix, magneto);
+      
+      // now, generate fake measurement with the last gravity measurement
+      fake = fake_orientation_measurement(gravity, magneto);
+      add_orientation_measurement(&attitude_profile_matrix, fake);
+    }
+    if(GPS_READY(e.data_valid)){
+      gps_measurements++;
+      // update the estimated bias
+      pos_0_ecef = NEW_MEAN(pos_0_ecef, VECT3_AS_VECTOR3D(e.ecef_pos)/100, 
gps_measurements);
+    }
+    
+    e = read_raw_log_entry(file_descriptor, &read_ok);
+  }
+  q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6);
+       orientation_0 = ecef2body_from_pprz_ned2body(pos_0_ecef,q_ned2body);
+  return e;
+}
+
+static void main_run_from_file(int file_descriptor, struct raw_log_entry 
first_entry){
+  struct raw_log_entry e = first_entry;
+  uint8_t read_ok = 1;
+  while (read_ok) {
+    main_run_ins(e.data_valid);
+    print_estimator_state(e.time);
+    e = read_raw_log_entry(file_descriptor, &read_ok);
+  }
+}
+
 static void main_run_ins(uint8_t data_valid) {
   
   double dt_imu_freq = 0.001953125; //  1/512; // doesn't work?
@@ -87,17 +149,36 @@
        
        ins_logfile = fopen(INS_LOG_FILE, "w");
        
-       LLA_ASSIGN(pos_0_lla, TOULOUSE_LATTITUDE, TOULOUSE_LONGITUDE, 
TOULOUSE_HEIGHT)
-       PPRZ_LLA_TO_EIGEN_ECEF(pos_0_lla, pos_0_ecef);
-       
-       printf("Starting position\t%f\t%f\t%f\n", pos_0_ecef(0), pos_0_ecef(1), 
pos_0_ecef(2));
-       
        ins.avg_state.position    = pos_0_ecef;
-       ins.avg_state.gyro_bias   = Vector3d::Zero();
-       ins.avg_state.orientation = Quaterniond::Identity();
+       ins.avg_state.gyro_bias   = bias_0;
+       ins.avg_state.orientation = orientation_0;
        ins.avg_state.velocity    = speed_0_ecef;
+  
+  struct DoubleQuat ecef2body;
+  struct DoubleEulers eu_ecef2body;
+  QUATERNIOND_AS_DOUBLEQUAT(ecef2body, orientation_0);
+  DOUBLE_EULERS_OF_QUAT(eu_ecef2body, ecef2body);
+
+  
+       printf("Initial state\n\n");
+  printf("Bias        % 7.2f\n", bias_0(0)*180*M_1_PI);
+  printf("Bias        % 7.2f\n", bias_0(1)*180*M_1_PI);
+  printf("Bias        % 7.2f\n", bias_0(2)*180*M_1_PI);
+  printf("\n");
+  printf("Orientation % 7.2f\n", orientation_0.w());
+  printf("Orientation % 7.2f %7.2f\n", orientation_0.x(),   
eu_ecef2body.phi*180*M_1_PI);
+  printf("Orientation % 7.2f %7.2f\n", orientation_0.y(), 
eu_ecef2body.theta*180*M_1_PI);
+  printf("Orientation % 7.2f %7.2f\n", orientation_0.z(),   
eu_ecef2body.psi*180*M_1_PI); 
+  printf("\n");
+  printf("Position    % 9.0f\n", pos_0_ecef(0));
+  printf("Position    % 9.0f\n", pos_0_ecef(1));
+  printf("Position    % 9.0f\n", pos_0_ecef(2));
+  printf("\n");
+  printf("Velocity    % 7.2f\n", speed_0_ecef(0));
+  printf("Velocity    % 7.2f\n", speed_0_ecef(1));
+  printf("Velocity    % 7.2f\n", speed_0_ecef(2));
+  printf("\n");
        
-       
        Matrix<double, 12, 1> diag_cov;
        diag_cov << Vector3d::Ones() * bias_cov_0  * bias_cov_0 ,
                                                        Vector3d::Ones() *  
M_PI*0.5   *  M_PI*0.5  ,
@@ -118,29 +199,54 @@
        ltp_def_from_ecef_d(&current_ltp, &pos_0_ecef_pprz);
        ecef_of_ned_vect_d(&ref_dir_ecef, &current_ltp, &ref_dir_ned);
        
-       //              THIS SOMEWHERE ELSE!
-       DoubleQuat initial_orientation;
-       FLOAT_QUAT_ZERO(initial_orientation);
-       ENU_NED_transformation(current_ltp.ltp_of_ecef);
-       DOUBLE_QUAT_OF_RMAT(initial_orientation, current_ltp.ltp_of_ecef);
-       ins.avg_state.orientation = 
DOUBLEQUAT_AS_QUATERNIOND(initial_orientation);
-       //              THIS SOMEWHERE ELSE! (END)
        
-       // old transformation:
-       //struct DoubleRMat ned2ecef;
-       //NED_TO_ECEF_MAT(pos_0_lla, ned2ecef.m);
-       //RMAT_VECT3_MUL(ref_dir_ecef, ned2ecef, ref_dir_ned);
-       
        reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef).normalized();
-       //reference_direction = Vector3d(1, 0, 0);
-       std::cout <<"reference direction NED : " << 
VECT3_AS_VECTOR3D(ref_dir_ned).transpose() << std::endl;
-       std::cout <<"reference direction ECEF: " << 
reference_direction.transpose() << std::endl;
 }
 
 
 /*             helpstuff               */
+/** Transformation **/
+Quaterniond ecef2body_from_pprz_ned2body(Vector3d ecef_pos, struct DoubleQuat 
q_ned2body){
+  Quaterniond       ecef2body;
+  struct LtpDef_d   current_ltp;
+       struct EcefCoor_d ecef_pos_pprz;
+  struct DoubleQuat q_ecef2enu,
+                    q_ecef2ned,
+                    q_ecef2body;
+  
+       VECTOR_AS_VECT3(ecef_pos_pprz, ecef_pos);
+  ltp_def_from_ecef_d(&current_ltp, &ecef_pos_pprz);
+  DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
+  QUAT_ENU_FROM_TO_NED(q_ecef2enu, q_ecef2ned);
+  
+  FLOAT_QUAT_COMP(q_ecef2body, q_ecef2ned, q_ned2body);
+  
+  return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body);
+}
+
+
 /** Logging **/
 
+static struct raw_log_entry read_raw_log_entry(int file_descriptor, uint8_t 
*read_ok){
+  struct raw_log_entry e;
+  ssize_t nb_read = read(file_descriptor, &e, sizeof(e));
+  *read_ok =  (nb_read == sizeof(e));
+  
+  COPY_RATES_ACCEL_TO_IMU_FLOAT(e);
+  COPY_MAG_TO_IMU_FLOAT(e);
+  COPY_GPS_TO_IMU(e);
+  return e;
+}
+
+static struct raw_log_entry next_GPS(int file_descriptor){
+  uint8_t read_ok;
+  struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
+  while ((read_ok)&&(!GPS_READY(e.data_valid))) {
+    e = read_raw_log_entry(file_descriptor, &read_ok);
+  }
+  return e;
+}
+
 static void print_estimator_state(double time) {
 
 #if FILTER_OUTPUT_IN_NED

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp     
2010-10-17 00:10:51 UTC (rev 6161)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/libeknav_from_log.hpp     
2010-10-17 18:30:56 UTC (rev 6162)
@@ -5,6 +5,8 @@
 
 #include "ins_qkf.hpp"
 #include "paparazzi_eigen_conversion.h"
+#include "estimate_attitude.h"
+#include "estimate_attitude.c"
 #include <stdint.h>
 
 #include <stdio.h>
@@ -49,32 +51,7 @@
 #define PRINT_EULER_NED 0
 
 /** geodetic **/
-//Toulouse Lat:        43° 35' 24''            Lon:    1° 25' 48''
-#define TOULOUSE_LATTITUDE ARCSEC_ACRMIN_ANGLE_IN_RADIANS(43,35,24)
-#define TOULOUSE_LONGITUDE ARCSEC_ACRMIN_ANGLE_IN_RADIANS(1,25,48)
-#define TOULOUSE_HEIGHT 0
-//Toulouse Declination is  22' West    and     Inclination 59° 8' Down
-#define TOULOUSE_DECLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(0,22,0)
-#define TOULOUSE_INCLINATION -ARCSEC_ACRMIN_ANGLE_IN_RADIANS(59,8,0)
-
-/** magnetic field 
- ** how to compute the magnetic field:
- **     http://gsc.nrcan.gc.ca/geomag/field/comp_e.php
- ** 
- ** online-calculator:
- **   http://geomag.nrcan.gc.ca/apps/mfcal-eng.php
- **/
-#if 0
-#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) { \
-  ref.z = sin(TOULOUSE_INCLINATION);                                           
\
-       double h = sqrt(1-ref.z*ref.z);                                         
                \
-       ref.x = h*cos(TOULOUSE_DECLINATION);                                    
\
-       ref.y = h*sin(TOULOUSE_DECLINATION);                                    
\
-}
-#else
-//#define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 
0.51292422348174, -0.00331095113378, 0.85842750338526)
 #define EARTHS_GEOMAGNETIC_FIELD_NORMED(ref) VECT3_ASSIGN(ref, 
0.51562740288882, -0.05707735220832, 0.85490967783446)
-#endif
 Vector3d reference_direction;
 
 static void set_reference_direction(void);
@@ -85,12 +62,14 @@
 
 /* Initialisation */
 static void main_init(void);
+static struct raw_log_entry first_entry_after_initialisation(int);
 
 /** initial state **/
 struct LlaCoor_f pos_0_lla;
-Vector3d pos_0_ecef;
-Vector3d speed_0_ecef = Vector3d::Zero();
-Vector3d bias_0(0., 0., 0.);
+Vector3d pos_0_ecef       = Vector3d::Zero();
+Vector3d speed_0_ecef     = Vector3d::Zero();
+Vector3d bias_0           = Vector3d::Zero();
+Quaterniond orientation_0 = Quaterniond::Identity();
 static void init_ins_state(void);
 
 /** initial covariance **/
@@ -111,17 +90,34 @@
 
 
 /* libeknav */
+static void main_run_from_file(int, struct raw_log_entry);
 static void main_run_ins(uint8_t);
 
 
 /* Logging */
+static struct raw_log_entry read_raw_log_entry(int, uint8_t *);
+static struct raw_log_entry next_GPS(int);
+
 static void print_estimator_state(double);
 #define INS_LOG_FILE "log_ins_test3.data"
 
 
 
 /* Other */
+/** Average-Calculation **/
+#define MINIMAL_IMU_MEASUREMENTS 100
+#define MINIMAL_MAGNETIC_FIELD_MEASUREMENTS 50
+#define MINIMAL_GPS_MEASUREMENTS 10
 
+#define NOT_ENOUGH_MEASUREMENTS(imu, mag, gps)      
(NOT_ENOUGH_IMU_MEASUREMENTS(imu)            && \
+                                                     
NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(mag) && \
+                                                     
NOT_ENOUGH_GPS_MEASUREMENTS(gps)               )
+#define NOT_ENOUGH_IMU_MEASUREMENTS(imu)            
((imu)<(MINIMAL_IMU_MEASUREMENTS)           )
+#define NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(mag) 
((mag)<(MINIMAL_MAGNETIC_FIELD_MEASUREMENTS))
+#define NOT_ENOUGH_GPS_MEASUREMENTS(gps)            
((gps)<(MINIMAL_GPS_MEASUREMENTS)           )
+
+#define NEW_MEAN(old_mean, new_observation, index) 
(((old_mean)*(index-1)+(new_observation))/(index))
+
        /** Sensors **/
 #define COPY_RATES_ACCEL_TO_IMU_FLOAT(pointer){                                
                \
   RATES_FLOAT_OF_BFP(imu_float.gyro, pointer.gyro);                    \
@@ -141,6 +137,8 @@
 #define CLOSE_TO_GRAVITY(accel) 
(ABS(FLOAT_VECT3_NORM(accel)-GRAVITY)<MAX_DISTANCE_FROM_GRAVITY_FOR_UPDATE)
 
        /** Conversions **/
+Quaterniond ecef2body_from_pprz_ned2body(Vector3d, struct DoubleQuat);
+
        /* copied and modified form pprz_geodetic */
 #define NED_TO_ECEF_MAT(lla, mat) {                    \
        const double sin_lat = sin(lla.lat);    \

Modified: paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h      
2010-10-17 00:10:51 UTC (rev 6161)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/paparazzi_eigen_conversion.h      
2010-10-17 18:30:56 UTC (rev 6162)
@@ -118,6 +118,14 @@
       }                                                                        
\
     }                                                                  \
   }
+  
+#define QUAT_ENU_FROM_TO_NED(from, to){        \
+       to.qi = -from.qx - from.qy;                                             
\
+  to.qi =  from.qi + from.qz;                                          \
+  to.qi =  from.qi - from.qz;                                          \
+  to.qi = -from.qx + from.qy;                                          \
+  QUAT_SMUL(to, to, M_SQRT1_2);                                        \
+}
 
 
 /*
@@ -137,6 +145,7 @@
  */
 
 #define VECTOR_AS_VECT3(coords, vector) { coords.x = vector(0); coords.y = 
vector(1); coords.z = vector(2);}
+#define QUATERNIOND_AS_DOUBLEQUAT(doublequat, quaterniond) {doublequat.qi = 
quaterniond.w(); doublequat.qx = quaterniond.x(); doublequat.qy = 
quaterniond.y(); doublequat.qz = quaterniond.z();}
 #define PPRZ_LLA_TO_EIGEN_ECEF(lla, ecef){     \
        struct EcefCoor_f ecef_pprz;                                            
        \
        ecef_of_lla_f(&ecef_pprz, &lla);                                        
\