diff --git a/EKF/common.h b/EKF/common.h index a2c8bfd946..22210512fa 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -168,11 +168,15 @@ struct stateSample { }; struct fault_status_t { - bool bad_mag_x: 1; - bool bad_mag_y: 1; - bool bad_mag_z: 1; - bool bad_airspeed: 1; - bool bad_sideslip: 1; + bool bad_mag_x: 1; // true if the fusion of the magnetometer X-axis has encountered a numerical error + bool bad_mag_y: 1; // true if the fusion of the magnetometer Y-axis has encountered a numerical error + bool bad_mag_z: 1; // true if the fusion of the magnetometer Z-axis has encountered a numerical error + bool bad_mag_hdg: 1; // true if the fusion of the magnetic heading has encountered a numerical error + bool bad_mag_decl: 1; // true if the fusion of the magnetic declination has encountered a numerical error + bool bad_airspeed: 1; // true if fusion of the airspeed has encountered a numerical error + bool bad_sideslip: 1; // true if fusion of the synthetic sideslip constraint has encountered a numerical error + bool bad_optflow_X: 1; // true if fusion of the optical flow X axis has encountered a numerical error + bool bad_optflow_Y: 1; // true if fusion of the optical flow Y axis has encountered a numerical error }; // publish the status of various GPS quality checks diff --git a/EKF/control.cpp b/EKF/control.cpp index 64ca3820e1..ae34a76504 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -109,6 +109,14 @@ void Ekf::controlFusionModes() _control_status.flags.mag_3D = false; } } + + // if we are using 3-axis magnetometer fusion, but without external aiding, then the declination needs to be fused as an observation to prevent long term heading drift + if (_control_status.flags.mag_3D && !_control_status.flags.gps) { + _control_status.flags.mag_dec = true; + + } else { + _control_status.flags.mag_dec = false; + } } void Ekf::calculateVehicleStatus() diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 2ded05eae3..269b160a22 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -148,9 +148,9 @@ bool Ekf::update() 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 + 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..3ea6449a24 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 8722681c80..58e27bc1c3 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -506,10 +506,10 @@ void Ekf::fuseHeading() float t30 = t26 * t26; float t31 = t30 + 1.0f; - float H_MAG[3] = {}; - H_MAG[0] = -t31 * (t20 * (magZ * t16 + magY * t18) + t25 * t27 * (magY * t8 + magZ * t10)); - H_MAG[1] = t31 * (t20 * (magX * t18 + magZ * t22) + t25 * t27 * (magX * t8 - magZ * t11)); - H_MAG[2] = t31 * (t20 * (magX * t16 - magY * t22) + t25 * t27 * (magX * t10 + magY * t11)); + float H_HDG[3] = {}; + H_HDG[0] = -t31 * (t20 * (magZ * t16 + magY * t18) + t25 * t27 * (magY * t8 + magZ * t10)); + H_HDG[1] = t31 * (t20 * (magX * t18 + magZ * t22) + t25 * t27 * (magX * t8 - magZ * t11)); + H_HDG[2] = t31 * (t20 * (magX * t16 - magY * t22) + t25 * t27 * (magX * t10 + magY * t11)); // calculate innovation matrix::Dcm R_to_earth(_state.quat_nominal); @@ -528,23 +528,19 @@ void Ekf::fuseHeading() for (unsigned row = 0; row < 3; row++) { for (unsigned column = 0; column < 3; column++) { - PH[row] += P[row][column] * H_MAG[column]; + PH[row] += P[row][column] * H_HDG[column]; } - innovation_var += H_MAG[row] * PH[row]; + innovation_var += H_HDG[row] * PH[row]; } 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(); @@ -558,7 +554,7 @@ void Ekf::fuseHeading() for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < 3; column++) { - Kfusion[row] += P[row][column] * H_MAG[column]; + Kfusion[row] += P[row][column] * H_HDG[column]; } Kfusion[row] *= innovation_var_inv; @@ -595,7 +591,7 @@ void Ekf::fuseHeading() for (unsigned column = 0; column < _k_num_states; column++) { for (unsigned row = 0; row < 3; row++) { - HP[column] += H_MAG[row] * P[row][column]; + HP[column] += H_HDG[row] * P[row][column]; } } @@ -608,3 +604,139 @@ 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(); +} diff --git a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt index b4ca1fdebd..8e1d146575 100644 --- a/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt +++ b/matlab/generated/Inertial Nav EKF/Magnetic Declination Fusion.txt @@ -1,7 +1,7 @@ /* Autocode for fusion of a magnetic declination estimate where the innovation is given by -innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle; +innovation = atanf(magMeasEarthFrameEast/magMeasEarthFrameNorth) - declinationAngle; magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to earth frame. declinationAngle is the estimated declination as that location @@ -11,5 +11,60 @@ This fusion method is used to constrain the rotation of the earth field vector w can cause the magnetic declination of the earth field estimates to slowly rotate. */ -// Calculate intermediate variable float t2 = 1.0f/magN; float t4 = magE*t2; float t3 = tanf(t4); float t5 = t3*t3; float t6 = t5+1.0f; float t7 = 1.0f/(magN*magN); float t8 = P[17][17]*t2*t6; float t15 = P[16][17]*magE*t6*t7; float t9 = t8-t15; float t10 = t2*t6*t9; float t11 = P[17][16]*t2*t6; float t16 = P[16][16]*magE*t6*t7; float t12 = t11-t16; float t17 = magE*t6*t7*t12; float t13 = R_DECL+t10-t17; 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_MAG[24]; H_MAG[16] = -t18*1.0f/(t19*t19)*t24; H_MAG[17] = t21*t24; \ No newline at end of file +// Calculate intermediate variable +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; +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 +H_MAG[16] = -t18*1.0f/(t19*t19)*t24; +H_MAG[17] = t21*t24; + +// Calculate the Kalman gains +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);