ekf2: innovation sequence monitoring for all aid sources

- add new 'innovation_filtered' and 'test_ratio_filtered' fields to
   estimator_aid_source topics

msg/EstimatorAidSource1d.msg

@@ -11,8 +11,12 @@ float32 observation
 float32 observation_variance
 
 float32 innovation
+float32 innovation_filtered
+
 float32 innovation_variance
-float32 test_ratio
+
+float32 test_ratio           # normalized innovation squared
+float32 test_ratio_filtered  # signed filtered test ratio
 
 bool innovation_rejected     # true if the observation has been rejected
 bool fused                   # true if the sample was successfully fused

msg/EstimatorAidSource2d.msg

@@ -11,8 +11,12 @@ float32[2] observation
 float32[2] observation_variance
 
 float32[2] innovation
+float32[2] innovation_filtered
+
 float32[2] innovation_variance
-float32[2] test_ratio
+
+float32[2] test_ratio           # normalized innovation squared
+float32[2] test_ratio_filtered  # signed filtered test ratio
 
 bool innovation_rejected        # true if the observation has been rejected
 bool fused                      # true if the sample was successfully fused

msg/EstimatorAidSource3d.msg

@@ -11,8 +11,12 @@ float32[3] observation
 float32[3] observation_variance
 
 float32[3] innovation
+float32[3] innovation_filtered
+
 float32[3] innovation_variance
-float32[3] test_ratio
+
+float32[3] test_ratio           # normalized innovation squared
+float32[3] test_ratio_filtered  # signed filtered test ratio
 
 bool innovation_rejected        # true if the observation has been rejected
 bool fused                      # true if the sample was successfully fused

src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp

@@ -63,6 +63,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 	}
 
 #if defined(CONFIG_EKF2_GNSS)
+
 	// clear yaw estimator airspeed (updated later with true airspeed if airspeed fusion is active)
 	if (_control_status.flags.fixed_wing) {
 		if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
@@ -74,6 +75,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 			_yawEstimator.setTrueAirspeed(0.f);
 		}
 	}
+
 #endif // CONFIG_EKF2_GNSS
 
 	if (_params.arsp_thr <= 0.f) {
@@ -89,10 +91,14 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 
 		_innov_check_fail_status.flags.reject_airspeed = _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag
 
-		const bool continuing_conditions_passing = _control_status.flags.in_air && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos;
+		const bool continuing_conditions_passing = _control_status.flags.in_air
+				&& _control_status.flags.fixed_wing
+				&& !_control_status.flags.fake_pos;
+
 		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
 		const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
-		const bool starting_conditions_passing = continuing_conditions_passing && is_airspeed_significant
+		const bool starting_conditions_passing = continuing_conditions_passing
+				&& is_airspeed_significant
 				&& (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning);
 
 		if (_control_status.flags.fuse_aspd) {
@@ -139,26 +145,22 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 
 void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const
 {
-	// reset flags
-	resetEstimatorAidStatus(aid_src);
-
 	// Variance for true airspeed measurement - (m/sec)^2
 	const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
 			   math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
 
 	float innov = 0.f;
 	float innov_var = 0.f;
-	sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON, &innov, &innov_var);
+	sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON,
+					     &innov, &innov_var);
 
-	aid_src.observation = airspeed_sample.true_airspeed;
+	updateAidSourceStatus(aid_src,
-	aid_src.observation_variance = R;
+				 airspeed_sample.time_us,                 // sample timestamp
-	aid_src.innovation = innov;
+				 airspeed_sample.true_airspeed,           // observation
-	aid_src.innovation_variance = innov_var;
+				 R,                                       // observation variance
-
+				 innov,                                   // innovation
-	aid_src.timestamp_sample = airspeed_sample.time_us;
+				 innov_var,                               // innovation variance
-
+				 math::max(_params.tas_innov_gate, 1.f)); // innovation gate
-	const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f);
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
 }
 
 void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)
@@ -221,7 +223,6 @@ void Ekf::stopAirspeedFusion()
 {
 	if (_control_status.flags.fuse_aspd) {
 		ECL_INFO("stopping airspeed fusion");
-		resetEstimatorAidStatus(_aid_src_airspeed);
 		_control_status.flags.fuse_aspd = false;
 
 #if defined(CONFIG_EKF2_GNSS)
@@ -235,16 +236,18 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
 	constexpr float sideslip_var = sq(math::radians(15.0f));
 
 	const float euler_yaw = getEulerYaw(_R_to_earth);
-	const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
+	const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f)
+				      * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));
 
 	matrix::SquareMatrix<float, State::wind_vel.dof> P_wind;
-	sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(), getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind);
+	sym::ComputeWindInitAndCovFromAirspeed(_state.vel, euler_yaw, airspeed_sample.true_airspeed, getVelocityVariance(),
+					       getYawVar(), sideslip_var, airspeed_var, &_state.wind_vel, &P_wind);
 
 	resetStateCovariance<State::wind_vel>(P_wind);
 
 	ECL_INFO("reset wind using airspeed to (%.3f, %.3f)", (double)_state.wind_vel(0), (double)_state.wind_vel(1));
 
-	_aid_src_airspeed.time_last_fuse = _time_delayed_us;
+	resetAidSourceStatusZeroInnovation(_aid_src_airspeed);
 }
 
 void Ekf::resetVelUsingAirspeed(const airspeedSample &airspeed_sample)

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -80,14 +80,17 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 			position = ekf.global_origin().project(sample.latitude, sample.longitude);
 			//const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude;
 			// relax the upper observation noise limit which prevents bad measurements perturbing the position estimate
-			float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get());
+			float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f);
 			const float pos_var = sq(pos_noise);
 			const Vector2f pos_obs_var(pos_var, pos_var);
-			ekf.updateHorizontalPositionAidSrcStatus(sample.time_us,
+
+			ekf.updateAidSourceStatus(aid_src,
+						     sample.time_us,                                    // sample timestamp
 						     position,                                          // observation
 						     pos_obs_var,                                       // observation variance
-					math::max(_param_ekf2_agp_gate.get(), 1.f), // innovation gate
+						     Vector2f(ekf.state().pos) - position,              // innovation
-					aid_src);
+						     Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance
+						     math::max(_param_ekf2_agp_gate.get(), 1.f));       // innovation gate
 		}
 
 		const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude)
@@ -97,6 +100,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 		switch (_state) {
 		case State::stopped:
+
 		/* FALLTHROUGH */
 		case State::starting:
 			if (starting_conditions) {
@@ -116,6 +120,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 					}
 				}
 			}
+
 			break;
 
 		case State::active:
@@ -124,8 +129,11 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max)
 				    || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) {
+
 					ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
-					aid_src.time_last_fuse = imu_delayed.time_us;
+
+					ekf.resetAidSourceStatusZeroInnovation(aid_src);
+
 					_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 				}
 
@@ -133,6 +141,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 				ekf.disableControlStatusAuxGpos();
 				_state = State::stopped;
 			}
+
 			break;
 
 		default:
@@ -143,6 +152,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 		aid_src.timestamp = hrt_absolute_time();
 		_estimator_aid_src_aux_global_position_pub.publish(aid_src);
 #endif // MODULE_NAME
