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_fc_update_mag_2d_dumb(struct Int32Vect3 *mag)
{
/* project mag on local tangeant plane */
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_rmat(<p_to_imu_euler, &ahrs_fc.ltp_to_imu_rmat);
struct FloatVect3 magf;
MAGS_FLOAT_OF_BFP(magf, *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_fc.ltp_to_imu_rmat, 0, 0) * me +
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 1, 0) * mn;
const struct FloatVect3 r2 = {RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 0),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 1),
RMAT_ELMT(ahrs_fc.ltp_to_imu_rmat, 2, 2)
};
const float mag_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, r2, (mag_rate_update_gain * res_norm));
const float mag_bias_update_gain = -2.5e-4;
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, r2, (mag_bias_update_gain * res_norm));
}
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_update_accel(void) {
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;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
// FIXME: check overflow ?
const struct Int32Vect3 Xdd_imu = {
0,
((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.r)
>>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC),
-((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.q)
>>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC)
};
struct Int32Vect3 corrected_gravity;
VECT3_DIFF(corrected_gravity, imu.accel, Xdd_imu);
INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
#else
INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2);
#endif
// residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^24 * 5e-2 = 1/81920
ahrs_impl.rate_correction.p += -residual.x/82000;
ahrs_impl.rate_correction.q += -residual.y/82000;
ahrs_impl.rate_correction.r += -residual.z/82000;
// residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// high_rez_bias = RATE_FRAC+28 = 40
// 2^40 / 2^24 * 5e-6 = 1/3.05
// ahrs_impl.high_rez_bias.p += residual.x*3;
// ahrs_impl.high_rez_bias.q += residual.y*3;
// ahrs_impl.high_rez_bias.r += residual.z*3;
ahrs_impl.high_rez_bias.p += residual.x;
ahrs_impl.high_rez_bias.q += residual.y;
ahrs_impl.high_rez_bias.r += residual.z;
/* */
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
{
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
DCM_Matrix[i][j] = RMAT_ELMT(*rmat, j, i);
}
}
}
void ahrs_fc_update_heading(float heading)
{
FLOAT_ANGLE_NORMALIZE(heading);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
struct FloatRMat ltp_to_body_rmat;
float_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat);
// 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_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
const float heading_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.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;
}
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain);
}
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));
}
float float_rmat_reorthogonalize(struct FloatRMat *rm)
{
const struct FloatVect3 r0 = {RMAT_ELMT(*rm, 0, 0),
RMAT_ELMT(*rm, 0, 1),
RMAT_ELMT(*rm, 0, 2)
};
const struct FloatVect3 r1 = {RMAT_ELMT(*rm, 1, 0),
RMAT_ELMT(*rm, 1, 1),
RMAT_ELMT(*rm, 1, 2)
};
float _err = -0.5 * VECT3_DOT_PRODUCT(r0, r1);
struct FloatVect3 r0_t;
VECT3_SUM_SCALED(r0_t, r0, r1, _err);
struct FloatVect3 r1_t;
VECT3_SUM_SCALED(r1_t, r1, r0, _err);
struct FloatVect3 r2_t;
VECT3_CROSS_PRODUCT(r2_t, r0_t, r1_t);
float s = renorm_factor(VECT3_NORM2(r0_t));
MAT33_ROW_VECT3_SMUL(*rm, 0, r0_t, s);
s = renorm_factor(VECT3_NORM2(r1_t));
MAT33_ROW_VECT3_SMUL(*rm, 1, r1_t, s);
s = renorm_factor(VECT3_NORM2(r2_t));
MAT33_ROW_VECT3_SMUL(*rm, 2, r2_t, s);
return _err;
}
void float_rmat_of_eulers_312(struct FloatRMat *rm, struct FloatEulers *e)
{
const float sphi = sinf(e->phi);
const float cphi = cosf(e->phi);
const float stheta = sinf(e->theta);
const float ctheta = cosf(e->theta);
const float spsi = sinf(e->psi);
const float cpsi = cosf(e->psi);
RMAT_ELMT(*rm, 0, 0) = ctheta * cpsi - sphi * stheta * spsi;
RMAT_ELMT(*rm, 0, 1) = ctheta * spsi + sphi * stheta * cpsi;
RMAT_ELMT(*rm, 0, 2) = -cphi * stheta;
RMAT_ELMT(*rm, 1, 0) = -cphi * spsi;
RMAT_ELMT(*rm, 1, 1) = cphi * cpsi;
RMAT_ELMT(*rm, 1, 2) = sphi;
RMAT_ELMT(*rm, 2, 0) = stheta * cpsi + sphi * ctheta * spsi;
RMAT_ELMT(*rm, 2, 1) = stheta * spsi - sphi * ctheta * cpsi;
RMAT_ELMT(*rm, 2, 2) = cphi * ctheta;
}
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 */
}
/* C n->b rotation matrix */
void float_rmat_of_quat(struct FloatRMat *rm, struct FloatQuat *q)
{
const float _a = M_SQRT2 * q->qi;
const float _b = M_SQRT2 * q->qx;
const float _c = M_SQRT2 * q->qy;
const float _d = M_SQRT2 * q->qz;
const float a2_1 = _a * _a - 1;
const float ab = _a * _b;
const float ac = _a * _c;
const float ad = _a * _d;
const float bc = _b * _c;
const float bd = _b * _d;
const float cd = _c * _d;
RMAT_ELMT(*rm, 0, 0) = a2_1 + _b * _b;
RMAT_ELMT(*rm, 0, 1) = bc + ad;
RMAT_ELMT(*rm, 0, 2) = bd - ac;
RMAT_ELMT(*rm, 1, 0) = bc - ad;
RMAT_ELMT(*rm, 1, 1) = a2_1 + _c * _c;
RMAT_ELMT(*rm, 1, 2) = cd + ab;
RMAT_ELMT(*rm, 2, 0) = bd + ac;
RMAT_ELMT(*rm, 2, 1) = cd - ab;
RMAT_ELMT(*rm, 2, 2) = a2_1 + _d * _d;
}
void guidance_indi_calcG(struct FloatMat33 *Gmat) {
struct FloatEulers *euler = stateGetNedToBodyEulers_f();
float sphi = sinf(euler->phi);
float cphi = cosf(euler->phi);
float stheta = sinf(euler->theta);
float ctheta = cosf(euler->theta);
float spsi = sinf(euler->psi);
float cpsi = cosf(euler->psi);
float T = -9.81; //minus gravity is a good guesstimate of the thrust force
// float T = (filt_accelzn-9.81)/(cphi*ctheta); //calculate specific force in body z axis by using the accelerometer
// float T = filt_accelzbody; //if body acceleration is available, use that!
RMAT_ELMT(*Gmat, 0, 0) = (cphi*spsi - sphi*cpsi*stheta)*T;
RMAT_ELMT(*Gmat, 1, 0) = (-sphi*spsi*stheta - cpsi*cphi)*T;
RMAT_ELMT(*Gmat, 2, 0) = -ctheta*sphi*T;
RMAT_ELMT(*Gmat, 0, 1) = (cphi*cpsi*ctheta)*T;
RMAT_ELMT(*Gmat, 1, 1) = (cphi*spsi*ctheta)*T;
RMAT_ELMT(*Gmat, 2, 1) = -stheta*cphi*T;
RMAT_ELMT(*Gmat, 0, 2) = sphi*spsi + cphi*cpsi*stheta;
RMAT_ELMT(*Gmat, 1, 2) = cphi*spsi*stheta - cpsi*sphi;
RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
}
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 */
}
/** initialises a rotation matrix from unit vector axis and angle */
void float_rmat_of_axis_angle(struct FloatRMat *rm, struct FloatVect3 *uv, float angle)
{
const float ux2 = uv->x * uv->x;
const float uy2 = uv->y * uv->y;
const float uz2 = uv->z * uv->z;
const float uxuy = uv->x * uv->y;
const float uyuz = uv->y * uv->z;
const float uxuz = uv->x * uv->z;
const float can = cosf(angle);
const float san = sinf(angle);
const float one_m_can = (1. - can);
RMAT_ELMT(*rm, 0, 0) = ux2 + (1. - ux2) * can;
RMAT_ELMT(*rm, 0, 1) = uxuy * one_m_can + uv->z * san;
RMAT_ELMT(*rm, 0, 2) = uxuz * one_m_can - uv->y * san;
RMAT_ELMT(*rm, 1, 0) = RMAT_ELMT(*rm, 0, 1);
RMAT_ELMT(*rm, 1, 1) = uy2 + (1. - uy2) * can;
RMAT_ELMT(*rm, 1, 2) = uyuz * one_m_can + uv->x * san;
RMAT_ELMT(*rm, 2, 0) = RMAT_ELMT(*rm, 0, 2);
RMAT_ELMT(*rm, 2, 1) = RMAT_ELMT(*rm, 1, 2);
RMAT_ELMT(*rm, 2, 2) = uz2 + (1. - uz2) * can;
}
/*
* Propagate our dynamic system in time
*
* Run the strapdown computation and the predict step of the filter
*
*
* quat_dot = Wxq(pqr) * quat
* bias_dot = 0
*
* Wxq is the quaternion omega matrix:
*
* [ 0, -p, -q, -r ]
* 1/2 * [ p, 0, r, -q ]
* [ q, -r, 0, p ]
* [ r, q, -p, 0 ]
*
*/
void ahrs_propagate(void) {
int i, j, k;
ahrs_lowpass_accel();
/* compute unbiased rates */
RATES_FLOAT_OF_BFP(bafl_rates, imu.gyro);
RATES_SUB(bafl_rates, bafl_bias);
/* run strapdown computation
*
*/
/* multiplicative quaternion update */
/* compute correction quaternion */
QUAT_ASSIGN(bafl_qr, 1., bafl_rates.p * BAFL_DT / 2, bafl_rates.q * BAFL_DT / 2, bafl_rates.r * BAFL_DT / 2);
/* normalize it. maybe not necessary?? */
float q_sq;
q_sq = (bafl_qr.qx * bafl_qr.qx +bafl_qr.qy * bafl_qr.qy + bafl_qr.qz * bafl_qr.qz) / 4;
if (q_sq > 1) { /* this should actually never happen */
FLOAT_QUAT_SMUL(bafl_qr, bafl_qr, 1 / sqrtf(1 + q_sq));
} else {
bafl_qr.qi = sqrtf(1 - q_sq);
}
/* propagate correction quaternion */
FLOAT_QUAT_COMP(bafl_qtemp, bafl_quat, bafl_qr);
FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
bafl_qnorm = FLOAT_QUAT_NORM(bafl_quat);
//TODO check if broot force normalization is good, use lagrange normalization?
FLOAT_QUAT_NORMALISE(bafl_quat);
/*
* 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();
/* error KF-Filter
* propagate only the error covariance matrix since error is corrected after each measurement
*
* F = [ 0 0 0 ]
* [ 0 0 0 -Cbn ]
* [ 0 0 0 ]
* [ 0 0 0 0 0 0 ]
* [ 0 0 0 0 0 0 ]
* [ 0 0 0 0 0 0 ]
*
* T = e^(dt * F)
*
* T = [ 1 0 0 ]
* [ 0 1 0 -Cbn*dt ]
* [ 0 0 1 ]
* [ 0 0 0 1 0 0 ]
* [ 0 0 0 0 1 0 ]
* [ 0 0 0 0 0 1 ]
*
* P_prio = T * P * T_T + Q
*
*/
/*
* compute state transition matrix T
*
* diagonal elements of T are always one
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
bafl_T[i][j + 3] = -RMAT_ELMT(bafl_dcm, j, i); /* inverted bafl_dcm */
}
}
/*
* estimate the a priori error covariance matrix P_k = T * P_k-1 * T_T + Q
*/
/* Temporary multiplication temp(6x6) = T(6x6) * P(6x6) */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_tempP[i][j] = 0.;
for (k = 0; k < BAFL_SSIZE; k++) {
bafl_tempP[i][j] += bafl_T[i][k] * bafl_P[k][j];
}
}
}
/* P_k(6x6) = temp(6x6) * T_T(6x6) + Q */
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_P[i][j] = 0.;
for (k = 0; k < BAFL_SSIZE; k++) {
bafl_P[i][j] += bafl_tempP[i][k] * bafl_T[j][k]; //T[j][k] = T_T[k][j]
}
}
if (i<3) {
bafl_P[i][i] += bafl_Q_att;
} else {
bafl_P[i][i] += bafl_Q_gyro;
}
}
#ifdef LKF_PRINT_P
printf("Pp =");
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
}
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 float_rmat_inv(struct FloatRMat *m_b2a, struct FloatRMat *m_a2b)
{
RMAT_ELMT(*m_b2a, 0, 0) = RMAT_ELMT(*m_a2b, 0, 0);
RMAT_ELMT(*m_b2a, 0, 1) = RMAT_ELMT(*m_a2b, 1, 0);
RMAT_ELMT(*m_b2a, 0, 2) = RMAT_ELMT(*m_a2b, 2, 0);
RMAT_ELMT(*m_b2a, 1, 0) = RMAT_ELMT(*m_a2b, 0, 1);
RMAT_ELMT(*m_b2a, 1, 1) = RMAT_ELMT(*m_a2b, 1, 1);
RMAT_ELMT(*m_b2a, 1, 2) = RMAT_ELMT(*m_a2b, 2, 1);
RMAT_ELMT(*m_b2a, 2, 0) = RMAT_ELMT(*m_a2b, 0, 2);
RMAT_ELMT(*m_b2a, 2, 1) = RMAT_ELMT(*m_a2b, 1, 2);
RMAT_ELMT(*m_b2a, 2, 2) = RMAT_ELMT(*m_a2b, 2, 2);
}
void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
{
const float tr = RMAT_TRACE(*rm);
if (tr > 0) {
const float two_qi = sqrtf(1. + tr);
const float four_qi = 2. * two_qi;
q->qi = 0.5 * two_qi;
q->qx = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qi;
q->qy = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qi;
q->qz = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qi;
/*printf("tr > 0\n");*/
} else {
if (RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 1, 1) &&
RMAT_ELMT(*rm, 0, 0) > RMAT_ELMT(*rm, 2, 2)) {
const float two_qx = sqrtf(RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1)
- RMAT_ELMT(*rm, 2, 2) + 1);
const float four_qx = 2. * two_qx;
q->qi = (RMAT_ELMT(*rm, 1, 2) - RMAT_ELMT(*rm, 2, 1)) / four_qx;
q->qx = 0.5 * two_qx;
q->qy = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qx;
q->qz = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qx;
/*printf("m00 largest\n");*/
} else if (RMAT_ELMT(*rm, 1, 1) > RMAT_ELMT(*rm, 2, 2)) {
const float two_qy =
sqrtf(RMAT_ELMT(*rm, 1, 1) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 2, 2) + 1);
const float four_qy = 2. * two_qy;
q->qi = (RMAT_ELMT(*rm, 2, 0) - RMAT_ELMT(*rm, 0, 2)) / four_qy;
q->qx = (RMAT_ELMT(*rm, 0, 1) + RMAT_ELMT(*rm, 1, 0)) / four_qy;
q->qy = 0.5 * two_qy;
q->qz = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qy;
/*printf("m11 largest\n");*/
} else {
const float two_qz =
sqrtf(RMAT_ELMT(*rm, 2, 2) - RMAT_ELMT(*rm, 0, 0) - RMAT_ELMT(*rm, 1, 1) + 1);
const float four_qz = 2. * two_qz;
q->qi = (RMAT_ELMT(*rm, 0, 1) - RMAT_ELMT(*rm, 1, 0)) / four_qz;
q->qx = (RMAT_ELMT(*rm, 2, 0) + RMAT_ELMT(*rm, 0, 2)) / four_qz;
q->qy = (RMAT_ELMT(*rm, 1, 2) + RMAT_ELMT(*rm, 2, 1)) / four_qz;
q->qz = 0.5 * two_qz;
/*printf("m22 largest\n");*/
}
}
}
static void ahrs_do_update_accel(void) {
int i, j, k;
#ifdef BAFL_DEBUG2
printf("Accel update.\n");
#endif
/* 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
*
* g = 9.81
* gx = [ 0 -g 0
* g 0 0
* 0 0 0 ]
* H = [ 0 0 0 ]
* [ -Cnb*gx 0 0 0 ]
* [ 0 0 0 ]
*
* */
bafl_H[0][0] = -RMAT_ELMT(bafl_dcm, 0, 1) * BAFL_g;
bafl_H[0][1] = RMAT_ELMT(bafl_dcm, 0, 0) * BAFL_g;
bafl_H[1][0] = -RMAT_ELMT(bafl_dcm, 1, 1) * BAFL_g;
bafl_H[1][1] = RMAT_ELMT(bafl_dcm, 1, 0) * BAFL_g;
bafl_H[2][0] = -RMAT_ELMT(bafl_dcm, 2, 1) * BAFL_g;
bafl_H[2][1] = RMAT_ELMT(bafl_dcm, 2, 0) * BAFL_g;
/* rest is zero
bafl_H[0][2] = 0.;
bafl_H[1][2] = 0.;
bafl_H[2][2] = 0.;*/
/**********************************************
* 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)
* last 4 columns of H are zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < BAFL_SSIZE; j++) {
bafl_tempS[i][j] = bafl_H[i][0] * bafl_Pprio[0][j];
bafl_tempS[i][j] += bafl_H[i][1] * bafl_Pprio[1][j];
}
}
/* S(3x3) = temp_S(3x6) * HT(6x3) + R(3x3)
*
* bottom 4 rows of HT are zero
*/
for (i = 0; i < 3; i++) {
for (j = 0; j < 3; j++) {
bafl_S[i][j] = bafl_tempS[i][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
bafl_S[i][j] += bafl_tempS[i][1] * bafl_H[j][1]; /* H[j][0] = HT[0][j] */
}
bafl_S[i][i] += bafl_R_accel;
}
/* invert S
*/
FLOAT_MAT33_INVERT(bafl_invS, bafl_S);
/* temp_K(6x3) = P_prio(6x6) * HT(6x3)
*
* bottom 4 rows of HT are 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][0] * bafl_H[j][0]; /* H[j][0] = HT[0][j] */
bafl_tempK[i][j] += bafl_Pprio[i][1] * bafl_H[j][1]; /* H[j][1] = HT[1][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
**********************************************/
/*printf("R = ");
for (i = 0; i < 3; i++) {
printf("[");
for (j = 0; j < 3; j++) {
printf("%f\t", RMAT_ELMT(bafl_dcm, i, j));
}
printf("]\n ");
}
printf("\n");*/
/* innovation
* y = Cnb * -[0; 0; g] - accel
*/
bafl_ya.x = -RMAT_ELMT(bafl_dcm, 0, 2) * BAFL_g - bafl_accel_measure.x;
bafl_ya.y = -RMAT_ELMT(bafl_dcm, 1, 2) * BAFL_g - bafl_accel_measure.y;
bafl_ya.z = -RMAT_ELMT(bafl_dcm, 2, 2) * BAFL_g - bafl_accel_measure.z;
/* X(6) = K(6x3) * y(3)
*/
for (i = 0; i < BAFL_SSIZE; i++) {
bafl_X[i] = bafl_K[i][0] * bafl_ya.x;
bafl_X[i] += bafl_K[i][1] * bafl_ya.y;
bafl_X[i] += bafl_K[i][2] * bafl_ya.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 4 columns of H are zero
*/
for (i = 0; i < BAFL_SSIZE; i++) {
for (j = 0; j < BAFL_SSIZE; 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("Pua=");
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_a_err, 1.0, bafl_X[0]/2, bafl_X[1]/2, bafl_X[2]/2);
FLOAT_QUAT_INVERT(bafl_q_a_err, bafl_q_a_err);
/* normalize
* Is this needed? Or is the approximation good enough?*/
float q_sq;
q_sq = bafl_q_a_err.qx * bafl_q_a_err.qx + bafl_q_a_err.qy * bafl_q_a_err.qy + bafl_q_a_err.qz * bafl_q_a_err.qz;
if (q_sq > 1) { /* this should actually never happen */
FLOAT_QUAT_SMUL(bafl_q_a_err, bafl_q_a_err, 1 / sqrtf(1 + q_sq));
printf("Accel error quaternion q_sq > 1!!\n");
} else {
bafl_q_a_err.qi = sqrtf(1 - q_sq);
}
/* correct attitude
*/
FLOAT_QUAT_COMP(bafl_qtemp, bafl_q_a_err, bafl_quat);
FLOAT_QUAT_COPY(bafl_quat, bafl_qtemp);
/* correct gyro bias
*/
RATES_ASSIGN(bafl_b_a_err, bafl_X[3], bafl_X[4], bafl_X[5]);
RATES_SUB(bafl_bias, bafl_b_a_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();
}
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();
}
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 */
}