From 802129f3841fef5ff950ad4a6ab26425d7609718 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 26 Jan 2016 16:49:35 +1100 Subject: [PATCH] EKF: Add magnetometer fusion error handling --- EKF/ekf.cpp | 4 +- EKF/ekf.h | 4 +- EKF/estimator_base.h | 5 +- EKF/mag_fusion.cpp | 705 +++++++++++++++++++++++-------------------- 4 files changed, 387 insertions(+), 331 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 4186f751f8..8ff0b496b6 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -49,7 +49,6 @@ Ekf::Ekf(): _fuse_pos(false), _fuse_hor_vel(false), _fuse_vert_vel(false), - _mag_fuse_index(0), _time_last_fake_gps(0), _vel_pos_innov{}, _mag_innov{}, @@ -96,8 +95,7 @@ bool Ekf::update() if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { //fuseHeading(); - fuseMag(_mag_fuse_index); - _mag_fuse_index = (_mag_fuse_index + 1) % 3; + fuseMag(); } if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { diff --git a/EKF/ekf.h b/EKF/ekf.h index 7567af22d1..640a7cf8e6 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -91,8 +91,6 @@ private: bool _fuse_hor_vel; // gps horizontal velocity measurement should be fused bool _fuse_vert_vel; // gps vertical velocity measurement should be fused - uint8_t _mag_fuse_index; // counter for sequential mag axis fusion - uint64_t _time_last_fake_gps; Vector3f _earth_rate_NED; @@ -124,7 +122,7 @@ private: void predictCovariance(); - void fuseMag(uint8_t index); + void fuseMag(); void fuseHeading(); diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index 4edee4c81d..43f8076426 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -84,7 +84,8 @@ struct parameters { float mag_heading_noise = 3e-2f; // measurement noise used for simple heading fusion float mag_declination_deg = 0.0f; // magnetic declination in degrees - float heading_innov_gate = 0.5f; // innovation gate for heading innovation test + float heading_innov_gate = 3.0f; // heading fusion innovation consistency gate size in standard deviations + float mag_innov_gate = 3.0f; // magnetometer fusion innovation consistency gate size in standard deviations // these parameters control the strictness of GPS quality checks used to determine uf the GPS is // good enough to set a local origin and commence aiding @@ -257,6 +258,8 @@ protected: bool _gps_speed_valid = false; bool _mag_healthy = false; // computed by mag innovation test + float _yaw_test_ratio; // yaw innovation consistency check ratio + float _mag_test_ratio[3]; // magnetometer XYZ innovation consistency check ratios bool _in_air = true; // indicates if the vehicle is in the air diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 91ba8071bf..01878e2e0c 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -36,358 +36,416 @@ * Magnetometer fusion methods. * * @author Roman Bast + * @author Paul Riseborough * */ #include "ekf.h" #include -void Ekf::fuseMag(uint8_t index) +void Ekf::fuseMag() { - // assign intermediate variables - float q0 = _state.quat_nominal(0); - float q1 = _state.quat_nominal(1); - float q2 = _state.quat_nominal(2); - float q3 = _state.quat_nominal(3); + // assign intermediate variables + float q0 = _state.quat_nominal(0); + float q1 = _state.quat_nominal(1); + float q2 = _state.quat_nominal(2); + float q3 = _state.quat_nominal(3); - float magN = _state.mag_I(0); - float magE = _state.mag_I(1); - float magD = _state.mag_I(2); + float magN = _state.mag_I(0); + float magE = _state.mag_I(1); + float magD = _state.mag_I(2); - // XXX Measurement uncertainty. Need to consider timing errors for fast rotations - float R_MAG = 1e-3f; + // XYZ Measurement uncertainty. Need to consider timing errors for fast rotations + float R_MAG = 1e-3f; - // intermediate variables from algebraic optimisation - float SH_MAG[9]; - SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); - SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); - SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); - SH_MAG[3] = 2 * q0 * q1 + 2 * q2 * q3; - SH_MAG[4] = 2 * q0 * q3 + 2 * q1 * q2; - SH_MAG[5] = 2 * q0 * q2 + 2 * q1 * q3; - SH_MAG[6] = magE * (2 * q0 * q1 - 2 * q2 * q3); - SH_MAG[7] = 2 * q1 * q3 - 2 * q0 * q2; - SH_MAG[8] = 2 * q0 * q3; + // intermediate variables from algebraic optimisation + float SH_MAG[9]; + SH_MAG[0] = sq(q0) - sq(q1) + sq(q2) - sq(q3); + SH_MAG[1] = sq(q0) + sq(q1) - sq(q2) - sq(q3); + SH_MAG[2] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SH_MAG[3] = 2 * q0 * q1 + 2 * q2 * q3; + SH_MAG[4] = 2 * q0 * q3 + 2 * q1 * q2; + SH_MAG[5] = 2 * q0 * q2 + 2 * q1 * q3; + SH_MAG[6] = magE * (2 * q0 * q1 - 2 * q2 * q3); + SH_MAG[7] = 2 * q1 * q3 - 2 * q0 * q2; + SH_MAG[8] = 2 * q0 * q3; - // rotate magnetometer earth field state into body frame - matrix::Dcm R_to_body(_state.quat_nominal); - R_to_body = R_to_body.transpose(); + // rotate magnetometer earth field state into body frame + matrix::Dcm R_to_body(_state.quat_nominal); + R_to_body = R_to_body.transpose(); - Vector3f mag_I_rot = R_to_body * _state.mag_I; + Vector3f mag_I_rot = R_to_body * _state.mag_I; - // compute magnetometer innovations - _mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0); - _mag_innov[1] = (mag_I_rot(1) + _state.mag_B(1)) - _mag_sample_delayed.mag(1); - _mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2); + // compute magnetometer innovations + _mag_innov[0] = (mag_I_rot(0) + _state.mag_B(0)) - _mag_sample_delayed.mag(0); + _mag_innov[1] = (mag_I_rot(1) + _state.mag_B(1)) - _mag_sample_delayed.mag(1); + _mag_innov[2] = (mag_I_rot(2) + _state.mag_B(2)) - _mag_sample_delayed.mag(2); - // XXX Do mag checks here! + // Note that although the observation jacobians and kalman gains are decalred as arrays + // sequential fusion of the X,Y and Z components is used. + float H_MAG[3][24] = {}; + float Kfusion[24] = {}; + + // Calculate observation Jacobians and kalman gains for each magentoemter axis + // X Axis + H_MAG[0][1] = SH_MAG[6] - magD * SH_MAG[2] - magN * SH_MAG[5]; + H_MAG[0][2] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); + H_MAG[0][16] = SH_MAG[1]; + H_MAG[0][17] = SH_MAG[4]; + H_MAG[0][18] = SH_MAG[7]; + H_MAG[0][19] = 1; + + // intermediate variables + float SK_MX[4] = {}; + // innovation variance + _mag_innov_var[0] = (P[19][19] + R_MAG - P[1][19] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][19] * SH_MAG[1] + + P[17][19] * SH_MAG[4] + P[18][19] * SH_MAG[7] + P[2][19] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[19][1] - P[1][1] * + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][1] * SH_MAG[1] + P[17][1] * SH_MAG[4] + P[18][1] * SH_MAG[7] + + P[2][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[1] * + (P[19][16] - P[1][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][16] * SH_MAG[1] + P[17][16] * + SH_MAG[4] + P[18][16] * SH_MAG[7] + P[2][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[4] * (P[19][17] - P[1][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + + P[16][17] * SH_MAG[1] + P[17][17] * SH_MAG[4] + P[18][17] * SH_MAG[7] + P[2][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] + - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[7] * (P[19][18] - P[1][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * + SH_MAG[5]) + P[16][18] * SH_MAG[1] + P[17][18] * SH_MAG[4] + P[18][18] * SH_MAG[7] + P[2][18] * + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + (magE * SH_MAG[0] + magD * SH_MAG[3] - + magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[19][2] - P[1][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][2] * + SH_MAG[1] + P[17][2] * SH_MAG[4] + P[18][2] * SH_MAG[7] + P[2][2] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)))); + + // check for a badly conditioned covariance matrix + if (_mag_innov_var[0] >= R_MAG) { + // the innovation variance contribution from the state covariances is non-negative - no fault + _fault_status.bad_mag_x = 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; + // we need to reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } - float H_MAG[24] = {}; - float Kfusion[24] = {}; + // Y axis - // index corresponds to a mag axis (x, y, z) - if (index == 0) { - H_MAG[1] = SH_MAG[6] - magD * SH_MAG[2] - magN * SH_MAG[5]; - H_MAG[2] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - H_MAG[16] = SH_MAG[1]; - H_MAG[17] = SH_MAG[4]; - H_MAG[18] = SH_MAG[7]; - H_MAG[19] = 1; + H_MAG[1][0] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; + H_MAG[1][2] = - magE * SH_MAG[4] - magD * SH_MAG[7] - magN * SH_MAG[1]; + H_MAG[1][16] = 2 * q1 * q2 - SH_MAG[8]; + H_MAG[1][17] = SH_MAG[0]; + H_MAG[1][18] = SH_MAG[3]; + H_MAG[1][20] = 1; - // intermediate variables - float SK_MX[4] = {}; - SK_MX[0] = 1 / (P[19][19] + R_MAG - P[1][19] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][19] * SH_MAG[1] - + P[17][19] * SH_MAG[4] + P[18][19] * SH_MAG[7] + P[2][19] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[19][1] - P[1][1] * - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][1] * SH_MAG[1] + P[17][1] * SH_MAG[4] + P[18][1] * SH_MAG[7] + - P[2][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[1] * - (P[19][16] - P[1][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][16] * SH_MAG[1] + P[17][16] * - SH_MAG[4] + P[18][16] * SH_MAG[7] + P[2][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[4] * (P[19][17] - P[1][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + - P[16][17] * SH_MAG[1] + P[17][17] * SH_MAG[4] + P[18][17] * SH_MAG[7] + P[2][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - - magN * (SH_MAG[8] - 2 * q1 * q2))) + SH_MAG[7] * (P[19][18] - P[1][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * - SH_MAG[5]) + P[16][18] * SH_MAG[1] + P[17][18] * SH_MAG[4] + P[18][18] * SH_MAG[7] + P[2][18] * - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2))) + (magE * SH_MAG[0] + magD * SH_MAG[3] - - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[19][2] - P[1][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[16][2] * - SH_MAG[1] + P[17][2] * SH_MAG[4] + P[18][2] * SH_MAG[7] + P[2][2] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)))); - SK_MX[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - SK_MX[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - SK_MX[3] = SH_MAG[7]; + // intermediate variables - note SK_MY[0] is 1/(innovation variance) + float SK_MY[4]; + _mag_innov_var[1] = (P[20][20] + R_MAG + P[0][20] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][20] * SH_MAG[0] + + P[18][20] * SH_MAG[3] - (SH_MAG[8] - 2 * q1 * q2) * (P[20][16] + P[0][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * + SH_MAG[5]) + P[17][16] * SH_MAG[0] + P[18][16] * SH_MAG[3] - P[2][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * + SH_MAG[1]) - P[16][16] * (SH_MAG[8] - 2 * q1 * q2)) - P[2][20] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * + SH_MAG[1]) + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[20][0] + P[0][0] * + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][0] * SH_MAG[0] + P[18][0] * SH_MAG[3] - P[2][0] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][0] * (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[0] * + (P[20][17] + P[0][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][17] * SH_MAG[0] + P[18][17] * + SH_MAG[3] - P[2][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][17] * + (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[3] * (P[20][18] + P[0][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + + P[17][18] * SH_MAG[0] + P[18][18] * SH_MAG[3] - P[2][18] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - + P[16][18] * (SH_MAG[8] - 2 * q1 * q2)) - P[16][20] * (SH_MAG[8] - 2 * q1 * q2) - (magE * SH_MAG[4] + magD * SH_MAG[7] + + magN * SH_MAG[1]) * (P[20][2] + P[0][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][2] * SH_MAG[0] + + P[18][2] * SH_MAG[3] - P[2][2] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][2] * + (SH_MAG[8] - 2 * q1 * q2))); - _mag_innov_var[0] = 1 / SK_MX[0]; - - Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][16] * SH_MAG[1] + P[0][17] * SH_MAG[4] - P[0][1] * SK_MX[2] + P[0][2] * - SK_MX[1] + P[0][18] * SK_MX[3]); - Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][16] * SH_MAG[1] + P[1][17] * SH_MAG[4] - P[1][1] * SK_MX[2] + P[1][2] * - SK_MX[1] + P[1][18] * SK_MX[3]); - Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][16] * SH_MAG[1] + P[2][17] * SH_MAG[4] - P[2][1] * SK_MX[2] + P[2][2] * - SK_MX[1] + P[2][18] * SK_MX[3]); - Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][16] * SH_MAG[1] + P[3][17] * SH_MAG[4] - P[3][1] * SK_MX[2] + P[3][2] * - SK_MX[1] + P[3][18] * SK_MX[3]); - Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][16] * SH_MAG[1] + P[4][17] * SH_MAG[4] - P[4][1] * SK_MX[2] + P[4][2] * - SK_MX[1] + P[4][18] * SK_MX[3]); - Kfusion[5] = SK_MX[0] * (P[5][19] + P[5][16] * SH_MAG[1] + P[5][17] * SH_MAG[4] - P[5][1] * SK_MX[2] + P[5][2] * - SK_MX[1] + P[5][18] * SK_MX[3]); - Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][16] * SH_MAG[1] + P[6][17] * SH_MAG[4] - P[6][1] * SK_MX[2] + P[6][2] * - SK_MX[1] + P[6][18] * SK_MX[3]); - Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][16] * SH_MAG[1] + P[7][17] * SH_MAG[4] - P[7][1] * SK_MX[2] + P[7][2] * - SK_MX[1] + P[7][18] * SK_MX[3]); - Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][16] * SH_MAG[1] + P[8][17] * SH_MAG[4] - P[8][1] * SK_MX[2] + P[8][2] * - SK_MX[1] + P[8][18] * SK_MX[3]); - Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][16] * SH_MAG[1] + P[9][17] * SH_MAG[4] - P[9][1] * SK_MX[2] + P[9][2] * - SK_MX[1] + P[9][18] * SK_MX[3]); - Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][16] * SH_MAG[1] + P[10][17] * SH_MAG[4] - P[10][1] * SK_MX[2] + P[10][2] * - SK_MX[1] + P[10][18] * SK_MX[3]); - Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][16] * SH_MAG[1] + P[11][17] * SH_MAG[4] - P[11][1] * SK_MX[2] + P[11][2] * - SK_MX[1] + P[11][18] * SK_MX[3]); - Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][16] * SH_MAG[1] + P[12][17] * SH_MAG[4] - P[12][1] * SK_MX[2] + P[12][2] * - SK_MX[1] + P[12][18] * SK_MX[3]); - Kfusion[13] = SK_MX[0] * (P[13][19] + P[13][16] * SH_MAG[1] + P[13][17] * SH_MAG[4] - P[13][1] * SK_MX[2] + P[13][2] * - SK_MX[1] + P[13][18] * SK_MX[3]); - Kfusion[14] = SK_MX[0] * (P[14][19] + P[14][16] * SH_MAG[1] + P[14][17] * SH_MAG[4] - P[14][1] * SK_MX[2] + P[14][2] * - SK_MX[1] + P[14][18] * SK_MX[3]); - Kfusion[15] = SK_MX[0] * (P[15][19] + P[15][16] * SH_MAG[1] + P[15][17] * SH_MAG[4] - P[15][1] * SK_MX[2] + P[15][2] * - SK_MX[1] + P[15][18] * SK_MX[3]); - Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][16] * SH_MAG[1] + P[16][17] * SH_MAG[4] - P[16][1] * SK_MX[2] + P[16][2] * - SK_MX[1] + P[16][18] * SK_MX[3]); - Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][16] * SH_MAG[1] + P[17][17] * SH_MAG[4] - P[17][1] * SK_MX[2] + P[17][2] * - SK_MX[1] + P[17][18] * SK_MX[3]); - Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][16] * SH_MAG[1] + P[18][17] * SH_MAG[4] - P[18][1] * SK_MX[2] + P[18][2] * - SK_MX[1] + P[18][18] * SK_MX[3]); - Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][16] * SH_MAG[1] + P[19][17] * SH_MAG[4] - P[19][1] * SK_MX[2] + P[19][2] * - SK_MX[1] + P[19][18] * SK_MX[3]); - Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][16] * SH_MAG[1] + P[20][17] * SH_MAG[4] - P[20][1] * SK_MX[2] + P[20][2] * - SK_MX[1] + P[20][18] * SK_MX[3]); - Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] * - SK_MX[1] + P[21][18] * SK_MX[3]); - Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] * - SK_MX[1] + P[22][18] * SK_MX[3]); - Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] * - SK_MX[1] + P[23][18] * SK_MX[3]); - - } else if (index == 1) { - - H_MAG[0] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - H_MAG[2] = - magE * SH_MAG[4] - magD * SH_MAG[7] - magN * SH_MAG[1]; - H_MAG[16] = 2 * q1 * q2 - SH_MAG[8]; - H_MAG[17] = SH_MAG[0]; - H_MAG[18] = SH_MAG[3]; - H_MAG[20] = 1; - - // intermediate variables - note SK_MY[0] is 1/(innovation variance) - float SK_MY[4]; - SK_MY[0] = 1 / (P[20][20] + R_MAG + P[0][20] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][20] * SH_MAG[0] - + P[18][20] * SH_MAG[3] - (SH_MAG[8] - 2 * q1 * q2) * (P[20][16] + P[0][16] * (magD * SH_MAG[2] - SH_MAG[6] + magN * - SH_MAG[5]) + P[17][16] * SH_MAG[0] + P[18][16] * SH_MAG[3] - P[2][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * - SH_MAG[1]) - P[16][16] * (SH_MAG[8] - 2 * q1 * q2)) - P[2][20] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * - SH_MAG[1]) + (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) * (P[20][0] + P[0][0] * - (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][0] * SH_MAG[0] + P[18][0] * SH_MAG[3] - P[2][0] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][0] * (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[0] * - (P[20][17] + P[0][17] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][17] * SH_MAG[0] + P[18][17] * - SH_MAG[3] - P[2][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][17] * - (SH_MAG[8] - 2 * q1 * q2)) + SH_MAG[3] * (P[20][18] + P[0][18] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + - P[17][18] * SH_MAG[0] + P[18][18] * SH_MAG[3] - P[2][18] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - - P[16][18] * (SH_MAG[8] - 2 * q1 * q2)) - P[16][20] * (SH_MAG[8] - 2 * q1 * q2) - (magE * SH_MAG[4] + magD * SH_MAG[7] + - magN * SH_MAG[1]) * (P[20][2] + P[0][2] * (magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]) + P[17][2] * SH_MAG[0] + - P[18][2] * SH_MAG[3] - P[2][2] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[16][2] * - (SH_MAG[8] - 2 * q1 * q2))); - SK_MY[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - SK_MY[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; - SK_MY[3] = SH_MAG[8] - 2 * q1 * q2; - - _mag_innov_var[1] = 1 / SK_MY[0]; - - Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][17] * SH_MAG[0] + P[0][18] * SH_MAG[3] + P[0][0] * SK_MY[2] - P[0][2] * - SK_MY[1] - P[0][16] * SK_MY[3]); - Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][17] * SH_MAG[0] + P[1][18] * SH_MAG[3] + P[1][0] * SK_MY[2] - P[1][2] * - SK_MY[1] - P[1][16] * SK_MY[3]); - Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][17] * SH_MAG[0] + P[2][18] * SH_MAG[3] + P[2][0] * SK_MY[2] - P[2][2] * - SK_MY[1] - P[2][16] * SK_MY[3]); - Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][17] * SH_MAG[0] + P[3][18] * SH_MAG[3] + P[3][0] * SK_MY[2] - P[3][2] * - SK_MY[1] - P[3][16] * SK_MY[3]); - Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][17] * SH_MAG[0] + P[4][18] * SH_MAG[3] + P[4][0] * SK_MY[2] - P[4][2] * - SK_MY[1] - P[4][16] * SK_MY[3]); - Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][17] * SH_MAG[0] + P[5][18] * SH_MAG[3] + P[5][0] * SK_MY[2] - P[5][2] * - SK_MY[1] - P[5][16] * SK_MY[3]); - Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][17] * SH_MAG[0] + P[6][18] * SH_MAG[3] + P[6][0] * SK_MY[2] - P[6][2] * - SK_MY[1] - P[6][16] * SK_MY[3]); - Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][17] * SH_MAG[0] + P[7][18] * SH_MAG[3] + P[7][0] * SK_MY[2] - P[7][2] * - SK_MY[1] - P[7][16] * SK_MY[3]); - Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][17] * SH_MAG[0] + P[8][18] * SH_MAG[3] + P[8][0] * SK_MY[2] - P[8][2] * - SK_MY[1] - P[8][16] * SK_MY[3]); - Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][17] * SH_MAG[0] + P[9][18] * SH_MAG[3] + P[9][0] * SK_MY[2] - P[9][2] * - SK_MY[1] - P[9][16] * SK_MY[3]); - Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][17] * SH_MAG[0] + P[10][18] * SH_MAG[3] + P[10][0] * SK_MY[2] - P[10][2] * - SK_MY[1] - P[10][16] * SK_MY[3]); - Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][17] * SH_MAG[0] + P[11][18] * SH_MAG[3] + P[11][0] * SK_MY[2] - P[11][2] * - SK_MY[1] - P[11][16] * SK_MY[3]); - Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][17] * SH_MAG[0] + P[12][18] * SH_MAG[3] + P[12][0] * SK_MY[2] - P[12][2] * - SK_MY[1] - P[12][16] * SK_MY[3]); - Kfusion[13] = SK_MY[0] * (P[13][20] + P[13][17] * SH_MAG[0] + P[13][18] * SH_MAG[3] + P[13][0] * SK_MY[2] - P[13][2] * - SK_MY[1] - P[13][16] * SK_MY[3]); - Kfusion[14] = SK_MY[0] * (P[14][20] + P[14][17] * SH_MAG[0] + P[14][18] * SH_MAG[3] + P[14][0] * SK_MY[2] - P[14][2] * - SK_MY[1] - P[14][16] * SK_MY[3]); - Kfusion[15] = SK_MY[0] * (P[15][20] + P[15][17] * SH_MAG[0] + P[15][18] * SH_MAG[3] + P[15][0] * SK_MY[2] - P[15][2] * - SK_MY[1] - P[15][16] * SK_MY[3]); - Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][17] * SH_MAG[0] + P[16][18] * SH_MAG[3] + P[16][0] * SK_MY[2] - P[16][2] * - SK_MY[1] - P[16][16] * SK_MY[3]); - Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][17] * SH_MAG[0] + P[17][18] * SH_MAG[3] + P[17][0] * SK_MY[2] - P[17][2] * - SK_MY[1] - P[17][16] * SK_MY[3]); - Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][17] * SH_MAG[0] + P[18][18] * SH_MAG[3] + P[18][0] * SK_MY[2] - P[18][2] * - SK_MY[1] - P[18][16] * SK_MY[3]); - Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][17] * SH_MAG[0] + P[19][18] * SH_MAG[3] + P[19][0] * SK_MY[2] - P[19][2] * - SK_MY[1] - P[19][16] * SK_MY[3]); - Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][17] * SH_MAG[0] + P[20][18] * SH_MAG[3] + P[20][0] * SK_MY[2] - P[20][2] * - SK_MY[1] - P[20][16] * SK_MY[3]); - Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] * - SK_MY[1] - P[21][16] * SK_MY[3]); - Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] * - SK_MY[1] - P[22][16] * SK_MY[3]); - Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] * - SK_MY[1] - P[23][16] * SK_MY[3]); + // check for a badly conditioned covariance matrix + if (_mag_innov_var[1] >= R_MAG) { + // the innovation variance contribution from the state covariances is non-negative - no fault + _fault_status.bad_mag_y = false; + } else { + // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned + _fault_status.bad_mag_y = true; + // we need to reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } - } else if (index == 2) + // Z axis - { - H_MAG[0] = magN * (SH_MAG[8] - 2 * q1 * q2) - magD * SH_MAG[3] - magE * SH_MAG[0]; - H_MAG[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - H_MAG[16] = SH_MAG[5]; - H_MAG[17] = 2 * q2 * q3 - 2 * q0 * q1; - H_MAG[18] = SH_MAG[2]; - H_MAG[21] = 1; + H_MAG[2][0] = magN * (SH_MAG[8] - 2 * q1 * q2) - magD * SH_MAG[3] - magE * SH_MAG[0]; + H_MAG[2][1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; + H_MAG[2][16] = SH_MAG[5]; + H_MAG[2][17] = 2 * q2 * q3 - 2 * q0 * q1; + H_MAG[2][18] = SH_MAG[2]; + H_MAG[2][21] = 1; - // intermediate variables - float SK_MZ[4]; - SK_MZ[0] = 1 / (P[21][21] + R_MAG + P[16][21] * SH_MAG[5] + P[18][21] * SH_MAG[2] - (2 * q0 * q1 - 2 * q2 * q3) * - (P[21][17] + P[16][17] * SH_MAG[5] + P[18][17] * SH_MAG[2] - P[0][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][17] * - (2 * q0 * q1 - 2 * q2 * q3)) - P[0][21] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][21] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) + SH_MAG[5] * - (P[21][16] + P[16][16] * SH_MAG[5] + P[18][16] * SH_MAG[2] - P[0][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][16] * - (2 * q0 * q1 - 2 * q2 * q3)) + SH_MAG[2] * (P[21][18] + P[16][18] * SH_MAG[5] + P[18][18] * SH_MAG[2] - P[0][18] * - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][18] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][18] * (2 * q0 * q1 - 2 * q2 * q3)) - - (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[21][0] + P[16][0] * SH_MAG[5] + P[18][0] * - SH_MAG[2] - P[0][0] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][0] * - (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][0] * (2 * q0 * q1 - 2 * q2 * q3)) - P[17][21] * - (2 * q0 * q1 - 2 * q2 * q3) + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) * - (P[21][1] + P[16][1] * SH_MAG[5] + P[18][1] * SH_MAG[2] - P[0][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * - (SH_MAG[8] - 2 * q1 * q2)) + P[1][1] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][1] * - (2 * q0 * q1 - 2 * q2 * q3))); - SK_MZ[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); - SK_MZ[2] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; - SK_MZ[3] = 2 * q0 * q1 - 2 * q2 * q3; + // intermediate variables + float SK_MZ[4]; + _mag_innov_var[2] = (P[21][21] + R_MAG + P[16][21] * SH_MAG[5] + P[18][21] * SH_MAG[2] - (2 * q0 * q1 - 2 * q2 * q3) * + (P[21][17] + P[16][17] * SH_MAG[5] + P[18][17] * SH_MAG[2] - P[0][17] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][17] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][17] * + (2 * q0 * q1 - 2 * q2 * q3)) - P[0][21] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][21] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) + SH_MAG[5] * + (P[21][16] + P[16][16] * SH_MAG[5] + P[18][16] * SH_MAG[2] - P[0][16] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][16] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][16] * + (2 * q0 * q1 - 2 * q2 * q3)) + SH_MAG[2] * (P[21][18] + P[16][18] * SH_MAG[5] + P[18][18] * SH_MAG[2] - P[0][18] * + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][18] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][18] * (2 * q0 * q1 - 2 * q2 * q3)) - + (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) * (P[21][0] + P[16][0] * SH_MAG[5] + P[18][0] * + SH_MAG[2] - P[0][0] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2)) + P[1][0] * + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][0] * (2 * q0 * q1 - 2 * q2 * q3)) - P[17][21] * + (2 * q0 * q1 - 2 * q2 * q3) + (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) * + (P[21][1] + P[16][1] * SH_MAG[5] + P[18][1] * SH_MAG[2] - P[0][1] * (magE * SH_MAG[0] + magD * SH_MAG[3] - magN * + (SH_MAG[8] - 2 * q1 * q2)) + P[1][1] * (magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]) - P[17][1] * + (2 * q0 * q1 - 2 * q2 * q3))); - _mag_innov_var[2] = 1 / SK_MZ[0]; - - Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][18] * SH_MAG[2] + P[0][16] * SH_MAG[5] - P[0][0] * SK_MZ[1] + P[0][1] * - SK_MZ[2] - P[0][17] * SK_MZ[3]); - Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][18] * SH_MAG[2] + P[1][16] * SH_MAG[5] - P[1][0] * SK_MZ[1] + P[1][1] * - SK_MZ[2] - P[1][17] * SK_MZ[3]); - Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][18] * SH_MAG[2] + P[2][16] * SH_MAG[5] - P[2][0] * SK_MZ[1] + P[2][1] * - SK_MZ[2] - P[2][17] * SK_MZ[3]); - Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][18] * SH_MAG[2] + P[3][16] * SH_MAG[5] - P[3][0] * SK_MZ[1] + P[3][1] * - SK_MZ[2] - P[3][17] * SK_MZ[3]); - Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][18] * SH_MAG[2] + P[4][16] * SH_MAG[5] - P[4][0] * SK_MZ[1] + P[4][1] * - SK_MZ[2] - P[4][17] * SK_MZ[3]); - Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][18] * SH_MAG[2] + P[5][16] * SH_MAG[5] - P[5][0] * SK_MZ[1] + P[5][1] * - SK_MZ[2] - P[5][17] * SK_MZ[3]); - Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][18] * SH_MAG[2] + P[6][16] * SH_MAG[5] - P[6][0] * SK_MZ[1] + P[6][1] * - SK_MZ[2] - P[6][17] * SK_MZ[3]); - Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][18] * SH_MAG[2] + P[7][16] * SH_MAG[5] - P[7][0] * SK_MZ[1] + P[7][1] * - SK_MZ[2] - P[7][17] * SK_MZ[3]); - Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][18] * SH_MAG[2] + P[8][16] * SH_MAG[5] - P[8][0] * SK_MZ[1] + P[8][1] * - SK_MZ[2] - P[8][17] * SK_MZ[3]); - Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][18] * SH_MAG[2] + P[9][16] * SH_MAG[5] - P[9][0] * SK_MZ[1] + P[9][1] * - SK_MZ[2] - P[9][17] * SK_MZ[3]); - Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][18] * SH_MAG[2] + P[10][16] * SH_MAG[5] - P[10][0] * SK_MZ[1] + P[10][1] * - SK_MZ[2] - P[10][17] * SK_MZ[3]); - Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][18] * SH_MAG[2] + P[11][16] * SH_MAG[5] - P[11][0] * SK_MZ[1] + P[11][1] * - SK_MZ[2] - P[11][17] * SK_MZ[3]); - Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][18] * SH_MAG[2] + P[12][16] * SH_MAG[5] - P[12][0] * SK_MZ[1] + P[12][1] * - SK_MZ[2] - P[12][17] * SK_MZ[3]); - Kfusion[13] = SK_MZ[0] * (P[13][21] + P[13][18] * SH_MAG[2] + P[13][16] * SH_MAG[5] - P[13][0] * SK_MZ[1] + P[13][1] * - SK_MZ[2] - P[13][17] * SK_MZ[3]); - Kfusion[14] = SK_MZ[0] * (P[14][21] + P[14][18] * SH_MAG[2] + P[14][16] * SH_MAG[5] - P[14][0] * SK_MZ[1] + P[14][1] * - SK_MZ[2] - P[14][17] * SK_MZ[3]); - Kfusion[15] = SK_MZ[0] * (P[15][21] + P[15][18] * SH_MAG[2] + P[15][16] * SH_MAG[5] - P[15][0] * SK_MZ[1] + P[15][1] * - SK_MZ[2] - P[15][17] * SK_MZ[3]); - Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][18] * SH_MAG[2] + P[16][16] * SH_MAG[5] - P[16][0] * SK_MZ[1] + P[16][1] * - SK_MZ[2] - P[16][17] * SK_MZ[3]); - Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][18] * SH_MAG[2] + P[17][16] * SH_MAG[5] - P[17][0] * SK_MZ[1] + P[17][1] * - SK_MZ[2] - P[17][17] * SK_MZ[3]); - Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][18] * SH_MAG[2] + P[18][16] * SH_MAG[5] - P[18][0] * SK_MZ[1] + P[18][1] * - SK_MZ[2] - P[18][17] * SK_MZ[3]); - Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][18] * SH_MAG[2] + P[19][16] * SH_MAG[5] - P[19][0] * SK_MZ[1] + P[19][1] * - SK_MZ[2] - P[19][17] * SK_MZ[3]); - Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][18] * SH_MAG[2] + P[20][16] * SH_MAG[5] - P[20][0] * SK_MZ[1] + P[20][1] * - SK_MZ[2] - P[20][17] * SK_MZ[3]); - Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] * - SK_MZ[2] - P[21][17] * SK_MZ[3]); - Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] * - SK_MZ[2] - P[22][17] * SK_MZ[3]); - Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] * - SK_MZ[2] - P[23][17] * SK_MZ[3]); - - } else { - } + // check for a badly conditioned covariance matrix + if (_mag_innov_var[2] >= R_MAG) { + // the innovation variance contribution from the state covariances is non-negative - no fault + _fault_status.bad_mag_z = false; + } else { + // the innovation variance contribution from the state covariances is negtive which means the covariance matrix is badly conditioned + _fault_status.bad_mag_z = true; + // we need to reinitialise the covariance matrix and abort this fusion step + initialiseCovariance(); + return; + } - // by definition our error state is zero at the time of fusion - _state.ang_error.setZero(); + // Perform an innovation consistency check on each measurement and if one axis fails + // do not fuse any data from the sensor because the most common errors affect multiple axes. + _mag_healthy = true; + for (uint8_t index=0; index<=2; index++) { + _mag_test_ratio[index] = sq(_mag_innov[index]) / (sq(math::max(_params.mag_innov_gate, 1.0f)) * _mag_innov_var[index]); + if (_mag_test_ratio[index] > 1.0f) { + _mag_healthy = false; + } + } + if (!_mag_healthy) { + return; + } - fuse(Kfusion, _mag_innov[index]); + // update the states and covariance usinng sequential fusion of the magnetometer components + for (uint8_t index=0; index<=2; index++) { + // Calculate Kalman gains + if (index == 0) { + // Calculate X axis Kalman gains + SK_MX[0] = 1.0f / _mag_innov_var[0]; + SK_MX[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); + SK_MX[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; + SK_MX[3] = SH_MAG[7]; - Quaternion q_correction; - q_correction.from_axis_angle(_state.ang_error); - _state.quat_nominal = q_correction * _state.quat_nominal; - _state.quat_nominal.normalize(); - _state.ang_error.setZero(); + Kfusion[0] = SK_MX[0] * (P[0][19] + P[0][16] * SH_MAG[1] + P[0][17] * SH_MAG[4] - P[0][1] * SK_MX[2] + P[0][2] * + SK_MX[1] + P[0][18] * SK_MX[3]); + Kfusion[1] = SK_MX[0] * (P[1][19] + P[1][16] * SH_MAG[1] + P[1][17] * SH_MAG[4] - P[1][1] * SK_MX[2] + P[1][2] * + SK_MX[1] + P[1][18] * SK_MX[3]); + Kfusion[2] = SK_MX[0] * (P[2][19] + P[2][16] * SH_MAG[1] + P[2][17] * SH_MAG[4] - P[2][1] * SK_MX[2] + P[2][2] * + SK_MX[1] + P[2][18] * SK_MX[3]); + Kfusion[3] = SK_MX[0] * (P[3][19] + P[3][16] * SH_MAG[1] + P[3][17] * SH_MAG[4] - P[3][1] * SK_MX[2] + P[3][2] * + SK_MX[1] + P[3][18] * SK_MX[3]); + Kfusion[4] = SK_MX[0] * (P[4][19] + P[4][16] * SH_MAG[1] + P[4][17] * SH_MAG[4] - P[4][1] * SK_MX[2] + P[4][2] * + SK_MX[1] + P[4][18] * SK_MX[3]); + Kfusion[5]= SK_MX[0] * (P[5][19] + P[5][16] * SH_MAG[1] + P[5][17] * SH_MAG[4] - P[5][1] * SK_MX[2] + P[5][2] * + SK_MX[1] + P[5][18] * SK_MX[3]); + Kfusion[6] = SK_MX[0] * (P[6][19] + P[6][16] * SH_MAG[1] + P[6][17] * SH_MAG[4] - P[6][1] * SK_MX[2] + P[6][2] * + SK_MX[1] + P[6][18] * SK_MX[3]); + Kfusion[7] = SK_MX[0] * (P[7][19] + P[7][16] * SH_MAG[1] + P[7][17] * SH_MAG[4] - P[7][1] * SK_MX[2] + P[7][2] * + SK_MX[1] + P[7][18] * SK_MX[3]); + Kfusion[8] = SK_MX[0] * (P[8][19] + P[8][16] * SH_MAG[1] + P[8][17] * SH_MAG[4] - P[8][1] * SK_MX[2] + P[8][2] * + SK_MX[1] + P[8][18] * SK_MX[3]); + Kfusion[9] = SK_MX[0] * (P[9][19] + P[9][16] * SH_MAG[1] + P[9][17] * SH_MAG[4] - P[9][1] * SK_MX[2] + P[9][2] * + SK_MX[1] + P[9][18] * SK_MX[3]); + Kfusion[10] = SK_MX[0] * (P[10][19] + P[10][16] * SH_MAG[1] + P[10][17] * SH_MAG[4] - P[10][1] * SK_MX[2] + P[10][2] * + SK_MX[1] + P[10][18] * SK_MX[3]); + Kfusion[11] = SK_MX[0] * (P[11][19] + P[11][16] * SH_MAG[1] + P[11][17] * SH_MAG[4] - P[11][1] * SK_MX[2] + P[11][2] * + SK_MX[1] + P[11][18] * SK_MX[3]); + Kfusion[12] = SK_MX[0] * (P[12][19] + P[12][16] * SH_MAG[1] + P[12][17] * SH_MAG[4] - P[12][1] * SK_MX[2] + P[12][2] * + SK_MX[1] + P[12][18] * SK_MX[3]); + Kfusion[13] = SK_MX[0] * (P[13][19] + P[13][16] * SH_MAG[1] + P[13][17] * SH_MAG[4] - P[13][1] * SK_MX[2] + P[13][2] * + SK_MX[1] + P[13][18] * SK_MX[3]); + Kfusion[14] = SK_MX[0] * (P[14][19] + P[14][16] * SH_MAG[1] + P[14][17] * SH_MAG[4] - P[14][1] * SK_MX[2] + P[14][2] * + SK_MX[1] + P[14][18] * SK_MX[3]); + Kfusion[15] = SK_MX[0] * (P[15][19] + P[15][16] * SH_MAG[1] + P[15][17] * SH_MAG[4] - P[15][1] * SK_MX[2] + P[15][2] * + SK_MX[1] + P[15][18] * SK_MX[3]); + Kfusion[16] = SK_MX[0] * (P[16][19] + P[16][16] * SH_MAG[1] + P[16][17] * SH_MAG[4] - P[16][1] * SK_MX[2] + P[16][2] * + SK_MX[1] + P[16][18] * SK_MX[3]); + Kfusion[17] = SK_MX[0] * (P[17][19] + P[17][16] * SH_MAG[1] + P[17][17] * SH_MAG[4] - P[17][1] * SK_MX[2] + P[17][2] * + SK_MX[1] + P[17][18] * SK_MX[3]); + Kfusion[18] = SK_MX[0] * (P[18][19] + P[18][16] * SH_MAG[1] + P[18][17] * SH_MAG[4] - P[18][1] * SK_MX[2] + P[18][2] * + SK_MX[1] + P[18][18] * SK_MX[3]); + Kfusion[19] = SK_MX[0] * (P[19][19] + P[19][16] * SH_MAG[1] + P[19][17] * SH_MAG[4] - P[19][1] * SK_MX[2] + P[19][2] * + SK_MX[1] + P[19][18] * SK_MX[3]); + Kfusion[20] = SK_MX[0] * (P[20][19] + P[20][16] * SH_MAG[1] + P[20][17] * SH_MAG[4] - P[20][1] * SK_MX[2] + P[20][2] * + SK_MX[1] + P[20][18] * SK_MX[3]); + Kfusion[21] = SK_MX[0] * (P[21][19] + P[21][16] * SH_MAG[1] + P[21][17] * SH_MAG[4] - P[21][1] * SK_MX[2] + P[21][2] * + SK_MX[1] + P[21][18] * SK_MX[3]); + Kfusion[22] = SK_MX[0] * (P[22][19] + P[22][16] * SH_MAG[1] + P[22][17] * SH_MAG[4] - P[22][1] * SK_MX[2] + P[22][2] * + SK_MX[1] + P[22][18] * SK_MX[3]); + Kfusion[23] = SK_MX[0] * (P[23][19] + P[23][16] * SH_MAG[1] + P[23][17] * SH_MAG[4] - P[23][1] * SK_MX[2] + P[23][2] * + SK_MX[1] + P[23][18] * SK_MX[3]); + } else if (index == 1) { + // Calculate Y axis Kalman gains + SK_MY[0] = 1.0f / _mag_innov_var[1]; + SK_MY[1] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; + SK_MY[2] = magD * SH_MAG[2] - SH_MAG[6] + magN * SH_MAG[5]; + SK_MY[3] = SH_MAG[8] - 2 * q1 * q2; - // apply covariance correction via P_new = (I -K*H)*P - // first calculate expression for KHP - // then calculate P - KHP - float KH[_k_num_states][_k_num_states] = {}; + Kfusion[0] = SK_MY[0] * (P[0][20] + P[0][17] * SH_MAG[0] + P[0][18] * SH_MAG[3] + P[0][0] * SK_MY[2] - P[0][2] * + SK_MY[1] - P[0][16] * SK_MY[3]); + Kfusion[1] = SK_MY[0] * (P[1][20] + P[1][17] * SH_MAG[0] + P[1][18] * SH_MAG[3] + P[1][0] * SK_MY[2] - P[1][2] * + SK_MY[1] - P[1][16] * SK_MY[3]); + Kfusion[2] = SK_MY[0] * (P[2][20] + P[2][17] * SH_MAG[0] + P[2][18] * SH_MAG[3] + P[2][0] * SK_MY[2] - P[2][2] * + SK_MY[1] - P[2][16] * SK_MY[3]); + Kfusion[3] = SK_MY[0] * (P[3][20] + P[3][17] * SH_MAG[0] + P[3][18] * SH_MAG[3] + P[3][0] * SK_MY[2] - P[3][2] * + SK_MY[1] - P[3][16] * SK_MY[3]); + Kfusion[4] = SK_MY[0] * (P[4][20] + P[4][17] * SH_MAG[0] + P[4][18] * SH_MAG[3] + P[4][0] * SK_MY[2] - P[4][2] * + SK_MY[1] - P[4][16] * SK_MY[3]); + Kfusion[5] = SK_MY[0] * (P[5][20] + P[5][17] * SH_MAG[0] + P[5][18] * SH_MAG[3] + P[5][0] * SK_MY[2] - P[5][2] * + SK_MY[1] - P[5][16] * SK_MY[3]); + Kfusion[6] = SK_MY[0] * (P[6][20] + P[6][17] * SH_MAG[0] + P[6][18] * SH_MAG[3] + P[6][0] * SK_MY[2] - P[6][2] * + SK_MY[1] - P[6][16] * SK_MY[3]); + Kfusion[7] = SK_MY[0] * (P[7][20] + P[7][17] * SH_MAG[0] + P[7][18] * SH_MAG[3] + P[7][0] * SK_MY[2] - P[7][2] * + SK_MY[1] - P[7][16] * SK_MY[3]); + Kfusion[8] = SK_MY[0] * (P[8][20] + P[8][17] * SH_MAG[0] + P[8][18] * SH_MAG[3] + P[8][0] * SK_MY[2] - P[8][2] * + SK_MY[1] - P[8][16] * SK_MY[3]); + Kfusion[9] = SK_MY[0] * (P[9][20] + P[9][17] * SH_MAG[0] + P[9][18] * SH_MAG[3] + P[9][0] * SK_MY[2] - P[9][2] * + SK_MY[1] - P[9][16] * SK_MY[3]); + Kfusion[10] = SK_MY[0] * (P[10][20] + P[10][17] * SH_MAG[0] + P[10][18] * SH_MAG[3] + P[10][0] * SK_MY[2] - P[10][2] * + SK_MY[1] - P[10][16] * SK_MY[3]); + Kfusion[11] = SK_MY[0] * (P[11][20] + P[11][17] * SH_MAG[0] + P[11][18] * SH_MAG[3] + P[11][0] * SK_MY[2] - P[11][2] * + SK_MY[1] - P[11][16] * SK_MY[3]); + Kfusion[12] = SK_MY[0] * (P[12][20] + P[12][17] * SH_MAG[0] + P[12][18] * SH_MAG[3] + P[12][0] * SK_MY[2] - P[12][2] * + SK_MY[1] - P[12][16] * SK_MY[3]); + Kfusion[13] = SK_MY[0] * (P[13][20] + P[13][17] * SH_MAG[0] + P[13][18] * SH_MAG[3] + P[13][0] * SK_MY[2] - P[13][2] * + SK_MY[1] - P[13][16] * SK_MY[3]); + Kfusion[14] = SK_MY[0] * (P[14][20] + P[14][17] * SH_MAG[0] + P[14][18] * SH_MAG[3] + P[14][0] * SK_MY[2] - P[14][2] * + SK_MY[1] - P[14][16] * SK_MY[3]); + Kfusion[15] = SK_MY[0] * (P[15][20] + P[15][17] * SH_MAG[0] + P[15][18] * SH_MAG[3] + P[15][0] * SK_MY[2] - P[15][2] * + SK_MY[1] - P[15][16] * SK_MY[3]); + Kfusion[16] = SK_MY[0] * (P[16][20] + P[16][17] * SH_MAG[0] + P[16][18] * SH_MAG[3] + P[16][0] * SK_MY[2] - P[16][2] * + SK_MY[1] - P[16][16] * SK_MY[3]); + Kfusion[17] = SK_MY[0] * (P[17][20] + P[17][17] * SH_MAG[0] + P[17][18] * SH_MAG[3] + P[17][0] * SK_MY[2] - P[17][2] * + SK_MY[1] - P[17][16] * SK_MY[3]); + Kfusion[18] = SK_MY[0] * (P[18][20] + P[18][17] * SH_MAG[0] + P[18][18] * SH_MAG[3] + P[18][0] * SK_MY[2] - P[18][2] * + SK_MY[1] - P[18][16] * SK_MY[3]); + Kfusion[19] = SK_MY[0] * (P[19][20] + P[19][17] * SH_MAG[0] + P[19][18] * SH_MAG[3] + P[19][0] * SK_MY[2] - P[19][2] * + SK_MY[1] - P[19][16] * SK_MY[3]); + Kfusion[20] = SK_MY[0] * (P[20][20] + P[20][17] * SH_MAG[0] + P[20][18] * SH_MAG[3] + P[20][0] * SK_MY[2] - P[20][2] * + SK_MY[1] - P[20][16] * SK_MY[3]); + Kfusion[21] = SK_MY[0] * (P[21][20] + P[21][17] * SH_MAG[0] + P[21][18] * SH_MAG[3] + P[21][0] * SK_MY[2] - P[21][2] * + SK_MY[1] - P[21][16] * SK_MY[3]); + Kfusion[22] = SK_MY[0] * (P[22][20] + P[22][17] * SH_MAG[0] + P[22][18] * SH_MAG[3] + P[22][0] * SK_MY[2] - P[22][2] * + SK_MY[1] - P[22][16] * SK_MY[3]); + Kfusion[23] = SK_MY[0] * (P[23][20] + P[23][17] * SH_MAG[0] + P[23][18] * SH_MAG[3] + P[23][0] * SK_MY[2] - P[23][2] * + SK_MY[1] - P[23][16] * SK_MY[3]); + } else if (index == 2) { + // Calculate Z axis Kalman gains + SK_MZ[0] = 1.0f / _mag_innov_var[2]; + SK_MZ[1] = magE * SH_MAG[0] + magD * SH_MAG[3] - magN * (SH_MAG[8] - 2 * q1 * q2); + SK_MZ[2] = magE * SH_MAG[4] + magD * SH_MAG[7] + magN * SH_MAG[1]; + SK_MZ[3] = 2 * q0 * q1 - 2 * q2 * q3; - for (unsigned row = 0; row < _k_num_states; row++) { - for (unsigned column = 0; column < 3; column++) { - KH[row][column] = Kfusion[row] * H_MAG[column]; - } + Kfusion[0] = SK_MZ[0] * (P[0][21] + P[0][18] * SH_MAG[2] + P[0][16] * SH_MAG[5] - P[0][0] * SK_MZ[1] + P[0][1] * + SK_MZ[2] - P[0][17] * SK_MZ[3]); + Kfusion[1] = SK_MZ[0] * (P[1][21] + P[1][18] * SH_MAG[2] + P[1][16] * SH_MAG[5] - P[1][0] * SK_MZ[1] + P[1][1] * + SK_MZ[2] - P[1][17] * SK_MZ[3]); + Kfusion[2] = SK_MZ[0] * (P[2][21] + P[2][18] * SH_MAG[2] + P[2][16] * SH_MAG[5] - P[2][0] * SK_MZ[1] + P[2][1] * + SK_MZ[2] - P[2][17] * SK_MZ[3]); + Kfusion[3] = SK_MZ[0] * (P[3][21] + P[3][18] * SH_MAG[2] + P[3][16] * SH_MAG[5] - P[3][0] * SK_MZ[1] + P[3][1] * + SK_MZ[2] - P[3][17] * SK_MZ[3]); + Kfusion[4] = SK_MZ[0] * (P[4][21] + P[4][18] * SH_MAG[2] + P[4][16] * SH_MAG[5] - P[4][0] * SK_MZ[1] + P[4][1] * + SK_MZ[2] - P[4][17] * SK_MZ[3]); + Kfusion[5] = SK_MZ[0] * (P[5][21] + P[5][18] * SH_MAG[2] + P[5][16] * SH_MAG[5] - P[5][0] * SK_MZ[1] + P[5][1] * + SK_MZ[2] - P[5][17] * SK_MZ[3]); + Kfusion[6] = SK_MZ[0] * (P[6][21] + P[6][18] * SH_MAG[2] + P[6][16] * SH_MAG[5] - P[6][0] * SK_MZ[1] + P[6][1] * + SK_MZ[2] - P[6][17] * SK_MZ[3]); + Kfusion[7] = SK_MZ[0] * (P[7][21] + P[7][18] * SH_MAG[2] + P[7][16] * SH_MAG[5] - P[7][0] * SK_MZ[1] + P[7][1] * + SK_MZ[2] - P[7][17] * SK_MZ[3]); + Kfusion[8] = SK_MZ[0] * (P[8][21] + P[8][18] * SH_MAG[2] + P[8][16] * SH_MAG[5] - P[8][0] * SK_MZ[1] + P[8][1] * + SK_MZ[2] - P[8][17] * SK_MZ[3]); + Kfusion[9] = SK_MZ[0] * (P[9][21] + P[9][18] * SH_MAG[2] + P[9][16] * SH_MAG[5] - P[9][0] * SK_MZ[1] + P[9][1] * + SK_MZ[2] - P[9][17] * SK_MZ[3]); + Kfusion[10] = SK_MZ[0] * (P[10][21] + P[10][18] * SH_MAG[2] + P[10][16] * SH_MAG[5] - P[10][0] * SK_MZ[1] + P[10][1] * + SK_MZ[2] - P[10][17] * SK_MZ[3]); + Kfusion[11] = SK_MZ[0] * (P[11][21] + P[11][18] * SH_MAG[2] + P[11][16] * SH_MAG[5] - P[11][0] * SK_MZ[1] + P[11][1] * + SK_MZ[2] - P[11][17] * SK_MZ[3]); + Kfusion[12] = SK_MZ[0] * (P[12][21] + P[12][18] * SH_MAG[2] + P[12][16] * SH_MAG[5] - P[12][0] * SK_MZ[1] + P[12][1] * + SK_MZ[2] - P[12][17] * SK_MZ[3]); + Kfusion[13] = SK_MZ[0] * (P[13][21] + P[13][18] * SH_MAG[2] + P[13][16] * SH_MAG[5] - P[13][0] * SK_MZ[1] + P[13][1] * + SK_MZ[2] - P[13][17] * SK_MZ[3]); + Kfusion[14] = SK_MZ[0] * (P[14][21] + P[14][18] * SH_MAG[2] + P[14][16] * SH_MAG[5] - P[14][0] * SK_MZ[1] + P[14][1] * + SK_MZ[2] - P[14][17] * SK_MZ[3]); + Kfusion[15] = SK_MZ[0] * (P[15][21] + P[15][18] * SH_MAG[2] + P[15][16] * SH_MAG[5] - P[15][0] * SK_MZ[1] + P[15][1] * + SK_MZ[2] - P[15][17] * SK_MZ[3]); + Kfusion[16] = SK_MZ[0] * (P[16][21] + P[16][18] * SH_MAG[2] + P[16][16] * SH_MAG[5] - P[16][0] * SK_MZ[1] + P[16][1] * + SK_MZ[2] - P[16][17] * SK_MZ[3]); + Kfusion[17] = SK_MZ[0] * (P[17][21] + P[17][18] * SH_MAG[2] + P[17][16] * SH_MAG[5] - P[17][0] * SK_MZ[1] + P[17][1] * + SK_MZ[2] - P[17][17] * SK_MZ[3]); + Kfusion[18] = SK_MZ[0] * (P[18][21] + P[18][18] * SH_MAG[2] + P[18][16] * SH_MAG[5] - P[18][0] * SK_MZ[1] + P[18][1] * + SK_MZ[2] - P[18][17] * SK_MZ[3]); + Kfusion[19] = SK_MZ[0] * (P[19][21] + P[19][18] * SH_MAG[2] + P[19][16] * SH_MAG[5] - P[19][0] * SK_MZ[1] + P[19][1] * + SK_MZ[2] - P[19][17] * SK_MZ[3]); + Kfusion[20] = SK_MZ[0] * (P[20][21] + P[20][18] * SH_MAG[2] + P[20][16] * SH_MAG[5] - P[20][0] * SK_MZ[1] + P[20][1] * + SK_MZ[2] - P[20][17] * SK_MZ[3]); + Kfusion[21] = SK_MZ[0] * (P[21][21] + P[21][18] * SH_MAG[2] + P[21][16] * SH_MAG[5] - P[21][0] * SK_MZ[1] + P[21][1] * + SK_MZ[2] - P[21][17] * SK_MZ[3]); + Kfusion[22] = SK_MZ[0] * (P[22][21] + P[22][18] * SH_MAG[2] + P[22][16] * SH_MAG[5] - P[22][0] * SK_MZ[1] + P[22][1] * + SK_MZ[2] - P[22][17] * SK_MZ[3]); + Kfusion[23] = SK_MZ[0] * (P[23][21] + P[23][18] * SH_MAG[2] + P[23][16] * SH_MAG[5] - P[23][0] * SK_MZ[1] + P[23][1] * + SK_MZ[2] - P[23][17] * SK_MZ[3]); + } else { + return; + } - for (unsigned column = 16; column < 22; column++) { - KH[row][column] = Kfusion[row] * H_MAG[column]; - } + // by definition our error state is zero at the time of fusion + _state.ang_error.setZero(); - } + fuse(Kfusion, _mag_innov[index]); - float KHP[_k_num_states][_k_num_states] = {}; + Quaternion q_correction; + q_correction.from_axis_angle(_state.ang_error); + _state.quat_nominal = q_correction * _state.quat_nominal; + _state.quat_nominal.normalize(); + _state.ang_error.setZero(); - 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][1] * P[1][column]; - tmp += KH[row][2] * P[2][column]; - tmp += KH[row][16] * P[16][column]; - tmp += KH[row][17] * P[17][column]; - tmp += KH[row][18] * P[18][column]; - tmp += KH[row][19] * P[19][column]; - tmp += KH[row][20] * P[20][column]; - tmp += KH[row][21] * P[21][column]; - KHP[row][column] = tmp; - } - } + // apply covariance correction via P_new = (I -K*H)*P + // first calculate expression for KHP + // then calculate P - KHP + float KH[_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 < 3; column++) { + KH[row][column] = Kfusion[row] * H_MAG[index][column]; + } - makeSymmetrical(); - limitCov(); + for (unsigned column = 16; column < 22; column++) { + KH[row][column] = Kfusion[row] * H_MAG[index][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][1] * P[1][column]; + tmp += KH[row][2] * P[2][column]; + tmp += KH[row][16] * P[16][column]; + tmp += KH[row][17] * P[17][column]; + tmp += KH[row][18] * P[18][column]; + tmp += KH[row][19] * P[19][column]; + tmp += KH[row][20] * P[20][column]; + tmp += KH[row][21] * P[21][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]; + } + } + + makeSymmetrical(); + limitCov(); + } } void Ekf::fuseHeading() @@ -464,14 +522,14 @@ void Ekf::fuseHeading() } if (innovation_var >= R_mag) { - // variance has increased, no failure + // 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; } else { - // our innovation variance has decreased, our calculation is thus badly conditioned - _fault_status.bad_mag_x = true; + // 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; @@ -494,11 +552,10 @@ void Ekf::fuseHeading() } // innovation test ratio - float yaw_test_ratio = sq(innovation) / (sq(math::max(0.01f * (float)_params.heading_innov_gate, - 1.0f)) * innovation_var); + _yaw_test_ratio = sq(innovation) / (sq(math::max(_params.heading_innov_gate, 1.0f)) * innovation_var); // set the magnetometer unhealthy if the test fails - if (yaw_test_ratio > 1.0f) { + if (_yaw_test_ratio > 1.0f) { _mag_healthy = false; // if we are in air we don't want to fuse the measurement @@ -537,4 +594,4 @@ void Ekf::fuseHeading() makeSymmetrical(); limitCov(); -} \ No newline at end of file +}