/** 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_NORM_SHORTEST(_a2c,          _a2b,        _b2c)
// a = ecef b = ned c = body
  FLOAT_QUAT_COMP_INV_NORM_SHORTEST(q_ecef2body, q_ecef2ned, q_ned2body);
  
  #ifdef EKNAV_FROM_LOG_DEBUG
  printf("Right after initialization:\n");
  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 /* EKNAV_FROM_LOG_DEBUG */
  
  return DOUBLEQUAT_AS_QUATERNIOND(q_ecef2body);
}
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 print_estimator_state(double time) {

#if FILTER_OUTPUT_IN_NED
	
	struct EcefCoor_d pos_ecef,
										cur_pos_ecef,
										cur_vel_ecef;
	struct NedCoor_d	pos_ned,
										vel_ned;
										
	struct DoubleQuat q_ecef2body,
										q_ecef2enu,
										q_enu2body,
										q_ned2enu,
										q_ned2body;
										
	VECTOR_AS_VECT3(pos_ecef,pos_0_ecef);
	VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position);
	VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity);
	
	ned_of_ecef_point_d(&pos_ned, &current_ltp, &cur_pos_ecef);
	ned_of_ecef_vect_d(&vel_ned, &current_ltp, &cur_vel_ecef);
	
  int32_t xdd = 0;
  int32_t ydd = 0;
  int32_t zdd = 0;
  
  int32_t xd = vel_ned.x/0.0000019073;
  int32_t yd = vel_ned.y/0.0000019073;
  int32_t zd = vel_ned.z/0.0000019073;
  
  int32_t x = pos_ned.x/0.0039;
  int32_t y = pos_ned.y/0.0039;
  int32_t z = pos_ned.z/0.0039;

  fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
  #if 0
  QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(),
	         -ins.avg_state.orientation.y(), -ins.avg_state.orientation.z());
  QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0);
  
  FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
	FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body);		// q_enu2body = q_ecef2body * (q_ecef2enu)^*
  FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body);					// q_ned2body = q_enu2body * q_ned2enu

  #else /* if 0 */
  QUATERNIOND_AS_DOUBLEQUAT(q_ecef2body, ins.avg_state.orientation);
  DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef);
  FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body);
  QUAT_ENU_FROM_TO_NED(q_enu2body, q_ned2body);
  
  #endif /* if 0 */
  
  struct FloatEulers e;
  FLOAT_EULERS_OF_QUAT(e, q_ned2body);
  
  
	#if PRINT_EULER_NED
		printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, e.theta*180*M_1_PI, e.psi*180*M_1_PI);
	#endif /* PRINT_EULER_NED */
  fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi);
  fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
				sqrt(ins.cov( 0, 0)),  sqrt(ins.cov( 1, 1)),  sqrt(ins.cov( 2, 2)), 
				sqrt(ins.cov( 3, 3)),  sqrt(ins.cov( 4, 4)),  sqrt(ins.cov( 5, 5)), 
				sqrt(ins.cov( 6, 6)),  sqrt(ins.cov( 7, 7)),  sqrt(ins.cov( 8, 8)), 
				sqrt(ins.cov( 9, 9)),  sqrt(ins.cov(10,10)),  sqrt(ins.cov(11,11)));
  fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));

#else /* FILTER_OUTPUT_IN_ECEF */
  int32_t xdd = 0;
  int32_t ydd = 0;
  int32_t zdd = 0;

  int32_t xd = ins.avg_state.velocity(0)/0.0000019073;
  int32_t yd = ins.avg_state.velocity(1)/0.0000019073;
  int32_t zd = ins.avg_state.velocity(2)/0.0000019073;
  int32_t x = ins.avg_state.position(0)/0.0039;
  int32_t y = ins.avg_state.position(1)/0.0039;
  int32_t z = ins.avg_state.position(2)/0.0039;

  fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z);
  
  struct FloatQuat q_ecef2body;
  QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(),
	         ins.avg_state.orientation.y(), ins.avg_state.orientation.z());
  struct FloatEulers e_ecef2body;
  FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body);

  fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e_ecef2body.phi, e_ecef2body.theta, e_ecef2body.psi);
  fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID,
				sqrt(ins.cov( 0, 0)),  sqrt(ins.cov( 1, 1)),  sqrt(ins.cov( 2, 2)), 
				sqrt(ins.cov( 3, 3)),  sqrt(ins.cov( 4, 4)),  sqrt(ins.cov( 5, 5)), 
				sqrt(ins.cov( 6, 6)),  sqrt(ins.cov( 7, 7)),  sqrt(ins.cov( 8, 8)), 
				sqrt(ins.cov( 9, 9)),  sqrt(ins.cov(10,10)),  sqrt(ins.cov(11,11)));
  fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2));
#endif /* FILTER_OUTPUT_IN_NED / ECEF */
}