+
 	} else if ((_state != State::stopped) && isTimedOut(_time_last_buffer_push, imu_delayed.time_us, (uint64_t)5e6)) {
 		ekf.disableControlStatusAuxGpos();
 		_state = State::stopped;

src/modules/ekf2/EKF/aid_sources/auxvel/auxvel_fusion.cpp

@@ -36,13 +36,17 @@
 void Ekf::controlAuxVelFusion()
 {
 	if (_auxvel_buffer) {
-		auxVelSample auxvel_sample_delayed;
+		auxVelSample sample;
 
-		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &auxvel_sample_delayed)) {
+		if (_auxvel_buffer->pop_first_older_than(_time_delayed_us, &sample)) {
 
-			resetEstimatorAidStatus(_aid_src_aux_vel);
+			updateAidSourceStatus(_aid_src_aux_vel,
-
+						 sample.time_us,                                           // sample timestamp
-			updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
+						 sample.vel,                                               // observation
+						 sample.velVar,                                            // observation variance
+						 Vector2f(_state.vel.xy()) - sample.vel,                   // innovation
+						 Vector2f(getStateVariance<State::vel>()) + sample.velVar, // innovation variance
+						 math::max(_params.auxvel_gate, 1.f));                     // innovation gate
 
 			if (isHorizontalAidingActive()) {
 				fuseHorizontalVelocity(_aid_src_aux_vel);
@@ -55,5 +59,4 @@ void Ekf::stopAuxVelFusion()
 {
 	ECL_INFO("stopping aux vel fusion");
 	//_control_status.flags.aux_vel = false;
-	resetEstimatorAidStatus(_aid_src_aux_vel);
 }

src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp

@@ -59,8 +59,6 @@ void Ekf::controlBaroHeightFusion()
 
 		const float measurement_var = sq(_params.baro_noise);
 
-		const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
-
 		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
 
 		if (measurement_valid) {
@@ -80,11 +78,11 @@ void Ekf::controlBaroHeightFusion()
 		}
 
 		// vertical position innovation - baro measurement has opposite sign to earth z axis
-		updateVerticalPositionAidSrcStatus(baro_sample.time_us,
+		updateVerticalPositionAidStatus(aid_src,
-						   -(measurement - bias_est.getBias()),
+						baro_sample.time_us,
-						   measurement_var + bias_est.getBiasVar(),
+						-(measurement - bias_est.getBias()),      // observation
-						   innov_gate,
+						measurement_var + bias_est.getBiasVar(),  // observation variance
-						   aid_src);
+						math::max(_params.baro_innov_gate, 1.f)); // innovation gate
 
 		// Compensate for positive static pressure transients (negative vertical position innovations)
 		// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
@@ -191,7 +189,6 @@ void Ekf::stopBaroHgtFusion()
 		}
 
 		_baro_b_est.setFusionInactive();
-		resetEstimatorAidStatus(_aid_src_baro_hgt);
 
 		_control_status.flags.baro_hgt = false;
 	}

src/modules/ekf2/EKF/aid_sources/drag/drag_fusion.cpp

@@ -105,11 +105,16 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 		bcoef_inv(1) = bcoef_inv(0);
 	}
 
-	_aid_src_drag.timestamp_sample = drag_sample.time_us;
-	_aid_src_drag.fused = false;
-
 	bool fused[] {false, false};
 
+	Vector2f observation{};
+	Vector2f observation_variance{R_ACC, R_ACC};
+	Vector2f innovation{};
+	Vector2f innovation_variance{};
+
+	// Apply an innovation consistency check with a 5 Sigma threshold
+	const float innov_gate = 5.f;
+
 	VectorState H;
 
 	// perform sequential fusion of XY specific forces
@@ -120,16 +125,15 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 		// Drag is modelled as an arbitrary combination of bluff body drag that proportional to
 		// equivalent airspeed squared, and rotor momentum drag that is proportional to true airspeed
 		// parallel to the rotor disc and mass flow through the rotor disc.
-		const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed - rel_wind_body(axis_index) * mcoef_corrrected;
+		const float pred_acc = -0.5f * bcoef_inv(axis_index) * rho * rel_wind_body(axis_index) * rel_wind_speed
+				       - rel_wind_body(axis_index) * mcoef_corrrected;
 
-		_aid_src_drag.observation[axis_index] = mea_acc;
+		observation(axis_index) = mea_acc;
-		_aid_src_drag.observation_variance[axis_index] = R_ACC;
+		innovation(axis_index) = pred_acc - mea_acc;
-		_aid_src_drag.innovation[axis_index] = pred_acc - mea_acc;
-		_aid_src_drag.innovation_variance[axis_index] = NAN; // reset
 
 		if (axis_index == 0) {
 			sym::ComputeDragXInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
-						      &_aid_src_drag.innovation_variance[axis_index], &H);
+						      &innovation_variance(axis_index), &H);
 
 			if (!using_bcoef_x && !using_mcoef) {
 				continue;
@@ -137,35 +141,41 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 
 		} else if (axis_index == 1) {
 			sym::ComputeDragYInnovVarAndH(state_vector_prev, P, rho, bcoef_inv(axis_index), mcoef_corrrected, R_ACC, FLT_EPSILON,
-						      &_aid_src_drag.innovation_variance[axis_index], &H);
+						      &innovation_variance(axis_index), &H);
 
 			if (!using_bcoef_y && !using_mcoef) {
 				continue;
 			}
 		}
 
-		if (_aid_src_drag.innovation_variance[axis_index] < R_ACC) {
+		if (innovation_variance(axis_index) < R_ACC) {
 			// calculation is badly conditioned
-			return;
+			break;
 		}
 
-		// Apply an innovation consistency check with a 5 Sigma threshold
+		const float test_ratio = sq(innovation(axis_index)) / (sq(innov_gate) * innovation_variance(axis_index));
-		const float innov_gate = 5.f;
-		setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate);
 
 		if (_control_status.flags.in_air && _control_status.flags.wind && !_control_status.flags.fake_pos
-		    && PX4_ISFINITE(_aid_src_drag.innovation_variance[axis_index]) && PX4_ISFINITE(_aid_src_drag.innovation[axis_index])
+		    && PX4_ISFINITE(innovation_variance(axis_index)) && PX4_ISFINITE(innovation(axis_index))
-		    && (_aid_src_drag.test_ratio[axis_index] < 1.f)
+		    && (test_ratio < 1.f)
 		   ) {
 
-			VectorState K = P * H / _aid_src_drag.innovation_variance[axis_index];
+			VectorState K = P * H / innovation_variance(axis_index);
 
-			if (measurementUpdate(K, H, R_ACC, _aid_src_drag.innovation[axis_index])) {
+			if (measurementUpdate(K, H, R_ACC, innovation(axis_index))) {
 				fused[axis_index] = true;
 			}
 		}
 	}
 
+	updateAidSourceStatus(_aid_src_drag,
+				 drag_sample.time_us,  // sample timestamp
+				 observation,          // observation
+				 observation_variance, // observation variance
+				 innovation,           // innovation
+				 innovation_variance,  // innovation variance
+				 innov_gate);          // innovation gate
+
 	if (fused[0] && fused[1]) {
 		_aid_src_drag.fused = true;
 		_aid_src_drag.time_last_fuse = _time_delayed_us;

src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp

@@ -69,9 +69,6 @@ void Ekf::controlExternalVisionFusion()
 			_ev_sample_prev = ev_sample;
 		}
 
-		// record corresponding yaw state for future EV delta heading innovation (logging only)
-		_ev_yaw_pred_prev = getEulerYaw(_state.quat_nominal);
-
 	} else if ((_control_status.flags.ev_pos || _control_status.flags.ev_vel || _control_status.flags.ev_yaw
 		    || _control_status.flags.ev_hgt)
 		   && isTimedOut(_ev_sample_prev.time_us, 2 * EV_MAX_INTERVAL)) {

src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp

@@ -85,11 +85,11 @@ void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample, const bool com
 
 	const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
 
-	updateVerticalPositionAidSrcStatus(ev_sample.time_us,
+	updateVerticalPositionAidStatus(aid_src,
-					   measurement - bias_est.getBias(),
+					ev_sample.time_us,
-					   measurement_var + bias_est.getBiasVar(),
+					measurement - bias_est.getBias(),           // observation
-					   math::max(_params.ev_pos_innov_gate, 1.f),
+					measurement_var + bias_est.getBiasVar(),    // observation variance
-					   aid_src);
+					math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
 
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
@@ -220,7 +220,6 @@ void Ekf::stopEvHgtFusion()
 		}
 
 		_ev_hgt_b_est.setFusionInactive();
-		resetEstimatorAidStatus(_aid_src_ev_hgt);
 
 		_control_status.flags.ev_hgt = false;
 	}

src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp

@@ -126,12 +126,14 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 	}
 
 #if defined(CONFIG_EKF2_GNSS)
+
 	// increase minimum variance if GPS active (position reference)
 	if (_control_status.flags.gps) {
 		for (int i = 0; i < 2; i++) {
 			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
 		}
 	}
+
 #endif // CONFIG_EKF2_GNSS
 
 	const Vector2f measurement{pos(0), pos(1)};
@@ -155,18 +157,24 @@ void Ekf::controlEvPosFusion(const extVisionSample &ev_sample, const bool common
 		}
 	}
 
-	updateHorizontalPositionAidSrcStatus(ev_sample.time_us,
+	const Vector2f position = measurement - _ev_pos_b_est.getBias();
-					     measurement - _ev_pos_b_est.getBias(),        // observation
+	const Vector2f pos_obs_var = measurement_var + _ev_pos_b_est.getBiasVar();
-					     measurement_var + _ev_pos_b_est.getBiasVar(), // observation variance
+
-					     math::max(_params.ev_pos_innov_gate, 1.f),    // innovation gate
+	updateAidSourceStatus(aid_src,
-					     aid_src);
+				 ev_sample.time_us,                                      // sample timestamp
+				 position,                                               // observation
+				 pos_obs_var,                                            // observation variance
+				 Vector2f(_state.pos) - position,                        // innovation
+				 Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
+				 math::max(_params.ev_pos_innov_gate, 1.f));             // innovation gate
 
 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
 	if (measurement_valid && quality_sufficient) {
 		_ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1))));
 		_ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO
-		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), measurement_var + Vector2f(getStateVariance<State::pos>()));
+		_ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()),
+				       measurement_var + Vector2f(getStateVariance<State::pos>()));
 	}
 
 	if (!measurement_valid) {
@@ -297,8 +305,6 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure
 void Ekf::stopEvPosFusion()
 {
 	if (_control_status.flags.ev_pos) {
-		resetEstimatorAidStatus(_aid_src_ev_pos);
-
 		_control_status.flags.ev_pos = false;
 	}
 }

src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp

@@ -107,12 +107,14 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 	}
 
 #if defined(CONFIG_EKF2_GNSS)
+
 	// increase minimum variance if GPS active (position reference)
 	if (_control_status.flags.gps) {
 		for (int i = 0; i < 2; i++) {
 			vel_cov(i, i) = math::max(vel_cov(i, i), sq(_params.gps_vel_noise));
 		}
 	}
+
 #endif // CONFIG_EKF2_GNSS
 
 	const Vector3f measurement{vel};
@@ -125,11 +127,13 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 
 	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
 
-	updateVelocityAidSrcStatus(ev_sample.time_us,
+	updateAidSourceStatus(aid_src,
+				 ev_sample.time_us,                          // sample timestamp
 				 measurement,                                // observation
 				 measurement_var,                            // observation variance
-				   math::max(_params.ev_vel_innov_gate, 1.f), // innovation gate
+				 _state.vel - measurement,                   // innovation
-				   aid_src);
+				 getVelocityVariance() + measurement_var,    // innovation variance
+				 math::max(_params.ev_vel_innov_gate, 1.f)); // innovation gate
 
 	if (!measurement_valid) {
 		continuing_conditions_passing = false;
@@ -149,7 +153,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					_information_events.flags.reset_vel_to_vision = true;
 					resetVelocityTo(measurement, measurement_var);
-					aid_src.time_last_fuse = _time_delayed_us;
+					resetAidSourceStatusZeroInnovation(aid_src);
 
 				} else {
 					// EV has reset, but quality isn't sufficient
@@ -174,7 +178,7 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 					_information_events.flags.reset_vel_to_vision = true;
 					ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 					resetVelocityTo(measurement, measurement_var);
-					aid_src.time_last_fuse = _time_delayed_us;
+					resetAidSourceStatusZeroInnovation(aid_src);
 
 					if (_control_status.flags.in_air) {
 						_nb_ev_vel_reset_available--;
@@ -205,19 +209,25 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 		if (starting_conditions_passing) {
 			// activate fusion, only reset if necessary
 			if (!isHorizontalAidingActive() || yaw_alignment_changed) {
-				ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME, (double)measurement(0), (double)measurement(1), (double)measurement(2));
+				ECL_INFO("starting %s fusion, resetting velocity to (%.3f, %.3f, %.3f)", AID_SRC_NAME,
-				_information_events.flags.reset_vel_to_vision = true;
+					 (double)measurement(0), (double)measurement(1), (double)measurement(2));
-				resetVelocityTo(measurement, measurement_var);
 
-			} else {
+				_information_events.flags.reset_vel_to_vision = true;
+
+				resetVelocityTo(measurement, measurement_var);
+				resetAidSourceStatusZeroInnovation(aid_src);
+
+				_control_status.flags.ev_vel = true;
+
+			} else if (fuseVelocity(aid_src)) {
 				ECL_INFO("starting %s fusion", AID_SRC_NAME);
+				_control_status.flags.ev_vel = true;
 			}
 
-			aid_src.time_last_fuse = _time_delayed_us;
+			if (_control_status.flags.ev_vel) {
-
 				_nb_ev_vel_reset_available = 5;
 				_information_events.flags.starting_vision_vel_fusion = true;
-			_control_status.flags.ev_vel = true;
+			}
 		}
 	}
 }
@@ -225,7 +235,6 @@ void Ekf::controlEvVelFusion(const extVisionSample &ev_sample, const bool common
 void Ekf::stopEvVelFusion()
 {
 	if (_control_status.flags.ev_vel) {
-		resetEstimatorAidStatus(_aid_src_ev_vel);
 
 		_control_status.flags.ev_vel = false;
 	}

src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp

@@ -43,11 +43,22 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 {
 	static constexpr const char *AID_SRC_NAME = "EV yaw";
 
-	resetEstimatorAidStatus(aid_src);
+	float obs = getEulerYaw(ev_sample.quat);
-	aid_src.timestamp_sample = ev_sample.time_us;
+	float obs_var = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
-	aid_src.observation = getEulerYaw(ev_sample.quat);
+
-	aid_src.observation_variance = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
+	float innov = wrap_pi(getEulerYaw(_R_to_earth) - obs);
-	aid_src.innovation = wrap_pi(getEulerYaw(_R_to_earth) - aid_src.observation);
+	float innov_var = 0.f;
+
+	VectorState H_YAW;
+	computeYawInnovVarAndH(obs_var, innov_var, H_YAW);
+
+	updateAidSourceStatus(aid_src,
+		ev_sample.time_us,                           // sample timestamp
+		obs,                                         // observation
+		obs_var,                                     // observation variance
+		innov,                                       // innovation
+		innov_var,                                   // innovation variance
+		math::max(_params.heading_innov_gate, 1.f)); // innovation gate
 
 	if (ev_reset) {
 		_control_status.flags.ev_yaw_fault = false;
@@ -65,10 +76,6 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 	    && (ev_sample.pos_frame != PositionFrame::LOCAL_FRAME_NED)
 	   ) {
 		continuing_conditions_passing = false;
-
-		// use delta yaw for innovation logging
-		aid_src.innovation = wrap_pi(wrap_pi(getEulerYaw(_R_to_earth) - _ev_yaw_pred_prev)
-					     - wrap_pi(getEulerYaw(ev_sample.quat) - getEulerYaw(_ev_sample_prev.quat)));
 	}
 
 	const bool starting_conditions_passing = common_starting_conditions_passing
@@ -94,7 +101,7 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 				}
 
 			} else if (quality_sufficient) {
-				fuseYaw(aid_src);
+				fuseYaw(aid_src, H_YAW);
 
 			} else {
 				aid_src.innovation_rejected = true;
@@ -141,18 +148,25 @@ void Ekf::controlEvYawFusion(const extVisionSample &ev_sample, const bool common
 			if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_NED) {
 
 				if (_control_status.flags.yaw_align) {
+
+					if (fuseYaw(aid_src, H_YAW)) {
 						ECL_INFO("starting %s fusion", AID_SRC_NAME);
+						_information_events.flags.starting_vision_yaw_fusion = true;
+
+						_control_status.flags.ev_yaw = true;
+					}
 
 				} else {
 					// reset yaw to EV and set yaw_align
 					ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME);
+					_information_events.flags.starting_vision_yaw_fusion = true;
+
 					resetQuatStateYaw(aid_src.observation, aid_src.observation_variance);
 					_control_status.flags.yaw_align = true;
-				}
+					_control_status.flags.ev_yaw = true;
 
 					aid_src.time_last_fuse = _time_delayed_us;
-				_information_events.flags.starting_vision_yaw_fusion = true;
+				}
-				_control_status.flags.ev_yaw = true;
 
 			} else if (ev_sample.pos_frame == PositionFrame::LOCAL_FRAME_FRD) {
 				// turn on fusion of external vision yaw measurements
@@ -178,7 +192,6 @@ void Ekf::stopEvYawFusion()
 {
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	if (_control_status.flags.ev_yaw) {
-		resetEstimatorAidStatus(_aid_src_ev_yaw);
 
 		_control_status.flags.ev_yaw = false;
 	}

src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp

@@ -51,8 +51,7 @@ void Ekf::controlFakeHgtFusion()
 		const float obs_var = sq(_params.pos_noaid_noise);
 		const float innov_gate = 3.f;
 
-		updateVerticalPositionAidSrcStatus(_time_delayed_us, _last_known_pos(2), obs_var, innov_gate, aid_src);
+		updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate);
-
 
 		const bool continuing_conditions_passing = !isVerticalAidingActive();
 		const bool starting_conditions_passing = continuing_conditions_passing
@@ -113,7 +112,5 @@ void Ekf::stopFakeHgtFusion()
 	if (_control_status.flags.fake_hgt) {
 		ECL_INFO("stop fake height fusion");
 		_control_status.flags.fake_hgt = false;
-
-		resetEstimatorAidStatus(_aid_src_fake_hgt);
 	}
 }

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -63,10 +63,17 @@ void Ekf::controlFakePosFusion()
 			obs_var(0) = obs_var(1) = sq(0.5f);
 		}
 
+		const Vector2f position(_last_known_pos);
+
 		const float innov_gate = 3.f;
 
-		updateHorizontalPositionAidSrcStatus(_time_delayed_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src);
+		updateAidSourceStatus(aid_src,
-
+					 _time_delayed_us,
+					 position,                                           // observation
+					 obs_var,                                            // observation variance
+					 Vector2f(_state.pos) - position,                    // innovation
+					 Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
+					 innov_gate);                                        // innovation gate
 
 		const bool continuing_conditions_passing = !isHorizontalAidingActive()
 				&& ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
@@ -131,7 +138,5 @@ void Ekf::stopFakePosFusion()
 	if (_control_status.flags.fake_pos) {
 		ECL_INFO("stop fake position fusion");
 		_control_status.flags.fake_pos = false;
-
-		resetEstimatorAidStatus(_aid_src_fake_pos);
 	}
 }

src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp

@@ -67,16 +67,14 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 		const float measurement = gnss_alt - getEkfGlobalOriginAltitude();
 		const float measurement_var = sq(noise);
 
-		const float innov_gate = math::max(_params.gps_pos_innov_gate, 1.f);
-
 		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
 
 		// GNSS position, vertical position GNSS measurement has opposite sign to earth z axis
-		updateVerticalPositionAidSrcStatus(gps_sample.time_us,
+		updateVerticalPositionAidStatus(aid_src,
+						gps_sample.time_us,
 						-(measurement - bias_est.getBias()),
 						measurement_var + bias_est.getBiasVar(),
-						   innov_gate,
+						math::max(_params.gps_pos_innov_gate, 1.f));
-						   aid_src);
 
 		// update the bias estimator before updating the main filter but after
 		// using its current state to compute the vertical position innovation
@@ -171,7 +169,6 @@ void Ekf::stopGpsHgtFusion()
 		}
 
 		_gps_hgt_b_est.setFusionInactive();
-		resetEstimatorAidStatus(_aid_src_gnss_hgt);
 
 		_control_status.flags.gps_hgt = false;
 	}

src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp

@@ -183,13 +183,26 @@ void Ekf::updateGnssVel(const gnssSample &gnss_sample, estimator_aid_source3d_s
 	const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
 	const Vector3f velocity = gnss_sample.vel - vel_offset_earth;
 
-	const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise));
+	const float vel_var = sq(math::max(gnss_sample.sacc, _params.gps_vel_noise, 0.01f));
 	const Vector3f vel_obs_var(vel_var, vel_var, vel_var * sq(1.5f));
-	updateVelocityAidSrcStatus(gnss_sample.time_us,
+
+	const float innovation_gate = math::max(_params.gps_vel_innov_gate, 1.f);
+
+	updateAidSourceStatus(aid_src,
+				 gnss_sample.time_us,                  // sample timestamp
 				 velocity,                             // observation
 				 vel_obs_var,                          // observation variance
-				   math::max(_params.gps_vel_innov_gate, 1.f),                 // innovation gate
+				 _state.vel - velocity,                // innovation
-				   aid_src);
+				 getVelocityVariance() + vel_obs_var,  // innovation variance
+				 innovation_gate);                     // innovation gate
+
+	// vz special case if there is bad vertical acceleration data, then don't reject measurement if GNSS reports velocity accuracy is acceptable,
+	// but limit innovation to prevent spikes that could destabilise the filter
+	if (aid_src.innovation_rejected && _fault_status.flags.bad_acc_vertical && (gnss_sample.sacc < _params.req_sacc)) {
+		const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance[2]);
+		aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
+		aid_src.innovation_rejected = false;
+	}
 }
 
 void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s &aid_src)
@@ -210,13 +223,16 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s
 		}
 	}
 
-	const float pos_var = sq(pos_noise);
+	const float pos_var = math::max(sq(pos_noise), sq(0.01f));
 	const Vector2f pos_obs_var(pos_var, pos_var);
-	updateHorizontalPositionAidSrcStatus(gnss_sample.time_us,
+
+	updateAidSourceStatus(aid_src,
+				 gnss_sample.time_us,                                    // sample timestamp
 				 position,                                               // observation
 				 pos_obs_var,                                            // observation variance
-					     math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
+				 Vector2f(_state.pos) - position,                        // innovation
-					     aid_src);
+				 Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
+				 math::max(_params.gps_pos_innov_gate, 1.f));            // innovation gate
 }
 
 void Ekf::controlGnssYawEstimator(estimator_aid_source3d_s &aid_src_vel)
@@ -289,7 +305,8 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src)
 {
 	_information_events.flags.reset_vel_to_gps = true;
 	resetVelocityTo(Vector3f(aid_src.observation), Vector3f(aid_src.observation_variance));
-	aid_src.time_last_fuse = _time_delayed_us;
+
+	resetAidSourceStatusZeroInnovation(aid_src);
 }
 
 void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
@@ -297,7 +314,8 @@ void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src)
 	_information_events.flags.reset_pos_to_gps = true;
 	resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance));
 	_gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty
-	aid_src.time_last_fuse = _time_delayed_us;
+
+	resetAidSourceStatusZeroInnovation(aid_src);
 }
 
 bool Ekf::shouldResetGpsFusion() const
@@ -342,12 +360,12 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 		return;
 	}
 
-	updateGpsYaw(gps_sample);
-
 	const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
 
 	if (is_new_data_available) {
 
+		updateGpsYaw(gps_sample);
+
 		const bool continuing_conditions_passing = _control_status.flags.tilt_align;
 
 		const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push,
@@ -392,7 +410,9 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 
 					// Reset before starting the fusion
 					if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
-						_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
+
+						resetAidSourceStatusZeroInnovation(_aid_src_gnss_yaw);
+
 						_control_status.flags.gps_yaw = true;
 						_control_status.flags.yaw_align = true;
 					}
@@ -422,7 +442,6 @@ void Ekf::stopGpsYawFusion()
 	if (_control_status.flags.gps_yaw) {
 
 		_control_status.flags.gps_yaw = false;
-		resetEstimatorAidStatus(_aid_src_gnss_yaw);
 
 		ECL_INFO("stopping GPS yaw fusion");
 	}
@@ -433,8 +452,7 @@ void Ekf::stopGpsFusion()
 {
 	if (_control_status.flags.gps) {
 		ECL_INFO("stopping GPS position and velocity fusion");
-		resetEstimatorAidStatus(_aid_src_gnss_pos);
+
-		resetEstimatorAidStatus(_aid_src_gnss_vel);
 		_last_gps_fail_us = 0;
 		_last_gps_pass_us = 0;
 

src/modules/ekf2/EKF/aid_sources/gnss/gps_yaw_fusion.cpp

@@ -48,13 +48,6 @@
 
 void Ekf::updateGpsYaw(const gnssSample &gps_sample)
 {
-	if (PX4_ISFINITE(gps_sample.yaw)) {
-
-		auto &gnss_yaw = _aid_src_gnss_yaw;
-		resetEstimatorAidStatus(gnss_yaw);
-
-		// initially populate for estimator_aid_src_gnss_yaw logging
-
 	// calculate the observed yaw angle of antenna array, converting a from body to antenna yaw measurement
 	const float measured_hdg = wrap_pi(gps_sample.yaw + gps_sample.yaw_offset);
 
@@ -64,28 +57,24 @@ void Ekf::updateGpsYaw(const gnssSample &gps_sample)
 	float heading_pred;
 	float heading_innov_var;
 
-		{
 	VectorState H;
-		sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, gps_sample.yaw_offset, R_YAW, FLT_EPSILON,
-		}
+					    &heading_pred, &heading_innov_var, &H);
 
-		gnss_yaw.observation = measured_hdg;
+	updateAidSourceStatus(_aid_src_gnss_yaw,
-		gnss_yaw.observation_variance = R_YAW;
+				 gps_sample.time_us,                          // sample timestamp
-		gnss_yaw.innovation = wrap_pi(heading_pred - measured_hdg);
+				 measured_hdg,                                // observation
-		gnss_yaw.innovation_variance = heading_innov_var;
+				 R_YAW,                                       // observation variance
-
+				 wrap_pi(heading_pred - measured_hdg),        // innovation
-		gnss_yaw.timestamp_sample = gps_sample.time_us;
+				 heading_innov_var,                           // innovation variance
-
+				 math::max(_params.heading_innov_gate, 1.f)); // innovation gate
-		const float innov_gate = math::max(_params.heading_innov_gate, 1.0f);
-		setEstimatorAidStatusTestRatio(gnss_yaw, innov_gate);
-	}
 }
 
 void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 {
-	auto &gnss_yaw = _aid_src_gnss_yaw;
+	auto &aid_src = _aid_src_gnss_yaw;
 
-	if (gnss_yaw.innovation_rejected) {
+	if (aid_src.innovation_rejected) {
 		_innov_check_fail_status.flags.reject_yaw = true;
 		return;
 	}
@@ -94,19 +83,17 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 		antenna_yaw_offset = 0.f;
 	}
 
-	VectorState H;
-
-	{
 	float heading_pred;
 	float heading_innov_var;
+	VectorState H;
 
 	// Note: we recompute innov and innov_var because it doesn't cost much more than just computing H
 	// making a separate function just for H uses more flash space without reducing CPU load significantly
-	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, gnss_yaw.observation_variance, FLT_EPSILON, &heading_pred, &heading_innov_var, &H);
+	sym::ComputeGnssYawPredInnovVarAndH(_state.vector(), P, antenna_yaw_offset, aid_src.observation_variance, FLT_EPSILON,
-	}
+					    &heading_pred, &heading_innov_var, &H);
 
 	// check if the innovation variance calculation is badly conditioned
-	if (gnss_yaw.innovation_variance < gnss_yaw.observation_variance) {
+	if (aid_src.innovation_variance < aid_src.observation_variance) {
 		// the innovation variance contribution from the state covariances is negative which means the covariance matrix is badly conditioned
 		_fault_status.flags.bad_hdg = true;
 
@@ -119,11 +106,9 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 	_fault_status.flags.bad_hdg = false;
 	_innov_check_fail_status.flags.reject_yaw = false;
 
-	_gnss_yaw_signed_test_ratio_lpf.update(matrix::sign(gnss_yaw.innovation) * gnss_yaw.test_ratio);
+	if ((fabsf(aid_src.test_ratio_filtered) > 0.2f)
-
+	    && !_control_status.flags.in_air && isTimedOut(aid_src.time_last_fuse, (uint64_t)1e6)
-	if ((fabsf(_gnss_yaw_signed_test_ratio_lpf.getState()) > 0.2f)
+	   ) {
-		&& !_control_status.flags.in_air && isTimedOut(gnss_yaw.time_last_fuse, (uint64_t)1e6)) {
-
 		// A constant large signed test ratio is a sign of wrong gyro bias
 		// Reset the yaw gyro variance to converge faster and avoid
 		// being stuck on a previous bad estimate
@@ -132,15 +117,15 @@ void Ekf::fuseGpsYaw(float antenna_yaw_offset)
 
 	// calculate the Kalman gains
 	// only calculate gains for states we are using
-	VectorState Kfusion = P * H / gnss_yaw.innovation_variance;
+	VectorState Kfusion = P * H / aid_src.innovation_variance;
 
-	const bool is_fused = measurementUpdate(Kfusion, H, gnss_yaw.observation_variance, gnss_yaw.innovation);
+	const bool is_fused = measurementUpdate(Kfusion, H, aid_src.observation_variance, aid_src.innovation);
 	_fault_status.flags.bad_hdg = !is_fused;
-	gnss_yaw.fused = is_fused;
+	aid_src.fused = is_fused;
 
 	if (is_fused) {
 		_time_last_heading_fuse = _time_delayed_us;
-		gnss_yaw.time_last_fuse = _time_delayed_us;
+		aid_src.time_last_fuse = _time_delayed_us;
 	}
 }
 
@@ -161,8 +146,5 @@ bool Ekf::resetYawToGps(const float gnss_yaw, const float gnss_yaw_offset)
 	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
 	resetQuatStateYaw(measured_yaw, yaw_variance);
 
-	_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
-	_gnss_yaw_signed_test_ratio_lpf.reset(0.f);
-
 	return true;
 }

src/modules/ekf2/EKF/aid_sources/gravity/gravity_fusion.cpp

@@ -69,19 +69,13 @@ void Ekf::controlGravityFusion(const imuSample &imu)
 	sym::ComputeGravityXyzInnovVarAndHx(state_vector, P, measurement_var, &innovation_variance, &H);
 
 	// fill estimator aid source status
-	resetEstimatorAidStatus(_aid_src_gravity);
+	updateAidSourceStatus(_aid_src_gravity,
-	_aid_src_gravity.timestamp_sample = imu.time_us;
+				 imu.time_us,                                                 // sample timestamp
-	measurement.copyTo(_aid_src_gravity.observation);
+				 measurement,                                                 // observation
-
+				 Vector3f{measurement_var, measurement_var, measurement_var}, // observation variance
-	for (auto &var : _aid_src_gravity.observation_variance) {
+				 innovation,                                                  // innovation
-		var = measurement_var;
+				 innovation_variance,                                         // innovation variance
-	}
+				 0.25f);                                                      // innovation gate
-
-	innovation.copyTo(_aid_src_gravity.innovation);
-	innovation_variance.copyTo(_aid_src_gravity.innovation_variance);
-
-	float innovation_gate = 0.25f;
-	setEstimatorAidStatusTestRatio(_aid_src_gravity, innovation_gate);
 
 	bool fused[3] {false, false, false};
 

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -97,10 +97,6 @@ void Ekf::controlMagFusion()
 		_fault_status.flags.bad_mag_y = false;
 		_fault_status.flags.bad_mag_z = false;
 
-
-		resetEstimatorAidStatus(aid_src);
-		aid_src.timestamp_sample = mag_sample.time_us;
-
 		// XYZ Measurement uncertainty. Need to consider timing errors for fast rotations
 		const float R_MAG = math::max(sq(_params.mag_noise), sq(0.01f));
 
@@ -112,15 +108,13 @@ void Ekf::controlMagFusion()
 		VectorState H;
 		sym::ComputeMagInnovInnovVarAndHx(_state.vector(), P, mag_sample.mag, R_MAG, FLT_EPSILON, &mag_innov, &innov_var, &H);
 
-		for (int i = 0; i < 3; i++) {
+		updateAidSourceStatus(aid_src,
-			aid_src.observation[i] = mag_sample.mag(i);
+					mag_sample.time_us,                      // sample timestamp
-			aid_src.observation_variance[i] = R_MAG;
+					mag_sample.mag,                          // observation
-			aid_src.innovation[i] = mag_innov(i);
+					Vector3f(R_MAG, R_MAG, R_MAG),           // observation variance
-			aid_src.innovation_variance[i] = innov_var(i);
+					mag_innov,                               // innovation
-		}
+					innov_var,                               // innovation variance
-
+					math::max(_params.mag_innov_gate, 1.f)); // innovation gate
-		const float innov_gate = math::max(_params.mag_innov_gate, 1.f);
-		setEstimatorAidStatusTestRatio(aid_src, innov_gate);
 
 		// Perform an innovation consistency check and report the result
 		_innov_check_fail_status.flags.reject_mag_x = (aid_src.test_ratio[0] > 1.f);

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -164,8 +164,6 @@ void Ekf::stopFlowFusion()
 	if (_control_status.flags.opt_flow) {
 		ECL_INFO("stopping optical flow fusion");
 		_control_status.flags.opt_flow = false;
-
-		resetEstimatorAidStatus(_aid_src_optical_flow);
 	}
 }
 

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -50,9 +50,6 @@
 
 void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 {
-	resetEstimatorAidStatus(aid_src);
-	aid_src.timestamp_sample = _flow_sample_delayed.time_us;
-
 	const Vector2f vel_body = predictFlowVelBody();
 	const float range = predictFlowRange();
 
@@ -66,24 +63,26 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	_flow_vel_body(1) =  opt_flow_rate(0) * range;
 	_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
 
-	aid_src.observation[0] = opt_flow_rate(0); // flow around the X axis
+	Vector2f innovation{
-	aid_src.observation[1] = opt_flow_rate(1); // flow around the Y axis
+		(vel_body(1) / range) - opt_flow_rate(0),
-
+		(-vel_body(0) / range) - opt_flow_rate(1)
-	aid_src.innovation[0] =  (vel_body(1) / range) - aid_src.observation[0];
+	};
-	aid_src.innovation[1] = (-vel_body(0) / range) - aid_src.observation[1];
 
 	// calculate the optical flow observation variance
 	const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
-	aid_src.observation_variance[0] = R_LOS;
-	aid_src.observation_variance[1] = R_LOS;
 
 	Vector2f innov_var;
 	VectorState H;
 	sym::ComputeFlowXyInnovVarAndHx(_state.vector(), P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
-	innov_var.copyTo(aid_src.innovation_variance);
 
 	// run the innovation consistency check and record result
-	setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f));
+	updateAidSourceStatus(aid_src,
+		_flow_sample_delayed.time_us,  // sample timestamp
+		opt_flow_rate,                 // observation
+		Vector2f{R_LOS, R_LOS},        // observation variance
+		innovation,                    // innovation
+		innov_var,                     // innovation variance
+		math::max(_params.flow_innov_gate, 1.f));      // innovation gate
 }
 
 void Ekf::fuseOptFlow()
@@ -101,17 +100,13 @@ void Ekf::fuseOptFlow()
 	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
 	innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
 
-	if ((_aid_src_optical_flow.innovation_variance[0] < R_LOS)
+	if ((innov_var(0) < R_LOS) || (innov_var(1) < R_LOS)) {
-	    || (_aid_src_optical_flow.innovation_variance[1] < R_LOS)) {
 		// we need to reinitialise the covariance matrix and abort this fusion step
 		ECL_ERR("Opt flow error - covariance reset");
 		initialiseCovariance();
 		return;
 	}
 
-	// run the innovation consistency check and record result
-	setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
-
 	_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
 	_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
 

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -102,16 +102,14 @@ void Ekf::controlRangeHeightFusion()
 		const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
 		const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
 
-		const float innov_gate = math::max(_params.range_innov_gate, 1.f);
-
 		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
 
 		// vertical position innovation - baro measurement has opposite sign to earth z axis
-		updateVerticalPositionAidSrcStatus(_range_sensor.getSampleAddress()->time_us,
+		updateVerticalPositionAidStatus(aid_src,
-						   -(measurement - bias_est.getBias()),
+						_range_sensor.getSampleAddress()->time_us, // sample timestamp
-						   measurement_var + bias_est.getBiasVar(),
+						-(measurement - bias_est.getBias()),       // observation
-						   innov_gate,
+						measurement_var + bias_est.getBiasVar(),   // observation variance
-						   aid_src);
+						math::max(_params.range_innov_gate, 1.f)); // innovation gate
 
 		// update the bias estimator before updating the main filter but after
 		// using its current state to compute the vertical position innovation
@@ -259,7 +257,6 @@ void Ekf::stopRngHgtFusion()
 		}
 
 		_rng_hgt_b_est.setFusionInactive();
-		resetEstimatorAidStatus(_aid_src_rng_hgt);
 
 		_control_status.flags.rng_hgt = false;
 	}

src/modules/ekf2/EKF/aid_sources/sideslip/sideslip_fusion.cpp

@@ -49,8 +49,10 @@
 
 void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 {
-	_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
+	_control_status.flags.fuse_beta = _params.beta_fusion_enabled
-		&& _control_status.flags.in_air && !_control_status.flags.fake_pos;
+					  && _control_status.flags.fixed_wing
+					  && _control_status.flags.in_air
+					  && !_control_status.flags.fake_pos;
 
 	if (_control_status.flags.fuse_beta) {
 
@@ -76,26 +78,22 @@ void Ekf::controlBetaFusion(const imuSample &imu_delayed)
 	}
 }
 
-void Ekf::updateSideslip(estimator_aid_source1d_s &sideslip) const
+void Ekf::updateSideslip(estimator_aid_source1d_s &aid_src) const
 {
-	// reset flags
+	float observation = 0.f;
-	resetEstimatorAidStatus(sideslip);
+	const float R = math::max(sq(_params.beta_noise), sq(0.01f)); // observation noise variance
 
-	const float R = sq(_params.beta_noise); // observation noise variance
+	float innov;
-
+	float innov_var;
-	float innov = 0.f;
-	float innov_var = 0.f;
 	sym::ComputeSideslipInnovAndInnovVar(_state.vector(), P, R, FLT_EPSILON, &innov, &innov_var);
 
-	sideslip.observation = 0.f;
+	updateAidSourceStatus(aid_src,
-	sideslip.observation_variance = R;
+				 _time_delayed_us,                         // sample timestamp
-	sideslip.innovation = innov;
+				 observation,                              // observation
-	sideslip.innovation_variance = innov_var;
+				 R,                                        // observation variance
-
+				 innov,                                    // innovation
-	sideslip.timestamp_sample = _time_delayed_us;
+				 innov_var,                                // innovation variance
-
+				 math::max(_params.beta_innov_gate, 1.f)); // innovation gate
-	const float innov_gate = fmaxf(_params.beta_innov_gate, 1.f);
-	setEstimatorAidStatusTestRatio(sideslip, innov_gate);
 }
 
 void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
@@ -103,6 +101,7 @@ void Ekf::fuseSideslip(estimator_aid_source1d_s &sideslip)
 	if (sideslip.innovation_rejected) {
 		return;
 	}
+
 	// determine if we need the sideslip fusion to correct states other than wind
 	bool update_wind_only = !_control_status.flags.wind_dead_reckoning;
 

src/modules/ekf2/EKF/ekf.cpp

@@ -124,53 +124,6 @@ void Ekf::reset()
 	_time_good_vert_accel = 0;
 	_clip_counter = 0;
 
-#if defined(CONFIG_EKF2_BAROMETER)
-	resetEstimatorAidStatus(_aid_src_baro_hgt);
-#endif // CONFIG_EKF2_BAROMETER
-#if defined(CONFIG_EKF2_AIRSPEED)
-	resetEstimatorAidStatus(_aid_src_airspeed);
-#endif // CONFIG_EKF2_AIRSPEED
-#if defined(CONFIG_EKF2_SIDESLIP)
-	resetEstimatorAidStatus(_aid_src_sideslip);
-#endif // CONFIG_EKF2_SIDESLIP
-
-	resetEstimatorAidStatus(_aid_src_fake_pos);
-	resetEstimatorAidStatus(_aid_src_fake_hgt);
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	resetEstimatorAidStatus(_aid_src_ev_hgt);
-	resetEstimatorAidStatus(_aid_src_ev_pos);
-	resetEstimatorAidStatus(_aid_src_ev_vel);
-	resetEstimatorAidStatus(_aid_src_ev_yaw);
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_GNSS)
-	resetEstimatorAidStatus(_aid_src_gnss_hgt);
-	resetEstimatorAidStatus(_aid_src_gnss_pos);
-	resetEstimatorAidStatus(_aid_src_gnss_vel);
-
-# if defined(CONFIG_EKF2_GNSS_YAW)
-	resetEstimatorAidStatus(_aid_src_gnss_yaw);
-# endif // CONFIG_EKF2_GNSS_YAW
-#endif // CONFIG_EKF2_GNSS
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	resetEstimatorAidStatus(_aid_src_mag);
-#endif // CONFIG_EKF2_MAGNETOMETER
-
-#if defined(CONFIG_EKF2_AUXVEL)
-	resetEstimatorAidStatus(_aid_src_aux_vel);
-#endif // CONFIG_EKF2_AUXVEL
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	resetEstimatorAidStatus(_aid_src_optical_flow);
-	resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	resetEstimatorAidStatus(_aid_src_rng_hgt);
-#endif // CONFIG_EKF2_RANGE_FINDER
-
 	_zero_velocity_update.reset();
 }
 
@@ -332,7 +285,6 @@ void Ekf::predictState(const imuSample &imu_delayed)
 
 void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, uint64_t timestamp_observation)
 {
-
 	if (!_pos_ref.isInitialized()) {
 		return;
 	}
@@ -344,7 +296,10 @@ void Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, fl
 
 	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg) + _state.vel.xy() * dt;
 
-	resetHorizontalPositionToExternal(pos_corrected, math::max(accuracy, FLT_EPSILON));
+	resetHorizontalPositionTo(pos_corrected, sq(math::max(accuracy, 0.01f)));
+
+	ECL_INFO("reset position to external observation");
+	_information_events.flags.reset_pos_to_ext_obs = true;
 }
 
 void Ekf::updateParameters()

src/modules/ekf2/EKF/ekf.h

@@ -641,8 +641,6 @@ private:
 	estimator_aid_source3d_s _aid_src_ev_vel{};
 	estimator_aid_source1d_s _aid_src_ev_yaw{};
 
-	float _ev_yaw_pred_prev{}; ///< previous value of yaw state used by odometry fusion (m)
-
 	uint8_t _nb_ev_pos_reset_available{0};
 	uint8_t _nb_ev_vel_reset_available{0};
 	uint8_t _nb_ev_yaw_reset_available{0};
@@ -747,7 +745,6 @@ private:
 	}
 
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
-	bool fuseYaw(estimator_aid_source1d_s &aid_src_status);
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
 
@@ -802,8 +799,9 @@ private:
 	void resetHorizontalVelocityToZero();
 
 	void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var);
+
+
 	void resetHorizontalPositionToLastKnown();
-	void resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy);
 
 	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)); }
@@ -815,20 +813,15 @@ private:
 	void resetVerticalVelocityToZero();
 
 	// horizontal and vertical position aid source
-	void updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
+	void updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, const float observation, const float observation_variance, const float innovation_gate = 1.f) const;
-	void updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var, const float innov_gate, estimator_aid_source1d_s &aid_src) const;
-
-	// 2d & 3d velocity aid source
-	void updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var, const float innov_gate, estimator_aid_source2d_s &aid_src) const;
-	void updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var, const float innov_gate, estimator_aid_source3d_s &aid_src) const;
 
 	// horizontal and vertical position fusion
-	void fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src);
+	bool fuseHorizontalPosition(estimator_aid_source2d_s &pos_aid_src);
-	void fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
+	bool fuseVerticalPosition(estimator_aid_source1d_s &hgt_aid_src);
 
 	// 2d & 3d velocity fusion
-	void fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
+	bool fuseHorizontalVelocity(estimator_aid_source2d_s &vel_aid_src);
-	void fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
+	bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src);
 
 #if defined(CONFIG_EKF2_TERRAIN)
 	// terrain vertical position estimator
@@ -1130,88 +1123,200 @@ private:
 	bool _ev_q_error_initialized{false};
 #endif // CONFIG_EKF2_EXTERNAL_VISION
 
-	void resetEstimatorAidStatus(estimator_aid_source1d_s &status) const
+	// 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
 	{
-		// only bother resetting if timestamp_sample is set
+		status.time_last_fuse = _time_delayed_us;
-		if (status.timestamp_sample != 0) {
-			status.timestamp_sample = 0;
 
-			// preserve status.time_last_fuse
+		status.innovation = 0.f;
+		status.innovation_filtered = 0.f;
+		status.innovation_variance = status.observation_variance;
 
-			status.observation = 0;
+		status.test_ratio = 0.f;
-			status.observation_variance = 0;
+		status.test_ratio_filtered = 0.f;
 
-			status.innovation = 0;
+		status.innovation_rejected = false;
-			status.innovation_variance = 0;
+		status.fused = true;
-			status.test_ratio = INFINITY;
-
-			status.innovation_rejected = true;
-			status.fused = false;
-		}
 	}
 
-	template <typename T>
+	// helper used for populating and filtering estimator aid source struct for logging
-	void resetEstimatorAidStatus(T &status) const
+	void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t &timestamp_sample,
-	{
+				   const float &observation, const float &observation_variance,
-		// only bother resetting if timestamp_sample is set
+				   const float &innovation, const float &innovation_variance,
-		if (status.timestamp_sample != 0) {
+				   float innovation_gate = 1.f) const
-			status.timestamp_sample = 0;
-
-			// preserve status.time_last_fuse
-
-			for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
-				status.observation[i] = 0;
-				status.observation_variance[i] = 0;
-
-				status.innovation[i] = 0;
-				status.innovation_variance[i] = 0;
-				status.test_ratio[i] = INFINITY;
-			}
-
-			status.innovation_rejected = true;
-			status.fused = false;
-		}
-	}
-
-	void setEstimatorAidStatusTestRatio(estimator_aid_source1d_s &status, float innovation_gate) const
-	{
-		if (PX4_ISFINITE(status.innovation)
-		    && PX4_ISFINITE(status.innovation_variance)
-		    && (status.innovation_variance > 0.f)
-		   ) {
-			status.test_ratio = sq(status.innovation) / (sq(innovation_gate) * status.innovation_variance);
-			status.innovation_rejected = (status.test_ratio > 1.f);
-
-		} else {
-			status.test_ratio = INFINITY;
-			status.innovation_rejected = true;
-		}
-	}
-
-	template <typename T>
-	void setEstimatorAidStatusTestRatio(T &status, float innovation_gate) const
 	{
 		bool innovation_rejected = false;
 
-		for (size_t i = 0; i < (sizeof(status.test_ratio) / sizeof(status.test_ratio[0])); i++) {
+		const float test_ratio = sq(innovation) / (sq(innovation_gate) * innovation_variance);
-			if (PX4_ISFINITE(status.innovation[i])
-			    && PX4_ISFINITE(status.innovation_variance[i])
-			    && (status.innovation_variance[i] > 0.f)
-			   ) {
-				status.test_ratio[i] = sq(status.innovation[i]) / (sq(innovation_gate) * status.innovation_variance[i]);
 
-				if (status.test_ratio[i] > 1.f) {
+		if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) {
-					innovation_rejected = true;
+
+			const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f);
+
+			static constexpr float tau = 0.5f;
+			const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f);
+
+			// test_ratio_filtered
+			if (PX4_ISFINITE(status.test_ratio_filtered)) {
+				status.test_ratio_filtered += alpha * (matrix::sign(innovation) * test_ratio - status.test_ratio_filtered);
+
+			} else {
+				// otherwise, init the filtered test ratio
+				status.test_ratio_filtered = test_ratio;
+			}
+
+			// innovation_filtered
+			if (PX4_ISFINITE(status.innovation_filtered)) {
+				status.innovation_filtered += alpha * (innovation - status.innovation_filtered);
+
+			} else {
+				// otherwise, init the filtered innovation
+				status.innovation_filtered = innovation;
+			}
+
+
+			// limit extremes in filtered values
+			static constexpr float kNormalizedInnovationLimit = 2.f;
+			static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);
+
+			if (test_ratio > kTestRatioLimit) {
+
+				status.test_ratio_filtered = math::constrain(status.test_ratio_filtered, -kTestRatioLimit, kTestRatioLimit);
+
+				const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance);
+				status.innovation_filtered = math::constrain(status.innovation_filtered, -innov_limit, innov_limit);
 			}
 
 		} else {
-				status.test_ratio[i] = INFINITY;
+			// invalid timestamp_sample, reset
+			status.test_ratio_filtered = test_ratio;
+			status.innovation_filtered = innovation;
+		}
+
+		status.test_ratio = test_ratio;
+
+		status.observation = observation;
+		status.observation_variance = observation_variance;
+
+		status.innovation = innovation;
+		status.innovation_variance = innovation_variance;
+
+		if ((test_ratio > 1.f)
+		    || !PX4_ISFINITE(test_ratio)
+		    || !PX4_ISFINITE(status.innovation)
+		    || !PX4_ISFINITE(status.innovation_variance)
+		   ) {
+			innovation_rejected = true;
+		}
+
+		status.timestamp_sample = timestamp_sample;
+
+		// if any of the innovations are rejected, then the overall innovation is rejected
+		status.innovation_rejected = innovation_rejected;
+
+		// reset
+		status.fused = false;
+	}
+
+	// state was reset to aid source, keep observation and update all other fields appropriately (zero innovation, etc)
+	template <typename T>
+	void resetAidSourceStatusZeroInnovation(T &status) const
+	{
+		status.time_last_fuse = _time_delayed_us;
+
+		for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
+			status.innovation[i] = 0.f;
+			status.innovation_filtered[i] = 0.f;
+			status.innovation_variance[i] = status.observation_variance[i];
+
+			status.test_ratio[i] = 0.f;
+			status.test_ratio_filtered[i] = 0.f;
+		}
+
+		status.innovation_rejected = false;
+		status.fused = true;
+	}
+
+	// helper used for populating and filtering estimator aid source struct for logging
+	template <typename T, typename S>
+	void updateAidSourceStatus(T &status, const uint64_t &timestamp_sample,
+				   const S &observation, const S &observation_variance,
+				   const S &innovation, const S &innovation_variance,
+				   float innovation_gate = 1.f) const
+	{
+		bool innovation_rejected = false;
+
+		const float dt_s = math::constrain((timestamp_sample - status.timestamp_sample) * 1e-6f, 0.001f, 1.f);
+
+		static constexpr float tau = 0.5f;
+		const float alpha = math::constrain(dt_s / (dt_s + tau), 0.f, 1.f);
+
+		for (size_t i = 0; i < (sizeof(status.observation) / sizeof(status.observation[0])); i++) {
+
+			const float test_ratio = sq(innovation(i)) / (sq(innovation_gate) * innovation_variance(i));
+
+			if ((status.timestamp_sample > 0) && (timestamp_sample > status.timestamp_sample)) {
+
+				// test_ratio_filtered
+				if (PX4_ISFINITE(status.test_ratio_filtered[i])) {
+					status.test_ratio_filtered[i] += alpha * (matrix::sign(innovation(i)) * test_ratio - status.test_ratio_filtered[i]);
+
+				} else {
+					// otherwise, init the filtered test ratio
+					status.test_ratio_filtered[i] = test_ratio;
+				}
+
+				// innovation_filtered
+				if (PX4_ISFINITE(status.innovation_filtered[i])) {
+					status.innovation_filtered[i] += alpha * (innovation(i) - status.innovation_filtered[i]);
+
+				} else {
+					// otherwise, init the filtered innovation
+					status.innovation_filtered[i] = innovation(i);
+				}
+
+				// limit extremes in filtered values
+				static constexpr float kNormalizedInnovationLimit = 2.f;
+				static constexpr float kTestRatioLimit = sq(kNormalizedInnovationLimit);
+
+				if (test_ratio > kTestRatioLimit) {
+
+					status.test_ratio_filtered[i] = math::constrain(status.test_ratio_filtered[i], -kTestRatioLimit, kTestRatioLimit);
+
+					const float innov_limit = kNormalizedInnovationLimit * innovation_gate * sqrtf(innovation_variance(i));
+					status.innovation_filtered[i] = math::constrain(status.innovation_filtered[i], -innov_limit, innov_limit);
+				}
+
+			} else {
+				// invalid timestamp_sample, reset
+				status.test_ratio_filtered[i] = test_ratio;
+				status.innovation_filtered[i] = innovation(i);
+			}
+
+			status.test_ratio[i] = test_ratio;
+
+			status.observation[i] = observation(i);
+			status.observation_variance[i] = observation_variance(i);
+
+			status.innovation[i] = innovation(i);
+			status.innovation_variance[i] = innovation_variance(i);
+
+			if ((test_ratio > 1.f)
+			    || !PX4_ISFINITE(test_ratio)
+			    || !PX4_ISFINITE(status.innovation[i])
+			    || !PX4_ISFINITE(status.innovation_variance[i])
+			   ) {
 				innovation_rejected = true;
 			}
 		}
 
+		status.timestamp_sample = timestamp_sample;
+
 		// if any of the innovations are rejected, then the overall innovation is rejected
 		status.innovation_rejected = innovation_rejected;
+
+		// reset
+		status.fused = false;
 	}
 
 	ZeroGyroUpdate _zero_gyro_update{};

src/modules/ekf2/EKF/estimator_interface.h

@@ -401,7 +401,6 @@ protected:
 
 # if defined(CONFIG_EKF2_GNSS_YAW)
 	// innovation consistency check monitoring ratios
-	AlphaFilter<float> _gnss_yaw_signed_test_ratio_lpf{0.1f}; // average signed test ratio used to detect a bias in the state
 	uint64_t _time_last_gps_yaw_buffer_push{0};
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS

src/modules/ekf2/EKF/position_fusion.cpp

@@ -33,50 +33,27 @@
 
 #include "ekf.h"
 
-void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
+void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us,
-		const float innov_gate, estimator_aid_source2d_s &aid_src) const
+		const float observation, const float observation_variance, const float innovation_gate) const
 {
-	resetEstimatorAidStatus(aid_src);
+	float innovation = _state.pos(2) - observation;
+	float innovation_variance = getStateVariance<State::pos>()(2) + observation_variance;
 
-	for (int i = 0; i < 2; i++) {
+	updateAidSourceStatus(aid_src, time_us,
-		aid_src.observation[i] = obs(i);
+				 observation, observation_variance,
-		aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
+				 innovation, innovation_variance,
-
+				 innovation_gate);
-		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
-		const int state_index = State::pos.idx + i;
-		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
-	}
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-	aid_src.timestamp_sample = time_us;
-}
-
-void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const float obs, const float obs_var,
-		const float innov_gate, estimator_aid_source1d_s &aid_src) const
-{
-	resetEstimatorAidStatus(aid_src);
-
-	aid_src.observation = obs;
-	aid_src.innovation = _state.pos(2) - aid_src.observation;
-
-	aid_src.observation_variance = math::max(sq(0.01f), obs_var);
-	aid_src.innovation_variance = P(State::pos.idx + 2, State::pos.idx + 2) + aid_src.observation_variance;
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
 
 	// z special case if there is bad vertical acceleration data, then don't reject measurement,
 	// but limit innovation to prevent spikes that could destabilise the filter
 	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
-		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
+		const float innov_limit = innovation_gate * sqrtf(aid_src.innovation_variance);
 		aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
 		aid_src.innovation_rejected = false;
 	}
-
-	aid_src.timestamp_sample = time_us;
 }
 
-void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
+bool Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 {
 	// x & y
 	if (!aid_src.innovation_rejected
@@ -91,9 +68,11 @@ void Ekf::fuseHorizontalPosition(estimator_aid_source2d_s &aid_src)
 	} else {
 		aid_src.fused = false;
 	}
+
+	return aid_src.fused;
 }
 
-void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
+bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
 {
 	// z
 	if (!aid_src.innovation_rejected
@@ -107,6 +86,8 @@ void Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src)
 	} else {
 		aid_src.fused = false;
 	}
+
+	return aid_src.fused;
 }
 
 void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var)
@@ -200,11 +181,3 @@ 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::resetHorizontalPositionToExternal(const Vector2f &new_horiz_pos, float horiz_accuracy)
-{
-	ECL_INFO("reset position to external observation");
-	_information_events.flags.reset_pos_to_ext_obs = true;
-
-	resetHorizontalPositionTo(new_horiz_pos, sq(horiz_accuracy));
-}

src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp

@@ -205,8 +205,6 @@ void Ekf::stopHaglRngFusion()
 		ECL_INFO("stopping HAGL range fusion");
 
 		// reset flags
-		resetEstimatorAidStatus(_aid_src_terrain_range_finder);
-
 		_innov_check_fail_status.flags.reject_hagl = false;
 
 		_hagl_sensor_status.flags.range_finder = false;
@@ -221,30 +219,19 @@ void Ekf::updateHaglRng(estimator_aid_source1d_s &aid_src) const
 	// predict the hagl from the vehicle position and terrain height
 	const float pred_hagl = _terrain_vpos - _state.pos(2);
 
-	// calculate the innovation
-	const float hagl_innov = pred_hagl - meas_hagl;
-
 	// calculate the observation variance adding the variance of the vehicles own height uncertainty
 	const float obs_variance = getRngVar();
 
 	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
 	const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);
 
-	// perform an innovation consistency check and only fuse data if it passes
+	updateAidSourceStatus(aid_src,
-	const float innov_gate = fmaxf(_params.range_innov_gate, 1.0f);
+		_time_delayed_us,                          // sample timestamp
-
+		meas_hagl,                                 // observation
-
+		obs_variance,                              // observation variance
-	aid_src.timestamp_sample = _time_delayed_us; // TODO
+		pred_hagl - meas_hagl,                     // innovation
-
+		hagl_innov_var,                            // innovation variance
-	aid_src.observation = meas_hagl;
+		math::max(_params.range_innov_gate, 1.f)); // innovation gate
-	aid_src.observation_variance = obs_variance;
-
-	aid_src.innovation = hagl_innov;
-	aid_src.innovation_variance = hagl_innov_var;
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-	aid_src.fused = false;
 }
 
 void Ekf::fuseHaglRng(estimator_aid_source1d_s &aid_src)
@@ -329,7 +316,6 @@ void Ekf::stopHaglFlowFusion()
 		ECL_INFO("stopping HAGL flow fusion");
 
 		_hagl_sensor_status.flags.flow = false;
-		resetEstimatorAidStatus(_aid_src_terrain_optical_flow);
 	}
 }
 
@@ -367,9 +353,6 @@ void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow)
 		return;
 	}
 
-	// run the innovation consistency check and record result
-	setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f));
-
 	_innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f);
 	_innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f);
 

src/modules/ekf2/EKF/velocity_fusion.cpp

@@ -33,53 +33,7 @@
 
 #include "ekf.h"
 
-void Ekf::updateHorizontalVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &obs, const Vector2f &obs_var,
+bool Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
-		const float innov_gate, estimator_aid_source2d_s &aid_src) const
-{
-	resetEstimatorAidStatus(aid_src);
-
-	for (int i = 0; i < 2; i++) {
-		aid_src.observation[i] = obs(i);
-		aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
-
-		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
-		const int state_index = State::vel.idx + i;
-		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
-	}
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-	aid_src.timestamp_sample = time_us;
-}
-
-void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &obs, const Vector3f &obs_var,
-				     const float innov_gate, estimator_aid_source3d_s &aid_src) const
-{
-	resetEstimatorAidStatus(aid_src);
-
-	for (int i = 0; i < 3; i++) {
-		aid_src.observation[i] = obs(i);
-		aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
-
-		aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
-		const int state_index = State::vel.idx + i;
-		aid_src.innovation_variance[i] = P(state_index, state_index) + aid_src.observation_variance[i];
-	}
-
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
-
-	// vz special case if there is bad vertical acceleration data, then don't reject measurement,
-	// but limit innovation to prevent spikes that could destabilise the filter
-	if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
-		const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance[2]);
-		aid_src.innovation[2] = math::constrain(aid_src.innovation[2], -innov_limit, innov_limit);
-		aid_src.innovation_rejected = false;
-	}
-
-	aid_src.timestamp_sample = time_us;
-}
-
-void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
 {
 	// vx, vy
 	if (!aid_src.innovation_rejected
@@ -94,9 +48,11 @@ void Ekf::fuseHorizontalVelocity(estimator_aid_source2d_s &aid_src)
 	} else {
 		aid_src.fused = false;
 	}
+
+	return aid_src.fused;
 }
 
-void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
+bool Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 {
 	// vx, vy, vz
 	if (!aid_src.innovation_rejected
@@ -113,6 +69,8 @@ void Ekf::fuseVelocity(estimator_aid_source3d_s &aid_src)
 	} else {
 		aid_src.fused = false;
 	}
+
+	return aid_src.fused;
 }
 
 void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f &new_horz_vel_var)

src/modules/ekf2/EKF/yaw_fusion.cpp

@@ -37,23 +37,8 @@
 
 #include <mathlib/mathlib.h>
 
-// update quaternion states and covariances using the yaw innovation and yaw observation variance
-bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status)
-{
-	VectorState H_YAW;
-	sym::ComputeYawInnovVarAndH(_state.vector(), P, aid_src_status.observation_variance, &aid_src_status.innovation_variance, &H_YAW);
-
-	return fuseYaw(aid_src_status, H_YAW);
-}
-
 bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW)
 {
-	// define the innovation gate size
-	float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
-
-	// innovation test ratio
-	setEstimatorAidStatusTestRatio(aid_src_status, gate_sigma);
-
 	// check if the innovation variance calculation is badly conditioned
 	if (aid_src_status.innovation_variance >= aid_src_status.observation_variance) {
 		// the innovation variance contribution from the state covariances is not negative, no fault
@@ -83,7 +68,7 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 		Kfusion(row) *= heading_innov_var_inv;
 	}
 
-	// set the magnetometer unhealthy if the test fails
+	// set the heading unhealthy if the test fails
 	if (aid_src_status.innovation_rejected) {
 		_innov_check_fail_status.flags.reject_yaw = true;
 
@@ -97,7 +82,8 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 			// constrain the innovation to the maximum set by the gate
 			// we need to delay this forced fusion to avoid starting it
 			// immediately after touchdown, when the drone is still armed
-			float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
+			const float gate_sigma = math::max(_params.heading_innov_gate, 1.f);
+			const float gate_limit = sqrtf((sq(gate_sigma) * aid_src_status.innovation_variance));
 			aid_src_status.innovation = math::constrain(aid_src_status.innovation, -gate_limit, gate_limit);
 
 			// also reset the yaw gyro variance to converge faster and avoid
@@ -122,13 +108,10 @@ bool Ekf::fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H
 		_fault_status.flags.bad_hdg = false;
 
 		return true;
-
-	} else {
-		_fault_status.flags.bad_hdg = true;
 	}
 
 	// otherwise
-	aid_src_status.fused = false;
+	_fault_status.flags.bad_hdg = true;
 	return false;
 }