From 16fec19b2f03f7237747f73a8502c1a5992f2a09 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 11 Jul 2024 13:02:37 -0400 Subject: [PATCH] ekf2: add vel/pos/terr covariance reset helpers --- src/modules/ekf2/EKF/covariance.cpp | 43 ++---------------------- src/modules/ekf2/EKF/ekf.h | 7 ++++ src/modules/ekf2/EKF/position_fusion.cpp | 13 +++++++ src/modules/ekf2/EKF/terrain_control.cpp | 12 +++++-- src/modules/ekf2/EKF/velocity_fusion.cpp | 12 +++++++ 5 files changed, 45 insertions(+), 42 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 58e83b3bd7..d29995f81a 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -54,45 +54,9 @@ void Ekf::initialiseCovariance() P.zero(); resetQuatCov(0.f); // Start with no initial uncertainty to improve fine leveling through zero vel/pos fusion - - // velocity -#if defined(CONFIG_EKF2_GNSS) - const float vel_var = sq(fmaxf(_params.gps_vel_noise, 0.01f)); -#else - const float vel_var = sq(0.5f); -#endif - P.uncorrelateCovarianceSetVariance(State::vel.idx, Vector3f(vel_var, vel_var, sq(1.5f) * vel_var)); - - // position -#if defined(CONFIG_EKF2_BAROMETER) - float z_pos_var = sq(fmaxf(_params.baro_noise, 0.01f)); -#else - float z_pos_var = sq(1.f); -#endif // CONFIG_EKF2_BAROMETER - -#if defined(CONFIG_EKF2_GNSS) - const float xy_pos_var = sq(fmaxf(_params.gps_pos_noise, 0.01f)); - - if (_control_status.flags.gps_hgt) { - z_pos_var = sq(fmaxf(1.5f * _params.gps_pos_noise, 0.01f)); - } - -#else - const float xy_pos_var = sq(fmaxf(_params.pos_noaid_noise, 0.01f)); -#endif - -#if defined(CONFIG_EKF2_RANGE_FINDER) - - if (_control_status.flags.rng_hgt) { - z_pos_var = sq(fmaxf(_params.range_noise, 0.01f)); - } - -#endif // CONFIG_EKF2_RANGE_FINDER - - P.uncorrelateCovarianceSetVariance(State::pos.idx, Vector3f(xy_pos_var, xy_pos_var, z_pos_var)); - + resetVelocityCov(); + resetPositionCov(); resetGyroBiasCov(); - resetAccelBiasCov(); #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -104,8 +68,7 @@ void Ekf::initialiseCovariance() #endif // CONFIG_EKF2_WIND #if defined(CONFIG_EKF2_TERRAIN) - // use the ground clearance value as our uncertainty - P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); + resetTerrainCov(); #endif // CONFIG_EKF2_TERRAIN } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 85ea4a7625..4ff24068fd 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -110,6 +110,8 @@ public: // get the terrain variance float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); } + void resetTerrainCov(); + #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -796,12 +798,17 @@ private: void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var); + void resetVelocityCov(); + void resetHorizontalPositionToLastKnown(); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } + void resetPositionCov(); + + bool isHeightResetRequired() const; void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN); diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 6cc503de73..c135bc9a6d 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -181,3 +181,16 @@ void Ekf::resetHorizontalPositionToLastKnown() // Used when falling back to non-aiding mode of operation resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); } + +void Ekf::resetPositionCov() +{ + // reset position covariances + ECL_INFO("resetting position covariances"); + + float xy_var = sq(math::max(_params.pos_noaid_noise, 0.01f)); + float z_var = sq(1.f); + + Vector3f pos_var{xy_var, xy_var, z_var}; + + P.uncorrelateCovarianceSetVariance(State::pos.idx, pos_var); +} diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index 8f277abac6..e87ee7c947 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -45,8 +45,7 @@ void Ekf::initTerrain() // assume a ground clearance _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; - // use the ground clearance value as our uncertainty - P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); + resetTerrainCov(); } void Ekf::controlTerrainFakeFusion() @@ -101,3 +100,12 @@ bool Ekf::isTerrainEstimateValid() const return valid; } + +void Ekf::resetTerrainCov() +{ + // reset terrain covariances + + // use the ground clearance value as our uncertainty + float terrain_var = math::max(sq(_params.rng_gnd_clearance), sq(0.01f)); + P.uncorrelateCovarianceSetVariance(State::terrain.idx, terrain_var); +} diff --git a/src/modules/ekf2/EKF/velocity_fusion.cpp b/src/modules/ekf2/EKF/velocity_fusion.cpp index 349a40092f..36f22ee55c 100644 --- a/src/modules/ekf2/EKF/velocity_fusion.cpp +++ b/src/modules/ekf2/EKF/velocity_fusion.cpp @@ -156,3 +156,15 @@ void Ekf::resetVelocityTo(const Vector3f &new_vel, const Vector3f &new_vel_var) resetHorizontalVelocityTo(Vector2f(new_vel), Vector2f(new_vel_var(0), new_vel_var(1))); resetVerticalVelocityTo(new_vel(2), new_vel_var(2)); } + +void Ekf::resetVelocityCov() +{ + // reset velocity covariances + ECL_INFO("resetting velocity covariances"); + + float default_var = sq(1.f); // TODO: configurable? + + Vector3f vel_var{default_var, default_var, default_var}; + + P.uncorrelateCovarianceSetVariance(State::vel.idx, vel_var); +}