From 5ea4187c4ebd77c6b01ee0508e43f08e031cebdc Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 23 Feb 2024 11:49:24 -0500 Subject: [PATCH] revert and cleanup --- src/lib/matrix/matrix/Matrix.hpp | 18 ----------- src/modules/ekf2/EKF/airspeed_fusion.cpp | 2 +- src/modules/ekf2/EKF/covariance.cpp | 24 --------------- src/modules/ekf2/EKF/drag_fusion.cpp | 2 +- src/modules/ekf2/EKF/ekf.h | 38 ++---------------------- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 2 +- src/modules/ekf2/EKF/gravity_fusion.cpp | 2 +- src/modules/ekf2/EKF/mag_fusion.cpp | 4 +-- src/modules/ekf2/EKF/optflow_fusion.cpp | 2 +- src/modules/ekf2/EKF/sideslip_fusion.cpp | 2 +- src/modules/ekf2/EKF/yaw_fusion.cpp | 2 +- 11 files changed, 11 insertions(+), 87 deletions(-) diff --git a/src/lib/matrix/matrix/Matrix.hpp b/src/lib/matrix/matrix/Matrix.hpp index 4ad1d627b29d..e3eecf2dd871 100644 --- a/src/lib/matrix/matrix/Matrix.hpp +++ b/src/lib/matrix/matrix/Matrix.hpp @@ -162,24 +162,6 @@ class Matrix return res; } - // Using this function reduces the number of temporary variables needed to compute A * B.T - template - Matrix multiplyByTranspose(const Matrix &other) const - { - Matrix res; - const Matrix &self = *this; - - for (size_t i = 0; i < M; i++) { - for (size_t k = 0; k < P; k++) { - for (size_t j = 0; j < N; j++) { - res(i, k) += self(i, j) * other(k, j); - } - } - } - - return res; - } - // Element-wise multiplication Matrix emult(const Matrix &other) const { diff --git a/src/modules/ekf2/EKF/airspeed_fusion.cpp b/src/modules/ekf2/EKF/airspeed_fusion.cpp index 35290d34e817..33985779c20c 100644 --- a/src/modules/ekf2/EKF/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/airspeed_fusion.cpp @@ -208,7 +208,7 @@ void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_sour K.slice(State::wind_vel.idx, 0) = K_wind; } - const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation, aid_src.innovation_variance); + const bool is_fused = measurementUpdate(K, H, aid_src.observation_variance, aid_src.innovation); aid_src.fused = is_fused; _fault_status.flags.bad_airspeed = !is_fused; diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index b95e1c171c24..b1a4fff58df0 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -222,14 +222,6 @@ void Ekf::constrainStateVariances() #endif // CONFIG_EKF2_WIND } -void Ekf::forceCovarianceSymmetry() -{ - // DEBUG - // P.isBlockSymmetric(0, 1e-9f); - - P.makeRowColSymmetric(0); -} - void Ekf::constrainStateVar(const IdxDof &state, float min, float max) { for (unsigned i = state.idx; i < (state.idx + state.dof); i++) { @@ -256,22 +248,6 @@ void Ekf::constrainStateVarLimitRatio(const IdxDof &state, float min, float max, } } -// if the covariance correction will result in a negative variance, then -// the covariance matrix is unhealthy and must be corrected -bool Ekf::checkAndFixCovarianceUpdate(const SquareMatrixState &KHP) -{ - bool healthy = true; - - for (int i = 0; i < State::size; i++) { - if (P(i, i) < KHP(i, i)) { - P.uncorrelateCovarianceSetVariance<1>(i, 0.f); - healthy = false; - } - } - - return healthy; -} - void Ekf::resetQuatCov(const float yaw_noise) { const float tilt_var = sq(math::max(_params.initial_tilt_err, 0.01f)); diff --git a/src/modules/ekf2/EKF/drag_fusion.cpp b/src/modules/ekf2/EKF/drag_fusion.cpp index 5b1544c6cc6f..5b4baedf123f 100644 --- a/src/modules/ekf2/EKF/drag_fusion.cpp +++ b/src/modules/ekf2/EKF/drag_fusion.cpp @@ -160,7 +160,7 @@ void Ekf::fuseDrag(const dragSample &drag_sample) VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index]; - if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index], _aid_src_drag.innovation_variance[axis_index])) { + if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) { fused[axis_index] = true; } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 4bb9a4962ed1..e98062623664 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -468,7 +468,7 @@ class Ekf final : public EstimatorInterface const auto &aid_src_aux_vel() const { return _aid_src_aux_vel; } #endif // CONFIG_EKF2_AUXVEL - bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation, const float innovation_variance) + bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation) { clearInhibitedStateKalmanGains(K); @@ -580,7 +580,6 @@ class Ekf final : public EstimatorInterface float _yaw_rate_lpf_ef{0.0f}; ///< Filtered angular rate about earth frame D axis (rad/sec) SquareMatrixState P{}; ///< state covariance matrix - SquareMatrixState _temp_square_matrix_state{}; ///< Used to pre-allocate a large matrix #if defined(CONFIG_EKF2_DRAG_FUSION) estimator_aid_source2d_s _aid_src_drag{}; @@ -910,28 +909,16 @@ class Ekf final : public EstimatorInterface float getMagDeclination(); #endif // CONFIG_EKF2_MAGNETOMETER - // returns true if the Kalman gains were modified - bool clearInhibitedStateKalmanGains(VectorState &K) const + void clearInhibitedStateKalmanGains(VectorState &K) const { - static constexpr float kMinKalmanGain = 1e-9f; - bool kalman_gains_modified = false; - for (unsigned i = 0; i < State::gyro_bias.dof; i++) { if (_gyro_bias_inhibit[i]) { - if (K(State::gyro_bias.idx + i) > kMinKalmanGain || K(State::gyro_bias.idx + i) < -kMinKalmanGain) { - kalman_gains_modified = true; - } - K(State::gyro_bias.idx + i) = 0.f; } } for (unsigned i = 0; i < State::accel_bias.dof; i++) { if (_accel_bias_inhibit[i]) { - if (K(State::accel_bias.idx + i) > kMinKalmanGain || K(State::accel_bias.idx + i) < -kMinKalmanGain) { - kalman_gains_modified = true; - } - K(State::accel_bias.idx + i) = 0.f; } } @@ -939,20 +926,12 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_MAGNETOMETER) if (!_control_status.flags.mag) { for (unsigned i = 0; i < State::mag_I.dof; i++) { - if (K(State::mag_I.idx + i) > kMinKalmanGain || K(State::mag_I.idx + i) < -kMinKalmanGain) { - kalman_gains_modified = true; - } - K(State::mag_I.idx + i) = 0.f; } } if (!_control_status.flags.mag) { for (unsigned i = 0; i < State::mag_B.dof; i++) { - if (K(State::mag_B.idx + i) > kMinKalmanGain || K(State::mag_B.idx + i) < -kMinKalmanGain) { - kalman_gains_modified = true; - } - K(State::mag_B.idx + i) = 0.f; } } @@ -961,28 +940,15 @@ class Ekf final : public EstimatorInterface #if defined(CONFIG_EKF2_WIND) if (!_control_status.flags.wind) { for (unsigned i = 0; i < State::wind_vel.dof; i++) { - - if (K(State::wind_vel.idx + i) > kMinKalmanGain || K(State::wind_vel.idx + i) < -kMinKalmanGain) { - kalman_gains_modified = true; - } - K(State::wind_vel.idx + i) = 0.f; } } #endif // CONFIG_EKF2_WIND - - return kalman_gains_modified; } - // if the covariance correction will result in a negative variance, then - // the covariance matrix is unhealthy and must be corrected - bool checkAndFixCovarianceUpdate(const SquareMatrixState &KHP); - // limit the diagonal of the covariance matrix void constrainStateVariances(); - void forceCovarianceSymmetry(); - void constrainStateVar(const IdxDof &state, float min, float max); void constrainStateVarLimitRatio(const IdxDof &state, float min, float max, float max_ratio = 1.e6f); diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index 0904c8ef51d9..13c057f26653 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -134,7 +134,7 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset) // only calculate gains for states we are using VectorState Kfusion = P * H / gnss_yaw.innovation_variance; - const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation, gnss_yaw.innovation_variance); + const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation); _fault_status.flags.bad_hdg = !is_fused; gnss_yaw.fused = is_fused; diff --git a/src/modules/ekf2/EKF/gravity_fusion.cpp b/src/modules/ekf2/EKF/gravity_fusion.cpp index 3792154c5010..70a0a8ded200 100644 --- a/src/modules/ekf2/EKF/gravity_fusion.cpp +++ b/src/modules/ekf2/EKF/gravity_fusion.cpp @@ -111,7 +111,7 @@ void Ekf::controlGravityFusion(const imuSample &imu) const bool accel_clipping = imu.delta_vel_clipping[0] || imu.delta_vel_clipping[1] || imu.delta_vel_clipping[2]; if (_control_status.flags.gravity_vector && !_aid_src_gravity.innovation_rejected && !accel_clipping) { - fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index], _aid_src_gravity.innovation_variance[index]); + fused[index] = measurementUpdate(K, H, _aid_src_gravity.observation_variance[index], _aid_src_gravity.innovation[index]); } } diff --git a/src/modules/ekf2/EKF/mag_fusion.cpp b/src/modules/ekf2/EKF/mag_fusion.cpp index 487234db8b60..b1a8cc68fe29 100644 --- a/src/modules/ekf2/EKF/mag_fusion.cpp +++ b/src/modules/ekf2/EKF/mag_fusion.cpp @@ -191,7 +191,7 @@ bool Ekf::fuseMag(const Vector3f &mag, estimator_aid_source3d_s &aid_src_mag, bo Kfusion.slice(State::mag_B.idx, 0) = K_mag_B; } - if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index], aid_src_mag.innovation_variance[index])) { + if (measurementUpdate(Kfusion, H, aid_src_mag.observation_variance[index], aid_src_mag.innovation[index])) { fused[index] = true; limitDeclination(); @@ -261,7 +261,7 @@ bool Ekf::fuseDeclination(float decl_sigma) // Calculate the Kalman gains VectorState Kfusion = P * H / innovation_variance; - const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation, innovation_variance); + const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation); _fault_status.flags.bad_mag_decl = !is_fused; diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index 405e1d66916b..5dd257c7251d 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -146,7 +146,7 @@ void Ekf::fuseOptFlow() VectorState Kfusion = P * H / _aid_src_optical_flow.innovation_variance[index]; - if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index], _aid_src_optical_flow.observation_variance[index])) { + if (measurementUpdate(Kfusion, H, _aid_src_optical_flow.observation_variance[index], _aid_src_optical_flow.innovation[index])) { fused[index] = true; } } diff --git a/src/modules/ekf2/EKF/sideslip_fusion.cpp b/src/modules/ekf2/EKF/sideslip_fusion.cpp index 5a85b3491d1d..c35501f241c9 100644 --- a/src/modules/ekf2/EKF/sideslip_fusion.cpp +++ b/src/modules/ekf2/EKF/sideslip_fusion.cpp @@ -142,7 +142,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip) K.slice(State::wind_vel.idx, 0) = K_wind; } - const bool is_fused = measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation, sideslip.innovation_variance); + const bool is_fused = measurementUpdate(K, H, sideslip.observation_variance, sideslip.innovation); sideslip.fused = is_fused; _fault_status.flags.bad_sideslip = !is_fused; diff --git a/src/modules/ekf2/EKF/yaw_fusion.cpp b/src/modules/ekf2/EKF/yaw_fusion.cpp index 2e5da9ee7366..e5f94938aa7b 100644 --- a/src/modules/ekf2/EKF/yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/yaw_fusion.cpp @@ -113,7 +113,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H _innov_check_fail_status.flags.reject_yaw = false; } - if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation, aid_src_status.innovation_variance)) { + if (measurementUpdate(Kfusion, H_YAW, aid_src_status.observation_variance, aid_src_status.innovation)) { _time_last_heading_fuse = _time_delayed_us;