void test1234(void)
{
struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)};
struct FloatVect3 uz = { 0., 0., 1.};
struct FloatRMat r_yaw;
FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
struct FloatVect3 uy = { 0., 1., 0.};
struct FloatRMat r_pitch;
FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
struct FloatVect3 ux = { 1., 0., 0.};
struct FloatRMat r_roll;
FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
struct FloatRMat r_tmp;
float_rmat_comp(&r_tmp, &r_yaw, &r_roll);
struct FloatRMat r_att;
float_rmat_comp(&r_att, &r_tmp, &r_pitch);
DISPLAY_FLOAT_RMAT("r_att_ref ", r_att);
float_rmat_of_eulers_312(&r_att, &eul);
DISPLAY_FLOAT_RMAT("r_att312 ", r_att);
}
void test1234(void) {
struct FloatEulers eul = {RadOfDeg(33.), RadOfDeg(25.), RadOfDeg(26.)};
struct FloatVect3 uz = { 0., 0., 1.};
struct FloatMat33 r_yaw;
FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
struct FloatVect3 uy = { 0., 1., 0.};
struct FloatMat33 r_pitch;
FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
struct FloatVect3 ux = { 1., 0., 0.};
struct FloatMat33 r_roll;
FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
struct FloatMat33 r_tmp;
FLOAT_RMAT_COMP(r_tmp, r_yaw, r_roll);
struct FloatMat33 r_att;
FLOAT_RMAT_COMP(r_att, r_tmp, r_pitch);
DISPLAY_FLOAT_RMAT("r_att_ref ", r_att);
FLOAT_RMAT_OF_EULERS_312(r_att, eul);
DISPLAY_FLOAT_RMAT("r_att312 ", r_att);
}
void test_of_axis_angle(void)
{
struct FloatVect3 axis = { 0., 1., 0.};
FLOAT_VECT3_NORMALIZE(axis);
DISPLAY_FLOAT_VECT3("axis", axis);
const float angle = RadOfDeg(30.);
printf("angle %f\n", DegOfRad(angle));
struct FloatQuat my_q;
FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q);
struct FloatRMat my_r1;
float_rmat_of_quat(&my_r1, &my_q);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1);
DISPLAY_FLOAT_RMAT("rmat1", my_r1);
struct FloatRMat my_r;
FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r);
DISPLAY_FLOAT_RMAT("rmat", my_r);
printf("\n");
struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.};
struct FloatVect3 uz = { 0., 0., 1.};
struct FloatRMat r_yaw;
FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi);
struct FloatVect3 uy = { 0., 1., 0.};
struct FloatRMat r_pitch;
FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta);
struct FloatVect3 ux = { 1., 0., 0.};
struct FloatRMat r_roll;
FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi);
struct FloatRMat r_yaw_pitch;
float_rmat_comp(&r_yaw_pitch, &r_yaw, &r_pitch);
struct FloatRMat r_yaw_pitch_roll;
float_rmat_comp(&r_yaw_pitch_roll, &r_yaw_pitch, &r_roll);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll);
DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll);
DISPLAY_FLOAT_EULERS_DEG("eul", eul);
struct FloatRMat rmat1;
float_rmat_of_eulers(&rmat1, &eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1);
DISPLAY_FLOAT_RMAT("rmat1", rmat1);
}
float test_INT32_QUAT_OF_RMAT(struct FloatEulers *eul_f, bool display)
{
struct Int32Eulers eul321_i;
EULERS_BFP_OF_REAL(eul321_i, (*eul_f));
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
if (display) { DISPLAY_INT32_EULERS("eul312_i", eul312_i); }
struct FloatRMat rmat_f;
float_rmat_of_eulers_321(&rmat_f, eul_f);
if (display) { DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f); }
if (display) { DISPLAY_FLOAT_RMAT("rmat float", rmat_f); }
struct Int32RMat rmat_i;
int32_rmat_of_eulers_321(&rmat_i, &eul321_i);
if (display) { DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i); }
if (display) { DISPLAY_INT32_RMAT("rmat int", rmat_i); }
if (display) { DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i); }
struct FloatQuat qf;
float_quat_of_rmat(&qf, &rmat_f);
//FLOAT_QUAT_WRAP_SHORTEST(qf);
if (display) { DISPLAY_FLOAT_QUAT("qf", qf); }
struct Int32Quat qi;
int32_quat_of_rmat(&qi, &rmat_i);
//int32_quat_wrap_shortest(&qi);
if (display) { DISPLAY_INT32_QUAT("qi", qi); }
if (display) { DISPLAY_INT32_QUAT_2("qi", qi); }
struct FloatQuat qif;
QUAT_FLOAT_OF_BFP(qif, qi);
// dot product of two quaternions is 1 if they represent same rotation
float qi_dot_qf = qif.qi * qf.qi + qif.qx * qf.qx + qif.qy * qf.qy + qif.qz * qf.qz;
float err_norm = fabs(fabs(qi_dot_qf) - 1.);
if (display) { printf("err %f\n", err_norm); }
if (display) { printf("\n"); }
return err_norm;
}
float test_quat_of_rmat(void)
{
// struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
// struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};
struct FloatRMat rm;
float_rmat_of_eulers(&rm, &eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);
struct FloatQuat q;
float_quat_of_rmat(&q, &rm);
DISPLAY_FLOAT_QUAT("q_of_rm ", q);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q);
struct FloatQuat qref;
float_quat_of_eulers(&qref, &eul);
DISPLAY_FLOAT_QUAT("q_of_euler", qref);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);
printf("\n\n\n");
struct FloatRMat r_att;
struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
float_rmat_of_eulers_312(&r_att, &e312);
DISPLAY_FLOAT_RMAT("r_att ", r_att);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att);
struct FloatQuat q_att;
float_quat_of_rmat(&q_att, &r_att);
struct FloatEulers e_att;
float_eulers_of_rmat(&e_att, &r_att);
DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);
float_eulers_of_quat(&e_att, &q_att);
DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);
return 0.;
}
float test_INT32_QUAT_OF_RMAT(struct FloatEulers* eul_f, bool_t display) {
struct Int32Eulers eul312_i;
EULERS_BFP_OF_REAL(eul312_i, (*eul_f));
if (display) DISPLAY_INT32_EULERS("eul312_i", eul312_i);
struct FloatRMat rmat_f;
FLOAT_RMAT_OF_EULERS_312(rmat_f, (*eul_f));
if (display) DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat float", rmat_f);
if (display) DISPLAY_FLOAT_RMAT("rmat float", rmat_f);
struct Int32RMat rmat_i;
INT32_RMAT_OF_EULERS_312(rmat_i, eul312_i);
if (display) DISPLAY_INT32_RMAT_AS_EULERS_DEG("rmat int", rmat_i);
if (display) DISPLAY_INT32_RMAT("rmat int", rmat_i);
if (display) DISPLAY_INT32_RMAT_AS_FLOAT("rmat int", rmat_i);
struct FloatQuat qf;
FLOAT_QUAT_OF_RMAT(qf, rmat_f);
FLOAT_QUAT_WRAP_SHORTEST(qf);
if (display) DISPLAY_FLOAT_QUAT("qf", qf);
struct Int32Quat qi;
INT32_QUAT_OF_RMAT(qi, rmat_i);
INT32_QUAT_WRAP_SHORTEST(qi);
if (display) DISPLAY_INT32_QUAT("qi", qi);
if (display) DISPLAY_INT32_QUAT_2("qi", qi);
struct FloatQuat qif;
QUAT_FLOAT_OF_BFP(qif, qi);
struct FloatQuat qerr;
QUAT_DIFF(qerr, qif, qf);
float err_norm = FLOAT_QUAT_NORM(qerr);
if (display) printf("err %f\n", err_norm);
if (display) printf("\n");
return err_norm;
}
float test_quat_of_rmat(void) {
// struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
// struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};
struct FloatMat33 rm;
FLOAT_RMAT_OF_EULERS(rm, eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);
struct FloatQuat q;
FLOAT_QUAT_OF_RMAT(q, rm);
DISPLAY_FLOAT_QUAT("q_of_rm ", q);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q);
struct FloatQuat qref;
FLOAT_QUAT_OF_EULERS(qref, eul);
DISPLAY_FLOAT_QUAT("q_of_euler", qref);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);
printf("\n\n\n");
struct FloatMat33 r_att;
struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
FLOAT_RMAT_OF_EULERS_312(r_att, e312);
DISPLAY_FLOAT_RMAT("r_att ", r_att);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att);
struct FloatQuat q_att;
FLOAT_QUAT_OF_RMAT(q_att, r_att);
struct FloatEulers e_att;
FLOAT_EULERS_OF_RMAT(e_att, r_att);
DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);
FLOAT_EULERS_OF_QUAT(e_att, q_att);
DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);
return 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;
}
