diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 82e02bc74b..cf7946afb9 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -187,8 +187,7 @@ void Ekf::fuseAirspeed() } // correct the covariance marix for gross errors - makeSymmetrical(); - limitCov(); + fixCovarianceErrors(); // apply the state corrections fuse(Kfusion, _airspeed_innov); diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 948bed6ccd..3816e30997 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -691,29 +691,13 @@ void Ekf::predictCovariance() P[i][i] = nextP[i][i]; } - // prevent variances from becoming too large or negative - limitCov(); - - // ensure coresponding rows and zeros for unwanted states are zero - if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) { - // deactivate XYZ accel bias states - zeroRows(P,13,15); - zeroCols(P,13,15); - } - if (!_control_status.flags.mag_3D) { - // deactivate magnetic field states - zeroRows(P,16,21); - zeroCols(P,16,21); - } - if (!_control_status.flags.wind) { - // deactivate wind states - zeroRows(P,22,23); - zeroCols(P,22,23); - } + // fix gross errors in the covariance matrix and ensure rows and + // columns for un-used states are zero + fixCovarianceErrors(); } -void Ekf::limitCov() +void Ekf::fixCovarianceErrors() { // NOTE: This limiting is a last resort and should not be relied on // TODO: Split covariance prediction into separate F*P*transpose(F) and Q contributions @@ -731,43 +715,70 @@ void Ekf::limitCov() P_lim[7] = 1e6f; // wind max var for (int i = 0; i <= 3; i++) { - + // quaternion states P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[0]); } for (int i = 4; i <= 6; i++) { - + // NED velocity states P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[1]); } for (int i = 7; i <= 9; i++) { - - + // NED position states P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[2]); } for (int i = 10; i <= 12; i++) { - - + // gyro bias states P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[3]); } - for (int i = 13; i <= 15; i++) { + // force symmetry on the quaternion, velocity and positon state covariances + makeSymmetrical(P,0,12); + // the following states are optional and are deactivaed when not required + // by ensuring the corresponding covariance matrix values are kept at zero - P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]); + // accelerometer bias states + if ((_params.fusion_mode & MASK_INHIBIT_ACC_BIAS)) { + zeroRows(P,13,15); + zeroCols(P,13,15); + } else { + // constrain variances + for (int i = 13; i <= 15; i++) { + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[4]); + } + // force symmetry + makeSymmetrical(P,13,15); } - - for (int i = 16; i <= 18; i++) { - P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]); + // magnetic field states + if (!_control_status.flags.mag_3D) { + zeroRows(P,16,21); + zeroCols(P,16,21); + } else { + // constrain variances + for (int i = 16; i <= 18; i++) { + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[5]); + } + for (int i = 19; i <= 21; i++) { + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]); + } + // force symmetry + makeSymmetrical(P,16,21); } - for (int i = 19; i <= 21; i++) { - P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[6]); - } - - for (int i = 22; i <= 23; i++) { - P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]); + // wind velocity states + if (!_control_status.flags.wind) { + zeroRows(P,22,23); + zeroCols(P,22,23); + } else { + // constrain variances + for (int i = 22; i <= 23; i++) { + P[i][i] = math::constrain(P[i][i], 0.0f, P_lim[7]); + } + // force symmetry + makeSymmetrical(P,22,23); } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 8cb99797cb..335eb59796 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -307,13 +307,11 @@ private: // reset height state of the ekf void resetHeight(); - void makeCovSymetrical(); - // limit the diagonal of the covariance matrix - void limitCov(); + void fixCovarianceErrors(); - // make ekf covariance matrix symmetric - void makeSymmetrical(); + // make ekf covariance matrix symmetric between a nominated state indexe range + void makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); // constrain the ekf states void constrainStates(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 0aca1f2ff1..4e889a973e 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -292,13 +292,13 @@ void Ekf::calcMagDeclination() } // This function forces the covariance matrix to be symmetric -void Ekf::makeSymmetrical() +void Ekf::makeSymmetrical(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last) { - for (unsigned row = 0; row < _k_num_states; row++) { + for (unsigned row = first; row <= last; row++) { for (unsigned column = 0; column < row; column++) { - float tmp = (P[row][column] + P[column][row]) / 2; - P[row][column] = tmp; - P[column][row] = tmp; + float tmp = (cov_mat[row][column] + cov_mat[column][row]) / 2; + cov_mat[row][column] = tmp; + cov_mat[column][row] = tmp; } } } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 2d68dc0149..ced31afb01 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -362,8 +362,7 @@ void Ekf::fuseMag() } // correct the covariance marix for gross errors - makeSymmetrical(); - limitCov(); + fixCovarianceErrors(); // apply the state corrections fuse(Kfusion, _mag_innov[index]); @@ -637,8 +636,7 @@ void Ekf::fuseHeading() } // correct the covariance marix for gross errors - makeSymmetrical(); - limitCov(); + fixCovarianceErrors(); // apply the state corrections fuse(Kfusion, _heading_innov); @@ -772,8 +770,7 @@ void Ekf::fuseDeclination() } // correct the covariance marix for gross errors - makeSymmetrical(); - limitCov(); + fixCovarianceErrors(); // apply the state corrections fuse(Kfusion, innovation); diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index eadf3f7d65..cc9d398dd4 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -486,8 +486,7 @@ void Ekf::fuseOptFlow() } // correct the covariance marix for gross errors - makeSymmetrical(); - limitCov(); + fixCovarianceErrors(); // apply the state corrections fuse(gain, _flow_innov[obs_index]); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index bcdeda0980..f638647fdb 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -262,8 +262,7 @@ void Ekf::fuseVelPosHeight() } // correct the covariance marix for gross errors - makeSymmetrical(); - limitCov(); + fixCovarianceErrors(); // apply the state corrections fuse(Kfusion, _vel_pos_innov[obs_index]);