From 0d0f46ec1c7495dc37191537929e35ee947209fa Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 19 Dec 2019 16:43:48 +1100 Subject: [PATCH] EKF: Don't run unnecessary makeRowColSymmetric operation This operation is expensive when done to the whole covariance matrix and unnecessary after covariance prediction because we calculate the upper diagonal and copy across so it is already symmetric. --- EKF/airspeed_fusion.cpp | 2 +- EKF/covariance.cpp | 20 +++++++++++++------- EKF/drag_fusion.cpp | 2 +- EKF/ekf.h | 3 ++- EKF/gps_yaw_fusion.cpp | 2 +- EKF/mag_fusion.cpp | 6 +++--- EKF/optflow_fusion.cpp | 2 +- EKF/sideslip_fusion.cpp | 2 +- EKF/vel_pos_fusion.cpp | 2 +- 9 files changed, 24 insertions(+), 17 deletions(-) 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);