From 402206a305a40f44117f8b6141451cf043479485 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 16 Feb 2016 10:03:50 +1100 Subject: [PATCH] EKF: Fix critical bug in fusion of yaw and declination observations This bug was in the derivation and resulted from use of a tan instead of an atan operator. The derivations have been reworked and the updated auto-code has been imported as part of this patch. --- EKF/mag_fusion.cpp | 210 +++++++++++++++++++++++---------------------- 1 file changed, 107 insertions(+), 103 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index e1f7857063..52c2b1433e 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -468,73 +468,71 @@ void Ekf::fuseHeading() float q2 = _state.quat_nominal(2); float q3 = _state.quat_nominal(3); - float magX = _mag_sample_delayed.mag(0); - float magY = _mag_sample_delayed.mag(1); - float magZ = _mag_sample_delayed.mag(2); - - float R_mag = fmaxf(_params.mag_heading_noise, 1.0e-2f); - R_mag = R_mag * R_mag; + float R_YAW = fmaxf(_params.mag_heading_noise, 1.0e-2f); + R_YAW = R_YAW * R_YAW; + // calculate intermediate variables for observation jacobian float t2 = q0 * q0; float t3 = q1 * q1; float t4 = q2 * q2; float t5 = q3 * q3; - float t6 = q0 * q2 * 2.0f; - float t7 = q1 * q3 * 2.0f; - float t8 = t6 + t7; - float t9 = q0 * q3 * 2.0f; - float t13 = q1 * q2 * 2.0f; - float t10 = t9 - t13; - float t11 = t2 + t3 - t4 - t5; - float t12 = magX * t11; - float t14 = magZ * t8; - float t19 = magY * t10; - float t15 = t12 + t14 - t19; - float t16 = t2 - t3 + t4 - t5; - float t17 = q0 * q1 * 2.0f; - float t24 = q2 * q3 * 2.0f; - float t18 = t17 - t24; - float t20 = 1.0f / t15; - float t21 = magY * t16; - float t22 = t9 + t13; - float t23 = magX * t22; - float t28 = magZ * t18; - float t25 = t21 + t23 - t28; - float t29 = t20 * t25; - float t26 = tan(t29); - float t27 = 1.0f / (t15 * t15); - float t30 = t26 * t26; - float t31 = t30 + 1.0f; - - 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)); + float t6 = t2 + t3 - t4 - t5; + float t7 = q0 * q3 * 2.0f; + float t8 = q1 * q2 * 2.0f; + float t9 = t7 + t8; + float t10; + + if (fabsf(t6) > 1e-6f) { + t10 = 1.0f / (t6 * t6); + + } else { + return; + } + + float t11 = t9 * t9; + float t12 = t10 * t11; + float t13 = t12 + 1.0f; + float t14; + + if (fabsf(t13) > 1e-6f) { + t14 = 1.0f / t13; + + } else { + return; + } + + float t15 = 1.0f / t6; + + // calculate observation jacobian + float H_YAW[3] = {}; + H_YAW[1] = t14 * (t15 * (q0 * q1 * 2.0f - q2 * q3 * 2.0f) + t9 * t10 * (q0 * q2 * 2.0f + q1 * q3 * 2.0f)); + H_YAW[2] = t14 * (t15 * (t2 - t3 + t4 - t5) + t9 * t10 * (t7 - t8)); // calculate innovation + // rotate body field magnetic field measurement into earth frame and compare to published declination to get heading measurement + // TODO - enable use of an off-board heading measurement matrix::Dcm R_to_earth(_state.quat_nominal); matrix::Vector3f mag_earth_pred = R_to_earth * _mag_sample_delayed.mag; - float innovation = atan2f(mag_earth_pred(1), mag_earth_pred(0)) - _mag_declination; - innovation = math::constrain(innovation, -0.5f, 0.5f); + // wrap the innovation to the interval between +-pi + innovation = matrix::wrap_pi(innovation); _heading_innov = innovation; - float innovation_var = R_mag; - _heading_innov_var = innovation_var; - // calculate innovation variance + float innovation_var = R_YAW; + _heading_innov_var = innovation_var; float PH[3] = {}; for (unsigned row = 0; row < 3; row++) { for (unsigned column = 0; column < 3; column++) { - PH[row] += P[row][column] * H_HDG[column]; + PH[row] += P[row][column] * H_YAW[column]; } - innovation_var += H_HDG[row] * PH[row]; + innovation_var += H_YAW[row] * PH[row]; } - if (innovation_var >= R_mag) { + if (innovation_var >= R_YAW) { // the innovation variance contribution from the state covariances is not negative, no fault _fault_status.bad_mag_hdg = false; @@ -547,14 +545,14 @@ void Ekf::fuseHeading() return; } - float innovation_var_inv = 1 / innovation_var; + float innovation_var_inv = 1.0f / innovation_var; - // calculate kalman gain + // calculate the kalman gains taking dvantage of the reduce size of H_YAW float Kfusion[_k_num_states] = {}; for (unsigned row = 0; row < _k_num_states; row++) { for (unsigned column = 0; column < 3; column++) { - Kfusion[row] += P[row][column] * H_HDG[column]; + Kfusion[row] += P[row][column] * H_YAW[column]; } Kfusion[row] *= innovation_var_inv; @@ -572,12 +570,18 @@ void Ekf::fuseHeading() // by interference or a large initial gyro bias if (_control_status.flags.in_air) { return; + + } else { + // constrain the innovation to the maximum set by the gate + float gate_limit = sqrtf((sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var)); + innovation = math::constrain(innovation, -gate_limit, gate_limit); } } else { _mag_healthy = true; } + // zero the attitude error states and use the kalman gain vector and innovation to update the states _state.ang_error.setZero(); fuse(Kfusion, innovation); @@ -587,11 +591,12 @@ void Ekf::fuseHeading() _state.quat_nominal = dq * _state.quat_nominal; _state.quat_nominal.normalize(); + // update the covariance matrix taking advantage of the reduced size of H_YAW float HP[_k_num_states] = {}; for (unsigned column = 0; column < _k_num_states; column++) { for (unsigned row = 0; row < 3; row++) { - HP[column] += H_HDG[row] * P[row][column]; + HP[column] += H_YAW[row] * P[row][column]; } } @@ -615,30 +620,28 @@ void Ekf::fuseDeclination() // Calculate intermediate variables // if the horizontal magnetic field is too small, this calculation will be badly conditioned - if (fabsf(magN) < 0.001f) { + if (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 + float t2 = magE * magE; + float t3 = magN * magN; + float t4 = t2 + t3; + float t5 = 1.0f / t4; + float t22 = magE * t5; + float t23 = magN * t5; + float t6 = P[16][16] * t22; + float t13 = P[17][16] * t23; + float t7 = t6 - t13; + float t8 = t22 * t7; + float t9 = P[16][17] * t22; + float t14 = P[17][17] * t23; + float t10 = t9 - t14; + float t15 = t23 * t10; + float t11 = R_DECL + t8 - t15; // innovation variance // check the innovation variance calculation for a badly conditioned covariance matrix - if (t13 >= R_DECL) { + if (t11 >= R_DECL) { // the innovation variance contribution from the state covariances is not negative, no fault _fault_status.bad_mag_decl = false; @@ -651,50 +654,51 @@ void Ekf::fuseDeclination() 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; + float t12 = 1.0f / t11; // 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; + H_DECL[16] = -magE * t5; + H_DECL[17] = magN * t5; // 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); + Kfusion[0] = -t12 * (P[0][16] * t22 - P[0][17] * t23); + Kfusion[1] = -t12 * (P[1][16] * t22 - P[1][17] * t23); + Kfusion[2] = -t12 * (P[2][16] * t22 - P[2][17] * t23); + Kfusion[3] = -t12 * (P[3][16] * t22 - P[3][17] * t23); + Kfusion[4] = -t12 * (P[4][16] * t22 - P[4][17] * t23); + Kfusion[5] = -t12 * (P[5][16] * t22 - P[5][17] * t23); + Kfusion[6] = -t12 * (P[6][16] * t22 - P[6][17] * t23); + Kfusion[7] = -t12 * (P[7][16] * t22 - P[7][17] * t23); + Kfusion[8] = -t12 * (P[8][16] * t22 - P[8][17] * t23); + Kfusion[9] = -t12 * (P[9][16] * t22 - P[9][17] * t23); + Kfusion[10] = -t12 * (P[10][16] * t22 - P[10][17] * t23); + Kfusion[11] = -t12 * (P[11][16] * t22 - P[11][17] * t23); + Kfusion[12] = -t12 * (P[12][16] * t22 - P[12][17] * t23); + Kfusion[13] = -t12 * (P[13][16] * t22 - P[13][17] * t23); + Kfusion[14] = -t12 * (P[14][16] * t22 - P[14][17] * t23); + Kfusion[15] = -t12 * (P[15][16] * t22 - P[15][17] * t23); + Kfusion[16] = -t12 * (t6 - P[16][17] * t23); + Kfusion[17] = t12 * (t14 - P[17][16] * t22); + Kfusion[18] = -t12 * (P[18][16] * t22 - P[18][17] * t23); + Kfusion[19] = -t12 * (P[19][16] * t22 - P[19][17] * t23); + Kfusion[20] = -t12 * (P[20][16] * t22 - P[20][17] * t23); + Kfusion[21] = -t12 * (P[21][16] * t22 - P[21][17] * t23); + + // Don't update wind states unless we are doing wind estimation + if (_control_status.flags.wind) { + Kfusion[22] = -t12 * (P[22][16] * t22 - P[22][17] * t23); + Kfusion[23] = -t12 * (P[23][16] * t22 - P[23][17] * t23); + + } else { + Kfusion[22] = 0.0f; + Kfusion[23] = 0.0f; + } // calculate innovation and constrain - float innovation = atanf(t4) - _mag_declination; + float innovation = atanf(magE / magN) - _mag_declination; innovation = math::constrain(innovation, -0.5f, 0.5f); // zero attitude error states and perform the state correction