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_align(void) { #if USE_MAGNETOMETER /* Compute an initial orientation from accel and mag directly as quaternion */ ahrs_float_get_quat_from_accel_mag(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); ahrs_impl.heading_aligned = TRUE; #else /* Compute an initial orientation from accel and just set heading to zero */ ahrs_float_get_quat_from_accel(&ahrs_impl.ltp_to_imu_quat, &ahrs_aligner.lp_accel); ahrs_impl.heading_aligned = FALSE; #endif /* Convert initial orientation from quat to rotation matrix representations. */ FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat); /* Compute initial body orientation */ compute_body_orientation_and_rates(); /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); ahrs.status = AHRS_RUNNING; }
int spi_ap_link_init() { if (spi_link_init()) { TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); return -1; } // Initialize IMU->Body orientation imuFloat.body_to_imu_quat = body_to_imu_quat; imuFloat.sample_count = 0; #ifdef IMU_ALIGN_BENCH // This code is for aligning body to imu rotation, turn this on, put the vehicle in hover, pointed north, read BOOZ2_AHRS_REF_QUAT as body to imu (in wing frame) struct FloatVect3 x_axis = { 0.0, 1.0, 0.0 }; FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH); #endif FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat); FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat); FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat); struct FloatRates bias0 = { 0., 0., 0.}; rdyb_mahrs_init(imuFloat.body_to_imu_quat, bias0); return 0; }
void test_of_axis_angle(void) { struct FloatVect3 axis = { 0., 1., 0.}; FLOAT_VECT3_NORMALIZE(axis); DISPLAY_FLOAT_VECT3("axis", axis); const float angle = RadOfDeg(30.); printf("angle %f\n", DegOfRad(angle)); struct FloatQuat my_q; FLOAT_QUAT_OF_AXIS_ANGLE(my_q, axis, angle); DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("quat", my_q); struct FloatMat33 my_r1; FLOAT_RMAT_OF_QUAT(my_r1, my_q); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", my_r1); DISPLAY_FLOAT_RMAT("rmat1", my_r1); struct FloatMat33 my_r; FLOAT_RMAT_OF_AXIS_ANGLE(my_r, axis, angle); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", my_r); DISPLAY_FLOAT_RMAT("rmat", my_r); printf("\n"); struct FloatEulers eul = {RadOfDeg(30.), RadOfDeg(30.), 0.}; struct FloatVect3 uz = { 0., 0., 1.}; struct FloatMat33 r_yaw; FLOAT_RMAT_OF_AXIS_ANGLE(r_yaw, uz, eul.psi); struct FloatVect3 uy = { 0., 1., 0.}; struct FloatMat33 r_pitch; FLOAT_RMAT_OF_AXIS_ANGLE(r_pitch, uy, eul.theta); struct FloatVect3 ux = { 1., 0., 0.}; struct FloatMat33 r_roll; FLOAT_RMAT_OF_AXIS_ANGLE(r_roll, ux, eul.phi); struct FloatMat33 r_yaw_pitch; FLOAT_RMAT_COMP(r_yaw_pitch, r_yaw, r_pitch); struct FloatMat33 r_yaw_pitch_roll; FLOAT_RMAT_COMP(r_yaw_pitch_roll, r_yaw_pitch, r_roll); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_RMAT("rmat", r_yaw_pitch_roll); DISPLAY_FLOAT_EULERS_DEG("eul", eul); struct FloatMat33 rmat1; FLOAT_RMAT_OF_EULERS(rmat1, eul); DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat1", rmat1); DISPLAY_FLOAT_RMAT("rmat1", rmat1); }
void ahrs_align(void) { /* Compute an initial orientation using euler angles */ ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag); /* Convert initial orientation in quaternion and rotation matrice representations. */ FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler); FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat); /* Compute initial body orientation */ compute_body_orientation_and_rates(); /* used averaged gyro as initial value for bias */ struct Int32Rates bias0; RATES_COPY(bias0, ahrs_aligner.lp_gyro); RATES_FLOAT_OF_BFP(ahrs_impl.gyro_bias, bias0); ahrs.status = AHRS_RUNNING; }
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 }
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) { /* cmd_x is positive to north = negative pitch * cmd_y is positive to east = positive roll * * orientation vector describing simultaneous rotation of roll/pitch */ const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0}; /* quaternion from that orientation vector */ struct FloatQuat q_rp; FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov); /* as rotation matrix */ struct FloatRMat R_rp; FLOAT_RMAT_OF_QUAT(R_rp, q_rp); /* body x-axis (before heading command) is first column */ struct FloatVect3 b_x; VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]); /* body z-axis (thrust vect) is last column */ struct FloatVect3 thrust_vect; VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]); /// @todo optimize yaw angle calculation /* * Instead of using the psi setpoint angle to rotate around the body z-axis, * calculate the real angle needed to align the projection of the body x-axis * onto the horizontal plane with the psi setpoint. * * angle between two vectors a and b: * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) * where the normal n is the thrust vector (i.e. both a and b lie in that plane) */ // desired heading vect in earth x-y plane const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0}; /* projection of desired heading onto body x-y plane * b = v - dot(v,n)*n */ float dot = FLOAT_VECT3_DOT_PRODUCT(psi_vect, thrust_vect); struct FloatVect3 dotn; FLOAT_VECT3_SMUL(dotn, thrust_vect, dot); // b = v - dot(v,n)*n struct FloatVect3 b; FLOAT_VECT3_DIFF(b, psi_vect, dotn); dot = FLOAT_VECT3_DOT_PRODUCT(b_x, b); struct FloatVect3 cross; VECT3_CROSS_PRODUCT(cross, b_x, b); // norm of the cross product float nc = FLOAT_VECT3_NORM(cross); // angle = atan2(norm(cross(a,b)), dot(a,b)) float yaw2 = atan2(nc, dot) / 2.0; // negative angle if needed // sign(dot(cross(a,b), n) float dot_cross_ab = FLOAT_VECT3_DOT_PRODUCT(cross, thrust_vect); if (dot_cross_ab < 0) { yaw2 = -yaw2; } /* quaternion with yaw command */ struct FloatQuat q_yaw; QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ FLOAT_QUAT_COMP(*quat, q_rp, q_yaw); FLOAT_QUAT_NORMALIZE(*quat); FLOAT_QUAT_WRAP_SHORTEST(*quat); }
static inline void compute_imu_rmat_and_euler_from_quat(void) { FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat); FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat); }