diff --git a/EKF/airspeed_fusion.cpp b/EKF/airspeed_fusion.cpp index 49d9582f27..867b3934d8 100644 --- a/EKF/airspeed_fusion.cpp +++ b/EKF/airspeed_fusion.cpp @@ -222,7 +222,7 @@ void Ekf::fuseAirspeed() P = P - KHP; // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(Kfusion, _airspeed_innov); diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 8454caccb1..8645991b04 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -706,11 +706,11 @@ void Ekf::predictCovariance() // fix gross errors in the covariance matrix and ensure rows and // columns for un-used states are zero - fixCovarianceErrors(); + fixCovarianceErrors(false); } -void Ekf::fixCovarianceErrors() +void Ekf::fixCovarianceErrors(bool force_symmetry) { // 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 @@ -748,7 +748,9 @@ void Ekf::fixCovarianceErrors() } // force symmetry on the quaternion, velocity and position state covariances - P.makeRowColSymmetric<13>(0); + if (force_symmetry) { + P.makeRowColSymmetric<13>(0); + } // the following states are optional and are deactivated when not required // by ensuring the corresponding covariance matrix values are kept at zero @@ -821,7 +823,7 @@ void Ekf::fixCovarianceErrors() _fault_status.flags.bad_acc_bias = false; ECL_WARN_TIMESTAMPED("invalid accel bias - resetting covariance"); - } else { + } else if (force_symmetry) { // ensure the covariance values are symmetrical P.makeRowColSymmetric<3>(13); } @@ -843,8 +845,10 @@ void Ekf::fixCovarianceErrors() } // force symmetry - P.makeRowColSymmetric<3>(16); - P.makeRowColSymmetric<3>(19); + if (force_symmetry) { + P.makeRowColSymmetric<3>(16); + P.makeRowColSymmetric<3>(19); + } } @@ -859,7 +863,9 @@ void Ekf::fixCovarianceErrors() } // force symmetry - P.makeRowColSymmetric<2>(22); + if (force_symmetry) { + P.makeRowColSymmetric<2>(22); + } } } diff --git a/EKF/drag_fusion.cpp b/EKF/drag_fusion.cpp index 6e5b44d27b..362919e4fa 100644 --- a/EKF/drag_fusion.cpp +++ b/EKF/drag_fusion.cpp @@ -301,7 +301,7 @@ void Ekf::fuseDrag() } // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(Kfusion, _drag_innov[axis_index]); diff --git a/EKF/ekf.h b/EKF/ekf.h index c86c36e52c..40e12d099e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -634,7 +634,8 @@ private: void resetExtVisRotMat(); // limit the diagonal of the covariance matrix - void fixCovarianceErrors(); + // force symmetry when the argument is true + void fixCovarianceErrors(bool force_symmetry); // constrain the ekf states void constrainStates(); diff --git a/EKF/gps_yaw_fusion.cpp b/EKF/gps_yaw_fusion.cpp index 2300e86650..68d1b18337 100644 --- a/EKF/gps_yaw_fusion.cpp +++ b/EKF/gps_yaw_fusion.cpp @@ -281,7 +281,7 @@ void Ekf::fuseGpsAntYaw() } // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(Kfusion, _heading_innov); diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 88414a14a8..463ff61ba8 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -423,7 +423,7 @@ void Ekf::fuseMag() } // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(Kfusion, _mag_innov[index]); @@ -802,7 +802,7 @@ void Ekf::fuseHeading() } // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(Kfusion, _heading_innov); @@ -939,7 +939,7 @@ void Ekf::fuseDeclination(float decl_sigma) } // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(Kfusion, innovation); diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 59eecf5770..a91d17ba3f 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -501,7 +501,7 @@ void Ekf::fuseOptFlow() } // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(gain, _flow_innov[obs_index]); diff --git a/EKF/sideslip_fusion.cpp b/EKF/sideslip_fusion.cpp index 3a780adb09..a67ca17856 100644 --- a/EKF/sideslip_fusion.cpp +++ b/EKF/sideslip_fusion.cpp @@ -274,7 +274,7 @@ void Ekf::fuseSideslip() } // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(Kfusion, _beta_innov); diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index c78b6bc052..336d0430f4 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -186,7 +186,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o } // correct the covariance matrix for gross errors - fixCovarianceErrors(); + fixCovarianceErrors(true); // apply the state corrections fuse(Kfusion, innov);