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_update_mag_full(void) {
struct FloatVect3 expected_imu;
FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_impl.ltp_to_imu_rmat, ahrs_impl.mag_h);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 residual_imu;
FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
// DISPLAY_FLOAT_VECT3("# expected", expected_imu);
// DISPLAY_FLOAT_VECT3("# measured", measured_imu);
// DISPLAY_FLOAT_VECT3("# residual", residual);
/* Complementary filter proportional gain.
* Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY
*/
const float mag_rate_update_gain = 2 * ahrs_impl.mag_zeta * ahrs_impl.mag_omega *
AHRS_PROPAGATE_FREQUENCY / AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
/* Complementary filter integral gain
* Correct the gyro bias.
* Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY
*/
const float mag_bias_update_gain = -(ahrs_impl.mag_omega * ahrs_impl.mag_omega) /
AHRS_MAG_CORRECT_FREQUENCY;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
void ahrs_update_accel(void) {
struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
struct FloatVect3 residual;
FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, (*r2));
/* heuristic on acceleration norm */
const float acc_norm = FLOAT_VECT3_NORM(accel_float);
const float weight = Chop(1.-2*fabs(1-acc_norm/9.81), 0., 1.);
//const float weight = 1.;
/* compute correction */
const float gravity_rate_update_gain = 5e-2;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
const float gravity_bias_update_gain = -5e-6;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
/* FIXME: saturate bias */
}
void ahrs_update_mag2(void) {
const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 expected_imu;
FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 residual;
FLOAT_VECT3_CROSS_PRODUCT(residual, measured_imu, expected_imu);
// FLOAT_VECT3_DIFF(residual, expected_imu, measured_imu);
DISPLAY_FLOAT_VECT3(" expected", expected_imu);
DISPLAY_FLOAT_VECT3(" measured", measured_imu);
DISPLAY_FLOAT_VECT3("residual", residual);
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, mag_rate_update_gain);
}
void ahrs_update_mag_full(void) {
struct FloatVect3 expected_imu;
FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_impl.ltp_to_imu_rmat, ahrs_impl.mag_h);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 residual_imu;
FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
// DISPLAY_FLOAT_VECT3("# expected", expected_imu);
// DISPLAY_FLOAT_VECT3("# measured", measured_imu);
// DISPLAY_FLOAT_VECT3("# residual", residual);
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
const float mag_bias_update_gain = -2.5e-3;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
void ahrs_update_accel(void) {
struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
float v = FLOAT_VECT3_NORM(ahrs_impl.est_ltp_speed);
accel_float.y -= v * ahrs_float.imu_rate.r;
accel_float.z -= -v * ahrs_float.imu_rate.q;
#endif
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)};
struct FloatVect3 residual;
FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, c2);
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
/* heuristic on acceleration norm */
const float acc_norm = FLOAT_VECT3_NORM(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);
#if 1
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);
#else
const float alpha = 5e-4;
FLOAT_RATES_SCALE(ahrs_impl.gyro_bias, 1.-alpha);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, alpha);
#endif
/* 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 */
}
