static void set_reference_direction(void){
	struct NedCoor_d	ref_dir_ned;
	struct EcefCoor_d pos_0_ecef_pprz,
										ref_dir_ecef;
	EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
	
	struct LtpDef_d current_ltp;
	VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef);
	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;
}
static void main_run_ins(uint8_t data_valid) {

  struct timespec now;
  clock_gettime(TIMER, &now);
  
  double dt_imu_freq = 0.001953125; //  1/512; // doesn't work?
  ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq);
  
  if(MAG_READY(data_valid)){
		ins.obs_vector(reference_direction, VECT3_AS_VECTOR3D(imu_float.mag), mag_noise);
	}
  
  #if UPDATE_WITH_GRAVITY
  if(CLOSE_TO_GRAVITY(imu_float.accel)){
		// use the gravity as reference
		ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), 1.0392e-3);
	}
  #endif
  
  if(GPS_READY(data_valid)){
		ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, gps_pos_noise, gps_speed_noise);
	}
  
  print_estimator_state(absTime(time_diff(now, start)));
  
}
Esempio n. 3
0
static void set_reference_direction(void){
	struct NedCoor_d	ref_dir_ned;
	struct EcefCoor_d pos_0_ecef_pprz,
										ref_dir_ecef;
	EARTHS_GEOMAGNETIC_FIELD_NORMED(ref_dir_ned);
	
	VECTOR_AS_VECT3(pos_0_ecef_pprz, pos_0_ecef);
	ltp_def_from_ecef_d(&current_ltp, &pos_0_ecef_pprz);
	ecef_of_ned_vect_d(&ref_dir_ecef, &current_ltp, &ref_dir_ned);
	
	reference_direction = VECT3_AS_VECTOR3D(ref_dir_ecef).normalized();
}
Esempio n. 4
0
static void main_run_ins(uint8_t data_valid) {
  
  double dt_imu_freq = 0.001953125; //  1/512; // doesn't work?
  ins.predict(RATES_AS_VECTOR3D(imu_float.gyro), VECT3_AS_VECTOR3D(imu_float.accel), dt_imu_freq);
  
  if(MAG_READY(data_valid)){
		ins.obs_vector(reference_direction, VECT3_AS_VECTOR3D(imu_float.mag), magnetometer_noise.norm());
	}
  
  #if UPDATE_WITH_GRAVITY
  if(CLOSE_TO_GRAVITY(imu_float.accel)){
		// use the gravity as reference
		ins.obs_vector(ins.avg_state.position.normalized(), VECT3_AS_VECTOR3D(imu_float.accel), accelerometer_noise.norm());
	}
  #endif /* UPDATE_WITH_GRAVITY */
  
   if(BARO_READY(data_valid)){
    ins.obs_baro_report(baro_0_height+imu_baro_height, baro_noise);
  }   // comment out multiple lines */
  
  if(GPS_READY(data_valid)){
		ins.obs_gps_pv_report(VECT3_AS_VECTOR3D(imu_ecef_pos)/100, VECT3_AS_VECTOR3D(imu_ecef_vel)/100, 10*gps_pos_noise, 10*gps_speed_noise);
	}   // comment out multiple lines */
}
Esempio n. 5
0
static struct raw_log_entry first_entry_after_initialisation(int file_descriptor){
  int        imu_measurements = 0,      // => Gyro + Accel
    magnetometer_measurements = 0,
            baro_measurements = 0,
             gps_measurements = 0;      // only the position
  
  struct DoubleMat33 attitude_profile_matrix, sigmaB;   // the attitude profile matrix is often called "B"
  struct Orientation_Measurement  gravity,
                                  magneto,
                                  fake;  
  struct DoubleQuat q_ned2body, sigma_q;
  
  /* Prepare the attitude profile matrix */
  FLOAT_MAT33_ZERO(attitude_profile_matrix);
  FLOAT_MAT33_ZERO(sigmaB);
  
  // for faster converging, but probably more rounding error
  #define MEASUREMENT_WEIGHT_SCALE 10
  
  /* set the gravity measurement */
  VECT3_ASSIGN(gravity.reference_direction, 0,0,-1);
  gravity.weight_of_the_measurement = MEASUREMENT_WEIGHT_SCALE/(double)(imu_frequency);    // originally 1/(imu_frequency*gravity.norm()
  //gravity.weight_of_the_measurement = 1;
  
  /* set the magneto - measurement */
  EARTHS_GEOMAGNETIC_FIELD_NORMED(magneto.reference_direction);
  magneto.weight_of_the_measurement = MEASUREMENT_WEIGHT_SCALE/(double)(mag_frequency);    // originally 1/(mag_frequency*reference_direction.norm()
  //magneto.weight_of_the_measurement = 1;
  
