void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt) { // realign all the filter if needed // a complete init cycle is required if (ahrs_float_inv.reset) { ahrs_float_inv.reset = false; ahrs_float_inv.is_aligned = false; init_invariant_state(); } // fill command vector struct FloatRates gyro_meas_body; struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu); float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro); ahrs_float_inv.cmd.rates = gyro_meas_body; // update correction gains error_output(&ahrs_float_inv); // propagate model struct inv_state new_state; runge_kutta_4_float((float *)&new_state, (float *)&ahrs_float_inv.state, INV_STATE_DIM, (float *)&ahrs_float_inv.cmd, INV_COMMAND_DIM, invariant_model, dt); ahrs_float_inv.state = new_state; // normalize quaternion float_quat_normalize(&ahrs_float_inv.state.quat); //------------------------------------------------------------// #if SEND_INVARIANT_FILTER struct FloatEulers eulers; float foo = 0.f; float_eulers_of_quat(&eulers, &ahrs_float_inv.state.quat); RunOnceEvery(3, pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device, AC_ID, &ahrs_float_inv.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &foo, &foo, &foo, &foo, &foo, &foo, &ahrs_float_inv.state.bias.p, &ahrs_float_inv.state.bias.q, &ahrs_float_inv.state.bias.r, &ahrs_float_inv.state.as, &ahrs_float_inv.state.cs, &foo, &foo); );
/** * Compute body orientation and rates from imu orientation and rates */ static inline void compute_body_orientation_and_rates(void) { /* Compute LTP to BODY quaternion */ struct FloatQuat ltp_to_body_quat; struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu); float_quat_comp_inv(<p_to_body_quat, &ahrs_impl.ltp_to_imu_quat, body_to_imu_quat); /* Set state */ stateSetNedToBodyQuat_f(<p_to_body_quat); /* compute body rates */ struct FloatRates body_rate; struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu); float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_impl.imu_rate); stateSetBodyRates_f(&body_rate); }
void ahrs_fc_update_accel(struct Int32Vect3 *accel, float dt) { // check if we had at least one propagation since last update if (ahrs_fc.accel_cnt == 0) { return; } /* last column of roation matrix = ltp z-axis in imu-frame */ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 0, 2), RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 2), RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2) }; struct FloatVect3 imu_accel_float; ACCELS_FLOAT_OF_BFP(imu_accel_float, *accel); struct FloatVect3 residual; struct FloatVect3 pseudo_gravity_measurement; if (ahrs_fc.correct_gravity && ahrs_fc.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) * (omega x r) = tangential velocity in body frame * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis */ const struct FloatVect3 vel_tangential_body = {ahrs_fc.ltp_vel_norm, 0.0, 0.0}; struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_fc.body_to_imu); struct FloatRates body_rate; float_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_fc.imu_rate); struct FloatVect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body); /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; float_rmat_vmult(&acc_c_imu, body_to_imu_rmat, &acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu); } else { VECT3_COPY(pseudo_gravity_measurement, imu_accel_float); } VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); /* FIR filtered pseudo_gravity_measurement */ #define FIR_FILTER_SIZE 8 static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.}; VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE - 1); VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); if (ahrs_fc.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. * e.g. for gravity_heuristic_factor 30: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ const float g_meas_norm = float_vect3_norm(&filtered_gravity_measurement) / 9.81; ahrs_fc.weight = 1.0 - ahrs_fc.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; Bound(ahrs_fc.weight, 0.15, 1.0); } else { ahrs_fc.weight = 1.0; } /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * ahrs_fc.accel_cnt * with ahrs_fc.accel_cnt beeing the number of propagations since last update */ const float gravity_rate_update_gain = -2 * ahrs_fc.accel_zeta * ahrs_fc.accel_omega * ahrs_fc.weight * ahrs_fc.accel_cnt / 9.81; RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual, gravity_rate_update_gain); // reset accel propagation counter ahrs_fc.accel_cnt = 0; /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2 * dt */ const float gravity_bias_update_gain = ahrs_fc.accel_omega * ahrs_fc.accel_omega * ahrs_fc.weight * ahrs_fc.weight * dt / 9.81; RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ }