From fad9ae855d1450ccb7876c58c88a839c54994756 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 28 Nov 2024 14:20:31 +0100 Subject: [PATCH] EKF2: reset aid src when resetting state to measurement Filtered innovations and test ratios can be large before the reset and would trigger pre-flight warnings --- .../aux_global_position/aux_global_position.cpp | 1 + .../aid_sources/barometer/baro_height_control.cpp | 2 ++ .../EKF/aid_sources/gnss/gnss_height_control.cpp | 2 ++ .../range_finder/range_height_control.cpp | 6 ++++++ src/modules/ekf2/EKF/ekf.h | 15 +-------------- src/modules/ekf2/EKF/ekf_helper.cpp | 15 +++++++++++++++ src/modules/ekf2/test/test_EKF_height_fusion.cpp | 6 ++---- 7 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index 6aec6936b2..3307dd56b9 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -112,6 +112,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed // Try to initialize using measurement if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv))) { + ekf.resetAidSourceStatusZeroInnovation(aid_src); ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index c645afd9d5..119c6fa1b3 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -133,6 +133,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) _information_events.flags.reset_hgt_to_baro = true; resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var); bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); + resetAidSourceStatusZeroInnovation(aid_src); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { @@ -165,6 +166,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) _information_events.flags.reset_hgt_to_baro = true; initialiseAltitudeTo(measurement, measurement_var); bias_est.reset(); + resetAidSourceStatusZeroInnovation(aid_src); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index c936442b14..c81af9c581 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -107,6 +107,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) _information_events.flags.reset_hgt_to_gps = true; resetAltitudeTo(measurement, measurement_var); bias_est.setBias(-_gpos.altitude() + measurement); + resetAidSourceStatusZeroInnovation(aid_src); aid_src.time_last_fuse = _time_delayed_us; @@ -131,6 +132,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) initialiseAltitudeTo(measurement, measurement_var); bias_est.reset(); + resetAidSourceStatusZeroInnovation(aid_src); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index fec1fd9d8e..6ab8036560 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -139,6 +139,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } else if (do_range_aid) { @@ -150,6 +151,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _information_events.flags.reset_hgt_to_rng = true; resetAltitudeTo(aid_src.observation, aid_src.observation_variance); _state.terrain = 0.f; + resetAidSourceStatusZeroInnovation(aid_src); _control_status.flags.rng_hgt = true; stopRngTerrFusion(); @@ -163,6 +165,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } } @@ -181,6 +184,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _information_events.flags.reset_hgt_to_rng = true; resetAltitudeTo(aid_src.observation - _state.terrain); + resetAidSourceStatusZeroInnovation(aid_src); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { @@ -198,6 +202,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } } @@ -218,6 +223,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { if (aid_src.innovation_rejected) { resetTerrainToRng(aid_src); + resetAidSourceStatusZeroInnovation(aid_src); } _control_status.flags.rng_terrain = true; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index f578a929aa..b831d096d1 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -1016,20 +1016,7 @@ private: #endif // CONFIG_EKF2_EXTERNAL_VISION // state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc) - void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const - { - status.time_last_fuse = _time_delayed_us; - - status.innovation = 0.f; - status.innovation_filtered = 0.f; - status.innovation_variance = status.observation_variance; - - status.test_ratio = 0.f; - status.test_ratio_filtered = 0.f; - - status.innovation_rejected = false; - status.fused = true; - } + void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const; // helper used for populating and filtering estimator aid source struct for logging void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9638abeb69..a6872b40d2 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1131,6 +1131,21 @@ bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R, return true; } +void Ekf::resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const +{ + status.time_last_fuse = _time_delayed_us; + + status.innovation = 0.f; + status.innovation_filtered = 0.f; + status.innovation_variance = status.observation_variance; + + status.test_ratio = 0.f; + status.test_ratio_filtered = 0.f; + + status.innovation_rejected = false; + status.fused = true; +} + void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t ×tamp_sample, const float &observation, const float &observation_variance, const float &innovation, const float &innovation_variance, diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index 17c95cad45..595d518abb 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -345,12 +345,10 @@ TEST_F(EkfHeightFusionTest, baroRefAllHgtFailReset) // Also check the reset counters to make sure the reset logic triggered reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); + // The velocity does not reset as baro only provides height measurement EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); - - // The height resets twice in a row as the baro innovation is not corrected after a height - // reset and triggers a new reset at the next iteration - EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2)); } TEST_F(EkfHeightFusionTest, changeEkfOriginAlt)