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 */
}