  uint8_t read_ok;
  #if WITH_GPS
  struct raw_log_entry e = next_GPS(file_descriptor);
  #else /* WITH_GPS */
  struct raw_log_entry e = read_raw_log_entry(file_descriptor, &read_ok);
  pos_0_ecef = Vector3d(4627578.56, 119659.25, 4373248.00);
  pos_cov_0 = Vector3d::Ones()*100;
  speed_0_ecef    = Vector3d::Zero();
  speed_cov_0 = Vector3d::Ones();
  #endif /* WITH_GPS */
  
  #ifdef EKNAV_FROM_LOG_DEBUG
    int imu_ready = 0, 
        mag_ready = 0,
        baro_ready = 0,
        gps_ready = 0;
  #endif /* EKNAV_FROM_LOG_DEBUG */
  
  for(read_ok = 1; (read_ok) && NOT_ENOUGH_MEASUREMENTS(imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements); e = read_raw_log_entry(file_descriptor, &read_ok)){
    if(IMU_READY(e.message.valid_sensors)){
      imu_measurements++;
      
      // update the estimated bias
      bias_0 = NEW_MEAN(bias_0, RATES_BFP_AS_VECTOR3D(e.message.gyro), imu_measurements);
      
      // update the attitude profile matrix
      ACCELS_FLOAT_OF_BFP(gravity.measured_direction,e.message.accel);
      add_orientation_measurement(&attitude_profile_matrix, gravity);
    }
    if(MAG_READY(e.message.valid_sensors)){
      magnetometer_measurements++;
      // update the attitude profile matrix
      MAGS_FLOAT_OF_BFP(magneto.measured_direction,e.message.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(BARO_READY(e.message.valid_sensors)){
      baro_measurements++;
      // 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;
    }
    if(GPS_READY(e.message.valid_sensors)){
      gps_measurements++;
      // update the estimated bias
      pos_0_ecef = NEW_MEAN(pos_0_ecef, VECT3_AS_VECTOR3D(e.message.ecef_pos)/100, gps_measurements);
      speed_0_ecef = NEW_MEAN(speed_0_ecef, VECT3_AS_VECTOR3D(e.message.ecef_vel)/100, gps_measurements);
    }
    
    #ifdef EKNAV_FROM_LOG_DEBUG
    if(imu_ready==0){
      if(!NOT_ENOUGH_IMU_MEASUREMENTS(imu_measurements)){
        printf("IMU READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
        imu_ready = 1;
      }
    }
    if(mag_ready==0){
      if(!NOT_ENOUGH_MAGNETIC_FIELD_MEASUREMENTS(magnetometer_measurements)){
        printf("MAG READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
        mag_ready = 1;
      }
    }
    if(baro_ready==0){
      if(!NOT_ENOUGH_BARO_MEASUREMENTS(baro_measurements)){
        printf("BARO READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
        baro_ready = 1;
      }
    }
    if(gps_ready==0){
      if(!NOT_ENOUGH_GPS_MEASUREMENTS(gps_measurements)){
        printf("GPS READY %3i %3i %3i %3i\n", imu_measurements, magnetometer_measurements, baro_measurements, gps_measurements);
        gps_ready = 1;
      }
    }
    #endif /* EKNAV_FROM_LOG_DEBUG */
  }
  // setting the covariance
  gravity.weight_of_the_measurement *= imu_measurements;
  VECTOR_AS_VECT3(gravity.measured_direction, accelerometer_noise);
  magneto.weight_of_the_measurement *= magnetometer_measurements;
  VECTOR_AS_VECT3(magneto.measured_direction, magnetometer_noise);
  add_set_of_three_measurements(&sigmaB, gravity, magneto);
  
  #ifdef EKNAV_FROM_LOG_DEBUG
  DISPLAY_FLOAT_RMAT("     B", attitude_profile_matrix);
  DISPLAY_FLOAT_RMAT("sigmaB", sigmaB);
  #endif /* EKNAV_FROM_LOG_DEBUG */
  
  //  setting the initial orientation
  q_ned2body = estimated_attitude(attitude_profile_matrix, 1000, 1e-6, sigmaB, &sigma_q);
	orientation_0 = ecef2body_from_pprz_ned2body(pos_0_ecef,q_ned2body);
  
  baro_0_height += pos_0_ecef.norm();
  
  struct DoubleEulers sigma_eu = sigma_euler_from_sigma_q(q_ned2body, sigma_q);
  orientation_cov_0 = EULER_AS_VECTOR3D(sigma_eu);
  #if WITH_GPS
  pos_cov_0 = 10*gps_pos_noise / gps_measurements;
  speed_cov_0 = 10*gps_speed_noise / gps_measurements;
  #endif  // WITH_GPS
  
  return e;
}