void ahrs_update_mag_2d(void) { const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y}; struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); struct FloatVect3 measured_ltp; FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu); const struct FloatVect3 residual_ltp = { 0, 0, measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x }; // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp); 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_mag_2d_dumb(void) { /* project mag on local tangeant plane */ struct FloatEulers ltp_to_imu_euler; FLOAT_EULERS_OF_RMAT(ltp_to_imu_euler, ahrs_impl.ltp_to_imu_rmat); struct FloatVect3 magf; MAGS_FLOAT_OF_BFP(magf, imu.mag); const float cphi = cosf(ltp_to_imu_euler.phi); const float sphi = sinf(ltp_to_imu_euler.phi); const float ctheta = cosf(ltp_to_imu_euler.theta); const float stheta = sinf(ltp_to_imu_euler.theta); const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; const float res_norm = -RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,0) * me + RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,0) * mn; const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,0), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,1), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, r2, (mag_rate_update_gain*res_norm)); const float mag_bias_update_gain = -2.5e-4; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, r2, (mag_bias_update_gain*res_norm)); }
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) { /* 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_2d(void) { struct FloatVect2 expected_ltp; VECT2_COPY(expected_ltp, ahrs_impl.mag_h); // normalize expected ltp in 2D (x,y) FLOAT_VECT2_NORMALIZE(expected_ltp); struct FloatVect3 measured_imu; MAGS_FLOAT_OF_BFP(measured_imu, imu.mag); struct FloatVect3 measured_ltp; FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_impl.ltp_to_imu_rmat, measured_imu); struct FloatVect2 measured_ltp_2d={measured_ltp.x, measured_ltp.y}; // normalize measured ltp in 2D (x,y) FLOAT_VECT2_NORMALIZE(measured_ltp_2d); const struct FloatVect3 residual_ltp = { 0, 0, measured_ltp_2d.x * expected_ltp.y - measured_ltp_2d.y * expected_ltp.x }; // printf("res : %f\n", residual_ltp.z); struct FloatVect3 residual_imu; FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp); /* 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_mag_full_2d_dumb(void) { /* project mag on local tangeant plane */ struct FloatVect3 magf; MAGS_FLOAT_OF_BFP(magf, imu.mag); const float cphi = cosf(ahrs_float.ltp_to_imu_euler.phi); const float sphi = sinf(ahrs_float.ltp_to_imu_euler.phi); const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta); const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta); const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn; struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0)); const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm)); const float mag_bias_update_gain = -2.5e-4; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm)); }
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_heading(float heading) { FLOAT_ANGLE_NORMALIZE(heading); struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f(); // row 0 of ltp_to_body_rmat = body x-axis in ltp frame // we only consider x and y struct FloatVect2 expected_ltp = { RMAT_ELMT(*ltp_to_body_rmat, 0, 0), RMAT_ELMT(*ltp_to_body_rmat, 0, 1) }; // expected_heading cross measured_heading struct FloatVect3 residual_ltp = { 0, 0, expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading) }; struct FloatVect3 residual_imu; FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp); const float heading_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain); float heading_bias_update_gain; /* crude attempt to only update bias if deviation is small * e.g. needed when you only have gps providing heading * and the inital heading is totally different from * the gps course information you get once you have a gps fix. * Otherwise the bias will be falsely "corrected". */ if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) heading_bias_update_gain = -2.5e-4; else heading_bias_update_gain = 0.0; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_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_mag(void) { /* project mag on local tangeant plane */ struct FloatVect3 magf; MAGS_FLOAT_OF_BFP(magf, imu.mag); const float cphi = cosf(ahrs_float.ltp_to_imu_euler.phi); const float sphi = sinf(ahrs_float.ltp_to_imu_euler.phi); const float ctheta = cosf(ahrs_float.ltp_to_imu_euler.theta); const float stheta = sinf(ahrs_float.ltp_to_imu_euler.theta); const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z; const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z; const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn; printf("res norm %f\n", res_norm); struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0)); DISPLAY_FLOAT_VECT3("r2", (*r2)); const float mag_rate_update_gain = 2.5; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm)); DISPLAY_FLOAT_RATES("corr", ahrs_impl.rate_correction); const float mag_bias_update_gain = -2.5e-4; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm)); /* 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_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 */ }