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

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;

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);

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);

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;

src/modules/ekf2/EKF/ekf.h

@@ -1016,20 +1016,7 @@ class Ekf final : public EstimatorInterface
 #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 &timestamp_sample,

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 &timestamp_sample,
 				const float &observation, const float &observation_variance,
 				const float &innovation, const float &innovation_variance,

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)