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)));
}
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 */
}
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, ¤t_ltp, &cur_pos_ecef);
ned_of_ecef_vect_d(&vel_ned, ¤t_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 */
}