From a22886544d65b6d00f6b1c535bc999056582d10d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Feb 2016 14:02:08 +1100 Subject: [PATCH 1/7] EKF: Add missing Kalman gain to Declination fusion autocode --- .../Magnetic Declination Fusion.txt | 61 ++++++++++++++++++- 1 file changed, 58 insertions(+), 3 deletions(-) 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); From 6aeccf01ff47f87305e6f144c33c3afff8395d2b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Feb 2016 14:07:45 +1100 Subject: [PATCH 2/7] EKF: Add missing fusion health status messages --- EKF/common.h | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index a2c8bfd946..9da43a3550 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 From 9347afe250b08fcc4555fc9804feb28ea10d1396 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Feb 2016 14:11:54 +1100 Subject: [PATCH 3/7] EKF: Use unique variable name for mag heading fusion observation Jacobian --- EKF/mag_fusion.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 8722681c80..448cf62a02 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,10 +528,10 @@ 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) { @@ -558,7 +558,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 +595,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]; } } From dbb8e12948f553007c17ba97698a963b036faea0 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Feb 2016 14:15:05 +1100 Subject: [PATCH 4/7] EKF: Add fusion method to constrain declination when unobservable When fusing 3-axis magnetometer data without absolute external aiding leg GPS), the declination is ultimately unobservable and the declination of the field states and the vehicle heading will slowly drift over time. To prevent this we need to fuse in a declination to constraint the NE earth field estimates. --- EKF/ekf.cpp | 8 +-- EKF/ekf.h | 2 + EKF/mag_fusion.cpp | 145 ++++++++++++++++++++++++++++++++++++++++++--- 3 files changed, 143 insertions(+), 12 deletions(-) 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(); +} From cdc42c1de067c20c477ca3d4e13f3066cedc81d7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Feb 2016 14:15:41 +1100 Subject: [PATCH 5/7] EKF: Add simple control logic for magnetic declination constraint --- EKF/control.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index 64ca3820e1..23df635ec5 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -109,6 +109,13 @@ 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 = false; + } else { + _control_status.flags.mag_dec = true; + } } void Ekf::calculateVehicleStatus() From 47ab5ebcdd7d01b3915b122f6bf3e73349d604eb Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 8 Feb 2016 15:12:38 +1100 Subject: [PATCH 6/7] EKF: Make PR comply with project convention for indenting --- EKF/common.h | 18 ++-- EKF/control.cpp | 13 +-- EKF/ekf.cpp | 8 +- EKF/ekf.h | 2 +- EKF/mag_fusion.cpp | 251 +++++++++++++++++++++++---------------------- 5 files changed, 148 insertions(+), 144 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 9da43a3550..22210512fa 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -168,15 +168,15 @@ struct stateSample { }; struct fault_status_t { - 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 + 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 23df635ec5..104a5f21bd 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -110,12 +110,13 @@ void Ekf::controlFusionModes() } } - // 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 = false; - } else { - _control_status.flags.mag_dec = true; - } + // 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 = false; + + } else { + _control_status.flags.mag_dec = true; + } } void Ekf::calculateVehicleStatus() diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index da63609199..269b160a22 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -146,9 +146,11 @@ 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) { - fuseDeclination(); - } + + 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 f3b1a11aca..3ea6449a24 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -162,7 +162,7 @@ private: void fuseHeading(); - void fuseDeclination(); + void fuseDeclination(); void fuseAirspeed(); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index b3f20ab8e1..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_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 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,19 +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_HDG[column]; + PH[row] += P[row][column] * H_HDG[column]; } - innovation_var += H_HDG[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_hdg = false; + _fault_status.bad_mag_hdg = false; } else { - // the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned - _fault_status.bad_mag_hdg = 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(); @@ -554,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_HDG[column]; + Kfusion[row] += P[row][column] * H_HDG[column]; } Kfusion[row] *= innovation_var_inv; @@ -591,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_HDG[row] * P[row][column]; + HP[column] += H_HDG[row] * P[row][column]; } } @@ -607,135 +607,136 @@ void Ekf::fuseHeading() void Ekf::fuseDeclination() { - // assign intermediate state variables - float magN = _state.mag_I(0); - float magE = _state.mag_I(1); + // assign intermediate state variables + float magN = _state.mag_I(0); + float magE = _state.mag_I(1); - float R_DECL = sq(0.5f); + 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 + // Calculate intermediate variables + // if the horizontal magnetic field is too small, this calculation will be badly conditioned + if (fabsf(magN) < 0.001f) { + return; + } - // 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; + 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 - } 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; + // 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; - // we reinitialise the covariance matrix and abort this fusion step - initialiseCovariance(); - return; - } + } 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; - 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; + // we reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } - // 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; + 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 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 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 innovation and constrain - float innovation = atanf(t4) - math::radians(_params.mag_declination_deg); - innovation = math::constrain(innovation, -0.5f, 0.5f); + // 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); - // zero attitude error states and perform the state correction - _state.ang_error.setZero(); - fuse(Kfusion, innovation); + // calculate innovation and constrain + float innovation = atanf(t4) - math::radians(_params.mag_declination_deg); + innovation = math::constrain(innovation, -0.5f, 0.5f); - // 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(); + // zero attitude error states and perform the state correction + _state.ang_error.setZero(); + fuse(Kfusion, innovation); - // 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] = {}; + // 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(); - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 16; column < 17; column++) { - KH[row][column] = Kfusion[row] * H_DECL[column]; - } - } + // 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] = {}; - float KHP[_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]; + } + } - 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; - } - } + 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++) { - P[row][column] -= KHP[row][column]; - } - } + 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; + } + } - // force the covariance matrix to be symmetrical and don't allow the variances to be negative. - makeSymmetrical(); - limitCov(); + 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(); } From 5ea0ef39cdd50e8b634dd38e72e48844e660a11c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 9 Feb 2016 07:44:34 +1100 Subject: [PATCH 7/7] EKF: Update declination fusion logic We do not need to run declination fusion if 3-axis mag fusion is not being used. --- EKF/control.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 104a5f21bd..ae34a76504 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -111,11 +111,11 @@ void Ekf::controlFusionModes() } // 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 = false; + if (_control_status.flags.mag_3D && !_control_status.flags.gps) { + _control_status.flags.mag_dec = true; } else { - _control_status.flags.mag_dec = true; + _control_status.flags.mag_dec = false; } }