void ahrs_propagate(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate, (1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate,gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
#if AHRS_PROPAGATE_RMAT
FLOAT_RMAT_INTEGRATE_FI(ahrs_impl.ltp_to_imu_rmat, omega, dt);
float_rmat_reorthogonalize(&ahrs_impl.ltp_to_imu_rmat);
FLOAT_QUAT_OF_RMAT(ahrs_impl.ltp_to_imu_quat, ahrs_impl.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, omega, dt);
FLOAT_QUAT_NORMALIZE(ahrs_impl.ltp_to_imu_quat);
FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
#endif
compute_body_orientation_and_rates();
}
void ahrs_propagate(void) {
/* compute unbiased rates */
RATES_FLOAT_OF_BFP(bafe_rates, imu.gyro);
RATES_SUB(bafe_rates, bafe_bias);
/* compute F
F is only needed later on to update the state covariance P.
However, its [0:3][0:3] region is actually the Wxq(pqr) which is needed to
compute the time derivative of the quaternion, so we compute F now
*/
/* Fill in Wxq(pqr) into F */
bafe_F[0][0] = bafe_F[1][1] = bafe_F[2][2] = bafe_F[3][3] = 0;
bafe_F[1][0] = bafe_F[2][3] = bafe_rates.p * 0.5;
bafe_F[2][0] = bafe_F[3][1] = bafe_rates.q * 0.5;
bafe_F[3][0] = bafe_F[1][2] = bafe_rates.r * 0.5;
bafe_F[0][1] = bafe_F[3][2] = -bafe_F[1][0];
bafe_F[0][2] = bafe_F[1][3] = -bafe_F[2][0];
bafe_F[0][3] = bafe_F[2][1] = -bafe_F[3][0];
/* Fill in [4:6][0:3] region */
bafe_F[0][4] = bafe_F[2][6] = bafe_quat.qx * 0.5;
bafe_F[0][5] = bafe_F[3][4] = bafe_quat.qy * 0.5;
bafe_F[0][6] = bafe_F[1][5] = bafe_quat.qz * 0.5;
bafe_F[1][4] = bafe_F[2][5] = bafe_F[3][6] = bafe_quat.qi * -0.5;
bafe_F[3][5] = -bafe_F[0][4];
bafe_F[1][6] = -bafe_F[0][5];
bafe_F[2][4] = -bafe_F[0][6];
/* quat_dot = Wxq(pqr) * quat */
bafe_qdot.qi= bafe_F[0][1]*bafe_quat.qx+bafe_F[0][2]*bafe_quat.qy+bafe_F[0][3] * bafe_quat.qz;
bafe_qdot.qx= bafe_F[1][0]*bafe_quat.qi +bafe_F[1][2]*bafe_quat.qy+bafe_F[1][3] * bafe_quat.qz;
bafe_qdot.qy= bafe_F[2][0]*bafe_quat.qi+bafe_F[2][1]*bafe_quat.qx +bafe_F[2][3] * bafe_quat.qz;
bafe_qdot.qz= bafe_F[3][0]*bafe_quat.qi+bafe_F[3][1]*bafe_quat.qx+bafe_F[3][2]*bafe_quat.qy ;
/* propagate quaternion */
bafe_quat.qi += bafe_qdot.qi * BAFE_DT;
bafe_quat.qx += bafe_qdot.qx * BAFE_DT;
bafe_quat.qy += bafe_qdot.qy * BAFE_DT;
bafe_quat.qz += bafe_qdot.qz * BAFE_DT;
}
static inline void propagate_ref(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
/* lowpass angular rates */
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
(1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate, gyro_float);
#endif
/* propagate reference quaternion only if rate is non null */
const float no = FLOAT_RATES_NORM(ahrs_impl.imu_rate);
if (no > FLT_MIN) {
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
const float a = 0.5*no*dt;
const float ca = cosf(a);
const float sa_ov_no = sinf(a)/no;
const float dp = sa_ov_no*ahrs_impl.imu_rate.p;
const float dq = sa_ov_no*ahrs_impl.imu_rate.q;
const float dr = sa_ov_no*ahrs_impl.imu_rate.r;
const float qi = ahrs_impl.ltp_to_imu_quat.qi;
const float qx = ahrs_impl.ltp_to_imu_quat.qx;
const float qy = ahrs_impl.ltp_to_imu_quat.qy;
const float qz = ahrs_impl.ltp_to_imu_quat.qz;
ahrs_impl.ltp_to_imu_quat.qi = ca*qi - dp*qx - dq*qy - dr*qz;
ahrs_impl.ltp_to_imu_quat.qx = dp*qi + ca*qx + dr*qy - dq*qz;
ahrs_impl.ltp_to_imu_quat.qy = dq*qi - dr*qx + ca*qy + dp*qz;
ahrs_impl.ltp_to_imu_quat.qz = dr*qi + dq*qx - dp*qy + ca*qz;
// printf("%f\n", ahrs_impl.ltp_to_imu_quat.qi);
}
}
static inline void propagate_ref(void) {
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
/* lowpass angular rates */
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_impl.imu_rate, ahrs_impl.imu_rate,
(1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_impl.imu_rate, gyro_float);
#endif
/* propagate reference quaternion */
const float dt = 1. / (AHRS_PROPAGATE_FREQUENCY);
FLOAT_QUAT_INTEGRATE(ahrs_impl.ltp_to_imu_quat, ahrs_impl.imu_rate, dt);
}
void ahrs_fc_propagate(struct Int32Rates *gyro, float dt)
{
/* converts gyro to floating point */
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, *gyro);
/* unbias measurement */
RATES_SUB(gyro_float, ahrs_fc.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_fc.imu_rate, ahrs_fc.imu_rate, (1. - alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_fc.imu_rate, gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, gyro_float, ahrs_fc.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_fc.rate_correction);
#if AHRS_PROPAGATE_RMAT
float_rmat_integrate_fi(&ahrs_fc.ltp_to_imu_rmat, &omega, dt);
float_rmat_reorthogonalize(&ahrs_fc.ltp_to_imu_rmat);
float_quat_of_rmat(&ahrs_fc.ltp_to_imu_quat, &ahrs_fc.ltp_to_imu_rmat);
#endif
#if AHRS_PROPAGATE_QUAT
float_quat_integrate(&ahrs_fc.ltp_to_imu_quat, &omega, dt);
float_quat_normalize(&ahrs_fc.ltp_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
#endif
// increase accel and mag propagation counters
ahrs_fc.accel_cnt++;
ahrs_fc.mag_cnt++;
}
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();
}
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();
}
/*
* 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
}
