diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 2ded05eae3..da63609199 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -146,11 +146,9 @@ bool Ekf::update() if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { if (_control_status.flags.mag_3D && _control_status.flags.angle_align) { fuseMag(); - - if (_control_status.flags.mag_dec) { - // TODO need to fuse synthetic declination measurements if there is no GPS or equivalent aiding - // otherwise heading will slowly drift - } + if (_control_status.flags.mag_dec) { + fuseDeclination(); + } } else if (_control_status.flags.mag_hdg && _control_status.flags.angle_align) { fuseHeading(); } diff --git a/EKF/ekf.h b/EKF/ekf.h index 9b7ebc285f..f3b1a11aca 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -162,6 +162,8 @@ private: void fuseHeading(); + void fuseDeclination(); + void fuseAirspeed(); void fuseRange(); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 448cf62a02..b3f20ab8e1 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -536,15 +536,11 @@ void Ekf::fuseHeading() if (innovation_var >= R_mag) { // the innovation variance contribution from the state covariances is not negative, no fault - _fault_status.bad_mag_x = false; - _fault_status.bad_mag_y = false; - _fault_status.bad_mag_z = false; + _fault_status.bad_mag_hdg = false; } else { - // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned - _fault_status.bad_mag_x = true; - _fault_status.bad_mag_y = true; - _fault_status.bad_mag_z = true; + // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned + _fault_status.bad_mag_hdg = true; // we reinitialise the covariance matrix and abort this fusion step initialiseCovariance(); @@ -608,3 +604,138 @@ void Ekf::fuseHeading() makeSymmetrical(); limitCov(); } + +void Ekf::fuseDeclination() +{ + // assign intermediate state variables + float magN = _state.mag_I(0); + float magE = _state.mag_I(1); + + float R_DECL = sq(0.5f); + + // Calculate intermediate variables + // if the horizontal magnetic field is too small, this calculation will be badly conditioned + if (fabsf(magN) < 0.001f) { + return; + } + float t2 = 1.0f/magN; + float t4 = magE*t2; + float t3 = tanf(t4); + float t5 = t3*t3; + float t6 = t5+1.0f; + float t25 = t2*t6; + float t7 = 1.0f/(magN*magN); + float t26 = magE*t6*t7; + float t8 = P[17][17]*t25; + float t15 = P[16][17]*t26; + float t9 = t8-t15; + float t10 = t25*t9; + float t11 = P[17][16]*t25; + float t16 = P[16][16]*t26; + float t12 = t11-t16; + float t17 = t26*t12; + float t13 = R_DECL+t10-t17; // innovation variance + + // check the innovation variance calculation for a badly conditioned covariance matrix + if (t13 >= R_DECL) { + // the innovation variance contribution from the state covariances is not negative, no fault + _fault_status.bad_mag_decl = false; + + } else { + // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned + _fault_status.bad_mag_decl = true; + + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } + + float t14 = 1.0f/t13; + float t18 = magE; + float t19 = magN; + float t21 = 1.0f/t19; + float t22 = t18*t21; + float t20 = tanf(t22); + float t23 = t20*t20; + float t24 = t23+1.0f; + + // Calculate the observation Jacobian + // Note only 2 terms are non-zero which can be used in matrix operations for calculation of Kalman gains and covariance update to significantly reduce cost + float H_DECL[24] = {}; + H_DECL[16] = -t18*1.0f/(t19*t19)*t24; + H_DECL[17] = t21*t24; + + // Calculate the Kalman gains + float Kfusion[_k_num_states] = {}; + Kfusion[0] = t14*(P[0][17]*t25-P[0][16]*t26); + Kfusion[1] = t14*(P[1][17]*t25-P[1][16]*t26); + Kfusion[2] = t14*(P[2][17]*t25-P[2][16]*t26); + Kfusion[3] = t14*(P[3][17]*t25-P[3][16]*t26); + Kfusion[4] = t14*(P[4][17]*t25-P[4][16]*t26); + Kfusion[5] = t14*(P[5][17]*t25-P[5][16]*t26); + Kfusion[6] = t14*(P[6][17]*t25-P[6][16]*t26); + Kfusion[7] = t14*(P[7][17]*t25-P[7][16]*t26); + Kfusion[8] = t14*(P[8][17]*t25-P[8][16]*t26); + Kfusion[9] = t14*(P[9][17]*t25-P[9][16]*t26); + Kfusion[10] = t14*(P[10][17]*t25-P[10][16]*t26); + Kfusion[11] = t14*(P[11][17]*t25-P[11][16]*t26); + Kfusion[12] = t14*(P[12][17]*t25-P[12][16]*t26); + Kfusion[13] = t14*(P[13][17]*t25-P[13][16]*t26); + Kfusion[14] = t14*(P[14][17]*t25-P[14][16]*t26); + Kfusion[15] = t14*(P[15][17]*t25-P[15][16]*t26); + Kfusion[16] = -t14*(t16-P[16][17]*t25); + Kfusion[17] = t14*(t8-P[17][16]*t26); + Kfusion[18] = t14*(P[18][17]*t25-P[18][16]*t26); + Kfusion[19] = t14*(P[19][17]*t25-P[19][16]*t26); + Kfusion[20] = t14*(P[20][17]*t25-P[20][16]*t26); + Kfusion[21] = t14*(P[21][17]*t25-P[21][16]*t26); + Kfusion[22] = t14*(P[22][17]*t25-P[22][16]*t26); + Kfusion[23] = t14*(P[23][17]*t25-P[23][16]*t26); + + // calculate innovation and constrain + float innovation = atanf(t4) - math::radians(_params.mag_declination_deg); + innovation = math::constrain(innovation, -0.5f, 0.5f); + + // zero attitude error states and perform the state correction + _state.ang_error.setZero(); + fuse(Kfusion, innovation); + + // use the attitude error estimate to correct the quaternion + Quaternion dq; + dq.from_axis_angle(_state.ang_error); + _state.quat_nominal = dq * _state.quat_nominal; + _state.quat_nominal.normalize(); + + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP + // take advantage of the empty columns in KH to reduce the number of operations + float KH[_k_num_states][_k_num_states] = {}; + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 16; column < 17; column++) { + KH[row][column] = Kfusion[row] * H_DECL[column]; + } + } + + float KHP[_k_num_states][_k_num_states] = {}; + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + float tmp = KH[row][0] * P[0][column]; + tmp += KH[row][16] * P[16][column]; + tmp += KH[row][17] * P[17][column]; + KHP[row][column] = tmp; + } + } + + for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned column = 0; column < _k_num_states; column++) { + P[row][column] -= KHP[row][column]; + } + } + + // force the covariance matrix to be symmetrical and don't allow the variances to be negative. + makeSymmetrical(); + limitCov(); +}