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_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_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) {
/* 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 */
}
static void ahrs_do_update_mag(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
printf("Mag update.\n");
#endif
MAGS_FLOAT_OF_BFP(bafl_mag, imu.mag);
/* P_prio = P */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_Pprio[i][j] = bafl_P[i][j];
}
}
/*
* set up measurement matrix
*
* h = [236.; -2.; 396.];
* hx = [ h(2); -h(1); 0]; //only psi (phi and theta = 0)
* Hm = Cnb * hx;
* H = [ 0 0 0 0 0
* 0 0 Cnb*hx 0 0 0
* 0 0 0 0 0 ];
* */
/*bafl_H[0][2] = RMAT_ELMT(bafl_dcm, 0, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 0, 1) * bafl_h.x;
bafl_H[1][2] = RMAT_ELMT(bafl_dcm, 1, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 1, 1) * bafl_h.x;
bafl_H[2][2] = RMAT_ELMT(bafl_dcm, 2, 0) * bafl_h.y - RMAT_ELMT(bafl_dcm, 2, 1) * bafl_h.x;*/
bafl_H[0][2] = -RMAT_ELMT(bafl_dcm, 0, 1);
bafl_H[1][2] = -RMAT_ELMT(bafl_dcm, 1, 1);
bafl_H[2][2] = -RMAT_ELMT(bafl_dcm, 2, 1);
//rest is zero
/**********************************************
* compute Kalman gain K
*
* K = P_prio * H_T * inv(S)
* S = H * P_prio * HT + R
*
* K = P_prio * HT * inv(H * P_prio * HT + R)
*
**********************************************/
/* covariance residual S = H * P_prio * HT + R */
/* temp_S(3x6) = H(3x6) * P_prio(6x6)
*
* only third column of H is non-zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_tempS[i][j] = bafl_H[i][2] * bafl_Pprio[2][j];
}
}
/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
*
* only third row of HT is non-zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
bafl_S[i][j] = bafl_tempS[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
}
bafl_S[i][i] += bafl_R_mag;
}
/* invert S
*/
FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
*
* only third row of HT is non-zero
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < 3; j++) {
/* not computing zero elements */
bafl_tempK[i][j] = bafl_Pprio[i][2] * bafl_H[j][2]; /* H[j][2] = HT[2][j] */
}
}
/* K(6x3) = temp_K(6x3) * invS(3x3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < 3; j++) {
bafl_K[i][j] = bafl_tempK[i][0] * bafl_invS[0][j];
bafl_K[i][j] += bafl_tempK[i][1] * bafl_invS[1][j];
bafl_K[i][j] += bafl_tempK[i][2] * bafl_invS[2][j];
}
}
/**********************************************
* Update filter state.
*
* The a priori filter state is zero since the errors are corrected after each update.
*
* X = X_prio + K (y - H * X_prio)
* X = K * y
**********************************************/
/* innovation
* y = Cnb * [hx; hy; hz] - mag
*/
FLOAT_RMAT_VECT3_MUL(bafl_ym, bafl_dcm, bafl_h); //can be optimized
FLOAT_VECT3_SUB(bafl_ym, bafl_mag);
/* X(6) = K(6x3) * y(3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
bafl_X[i] = bafl_K[i][0] * bafl_ym.x;
bafl_X[i] += bafl_K[i][1] * bafl_ym.y;
bafl_X[i] += bafl_K[i][2] * bafl_ym.z;
}
/**********************************************
* Update the filter covariance.
*
* P = P_prio - K * H * P_prio
* P = ( I - K * H ) * P_prio
*
**********************************************/
/* temp(6x6) = I(6x6) - K(6x3)*H(3x6)
*
* last 3 columns of H are zero
*/
for (i = 0; i < 6; i++) {
for (j = 0; j < 6; j++) {
if (i == j) {
bafl_tempP[i][j] = 1.;
} else {
bafl_tempP[i][j] = 0.;
}
if (j == 2) { /* omit the parts where H is zero */
for (k = 0; k < 3; k++) {
bafl_tempP[i][j] -= bafl_K[i][k] * bafl_H[k][j];
}
}
}
}
/* P(6x6) = temp(6x6) * P_prio(6x6)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_P[i][j] = bafl_tempP[i][0] * bafl_Pprio[0][j];
for (k = 1; k < BAFL_SSIZE; k++) {
bafl_P[i][j] += bafl_tempP[i][k] * bafl_Pprio[k][j];
}
}
}
#ifdef LKF_PRINT_P
printf("Pum=");
for (i = 0; i < 6; i++) {
printf("[");
for (j = 0; j < 6; j++) {
printf("%f\t", bafl_P[i][j]);
}
printf("]\n ");
}
printf("\n");
#endif
/**********************************************
* Correct errors.
*
*
**********************************************/
/* Error quaternion.
*/
QUAT_ASSIGN(bafl_q_m_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
FLOAT_QUAT_INVERT(bafl_q_m_err, bafl_q_m_err);
/* normalize */
float q_sq;
q_sq = bafl_q_m_err.qx * bafl_q_m_err.qx + bafl_q_m_err.qy * bafl_q_m_err.qy + bafl_q_m_err.qz * bafl_q_m_err.qz;
if (q_sq > 1) { /* this should actually never happen */
FLOAT_QUAT_SMUL(bafl_q_m_err, bafl_q_m_err, 1 / sqrtf(1 + q_sq));
printf("mag error quaternion q_sq > 1!!\n");
} else {
bafl_q_m_err.qi = sqrtf(1 - q_sq);
}
/* correct attitude
*/
FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_m_err, bafl_quat);
FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
/* correct gyro bias
*/
RATES_ASSIGN(bafl_b_m_err, bafl_X[3], bafl_X[4], bafl_X[5]);
RATES_SUB(bafl_bias, bafl_b_m_err);
/*
* compute all representations
*/
/* maintain rotation matrix representation */
FLOAT_RMAT_OF_QUAT(bafl_dcm, bafl_quat);
/* maintain euler representation */
FLOAT_EULERS_OF_RMAT(bafl_eulers, bafl_dcm);
AHRS_TO_BFP();
AHRS_LTP_TO_BODY();
}
