void ahrs_update_accel(void) { // c2 = ltp z-axis in imu-frame struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2), RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2), RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)}; struct Int32Vect3 residual; struct Int32Vect3 pseudo_gravity_measurement; if (ahrs_impl.correct_gravity && ahrs_impl.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 */ // FIXME: check overflows ! const struct Int32Vect3 vel_tangential_body = {(ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC), 0.0, 0.0}; struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, ahrs.body_rate, vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; INT32_RMAT_VMULT(acc_c_imu, imu.body_to_imu_rmat, acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ INT32_VECT3_DIFF(pseudo_gravity_measurement, imu.accel, acc_c_imu); } else {
void ahrs_update_accel(void) { /* last column of roation matrix = ltp z-axis in imu-frame */ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2) }; struct FloatVect3 imu_accel_float; ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel); struct FloatVect3 residual; if (ahrs_impl.correct_gravity && ahrs_impl.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_impl.ltp_vel_norm, 0.0, 0.0}; struct FloatVect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body); /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ struct FloatVect3 corrected_gravity; VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu); /* compute the residual of gravity vector in imu frame */ FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2); } else { FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2); } #ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC /* heuristic on acceleration norm */ const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float); const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.); #else const float weight = 1.; #endif /* compute correction */ const float gravity_rate_update_gain = -5e-2; // -5e-2 FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain); const float gravity_bias_update_gain = 1e-5; // -5e-6 FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain); /* FIXME: saturate bias */ }
void ahrs_icq_update_accel(struct Int32Vect3 *accel, float dt) { // check if we had at least one propagation since last update if (ahrs_icq.accel_cnt == 0) { return; } // c2 = ltp z-axis in imu-frame struct Int32RMat ltp_to_imu_rmat; int32_rmat_of_quat(<p_to_imu_rmat, &ahrs_icq.ltp_to_imu_quat); struct Int32Vect3 c2 = { RMAT_ELMT(ltp_to_imu_rmat, 0, 2), RMAT_ELMT(ltp_to_imu_rmat, 1, 2), RMAT_ELMT(ltp_to_imu_rmat, 2, 2) }; struct Int32Vect3 residual; struct Int32Vect3 pseudo_gravity_measurement; if (ahrs_icq.correct_gravity && ahrs_icq.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 */ // FIXME: check overflows ! #define COMPUTATION_FRAC 16 #define ACC_FROM_CROSS_FRAC INT32_RATE_FRAC + INT32_SPEED_FRAC - INT32_ACCEL_FRAC - COMPUTATION_FRAC const struct Int32Vect3 vel_tangential_body = {ahrs_icq.ltp_vel_norm >> COMPUTATION_FRAC, 0, 0}; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&ahrs_icq.body_to_imu); struct Int32Rates body_rate; int32_rmat_transp_ratemult(&body_rate, body_to_imu_rmat, &ahrs_icq.imu_rate); struct Int32Vect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, body_rate, vel_tangential_body); INT32_VECT3_RSHIFT(acc_c_body, acc_c_body, ACC_FROM_CROSS_FRAC); /* convert centrifucal acceleration from body to imu frame */ struct Int32Vect3 acc_c_imu; int32_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, *accel, acc_c_imu); } else {
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 */ }
void ahrs_update_accel(void) { /* last column of roation matrix = ltp z-axis in imu-frame */ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; struct FloatVect3 imu_accel_float; ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel); struct FloatVect3 residual; struct FloatVect3 pseudo_gravity_measurement; if (ahrs_impl.correct_gravity && ahrs_impl.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_impl.ltp_vel_norm, 0.0, 0.0}; struct FloatVect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body); /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.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); } 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_impl.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_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; Bound(ahrs_impl.weight, 0.15, 1.0); } else { ahrs_impl.weight = 1.0; } /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY */ const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81); FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY */ const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega * ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81); FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ }