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

@@ -1,5 +1,5 @@
-uint64 timestamp             # time since system start (microseconds)
-uint64 timestamp_sample      # the timestamp of the raw data (microseconds)
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 uint8 estimator_instance
 
@@ -11,11 +11,15 @@ float32[2] observation
 float32[2] observation_variance
 
 float32[2] innovation
+float32[2] innovation_filtered
+
 float32[2] innovation_variance
-float32[2] test_ratio
 
-bool innovation_rejected     # true if the observation has been rejected
-bool fused                   # true if the sample was successfully fused
+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
 
 # TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position
 # TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow estimator_aid_src_terrain_optical_flow

msg/EstimatorAidSource3d.msg

@@ -1,5 +1,5 @@
-uint64 timestamp             # time since system start (microseconds)
-uint64 timestamp_sample      # the timestamp of the raw data (microseconds)
+uint64 timestamp                # time since system start (microseconds)
+uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 uint8 estimator_instance
 
@@ -11,10 +11,14 @@ float32[3] observation
 float32[3] observation_variance
 
 float32[3] innovation
+float32[3] innovation_filtered
+
 float32[3] innovation_variance
-float32[3] test_ratio
 
-bool innovation_rejected     # true if the observation has been rejected
-bool fused                   # true if the sample was successfully fused
+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
 
 # TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag

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,11 +91,15 @@ 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
-		                                         && (is_airspeed_consistent || !_control_status.flags.wind || _control_status.flags.inertial_dead_reckoning);
+		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) {
 			if (continuing_conditions_passing) {
@@ -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);
-
-	aid_src.observation = airspeed_sample.true_airspeed;
-	aid_src.observation_variance = R;
-	aid_src.innovation = innov;
-	aid_src.innovation_variance = innov_var;
-
-	aid_src.timestamp_sample = airspeed_sample.time_us;
-
-	const float innov_gate = fmaxf(_params.tas_innov_gate, 1.f);
-	setEstimatorAidStatusTestRatio(aid_src, innov_gate);
+	sym::ComputeAirspeedInnovAndInnovVar(_state.vector(), P, airspeed_sample.true_airspeed, R, FLT_EPSILON,
+					     &innov, &innov_var);
+
+	updateAidSourceStatus(aid_src,
+				 airspeed_sample.time_us,                 // sample timestamp
+				 airspeed_sample.true_airspeed,           // observation
+				 R,                                       // observation variance
+				 innov,                                   // innovation
+				 innov_var,                               // innovation variance
+				 math::max(_params.tas_innov_gate, 1.f)); // innovation 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,23 +80,27 @@ 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,
-					position,                                   // observation
-					pos_obs_var,                                // observation variance
-					math::max(_param_ekf2_agp_gate.get(), 1.f), // innovation gate
-					aid_src);
+
+			ekf.updateAidSourceStatus(aid_src,
+						     sample.time_us,                                    // sample timestamp
+						     position,                                          // observation
+						     pos_obs_var,                                       // observation variance
+						     Vector2f(ekf.state().pos) - position,              // innovation
+						     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)
-						   && ekf.control_status_flags().yaw_align;
+						 && ekf.control_status_flags().yaw_align;
 		const bool continuing_conditions = starting_conditions
 						   && ekf.global_origin_valid();
 
 		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,15 +129,19 @@ 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;
 				}
 
 			} else {
 				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);
-
-			updateHorizontalVelocityAidSrcStatus(auxvel_sample_delayed.time_us, auxvel_sample_delayed.vel, auxvel_sample_delayed.velVar, fmaxf(_params.auxvel_gate, 1.f), _aid_src_aux_vel);
+			updateAidSourceStatus(_aid_src_aux_vel,
+						 sample.time_us,                                           // sample timestamp
+						 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,
-						   -(measurement - bias_est.getBias()),
-						   measurement_var + bias_est.getBiasVar(),
-						   innov_gate,
-						   aid_src);
+		updateVerticalPositionAidStatus(aid_src,
+						baro_sample.time_us,
+						-(measurement - bias_est.getBias()),      // observation
+						measurement_var + bias_est.getBiasVar(),  // observation variance
+						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,52 +125,57 @@ 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;
-		_aid_src_drag.observation_variance[axis_index] = R_ACC;
-		_aid_src_drag.innovation[axis_index] = pred_acc - mea_acc;
-		_aid_src_drag.innovation_variance[axis_index] = NAN; // reset
+		observation(axis_index) = mea_acc;
+		innovation(axis_index) = pred_acc - mea_acc;
 
 		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;
 			}
 
 		} 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 innov_gate = 5.f;
-		setEstimatorAidStatusTestRatio(_aid_src_drag, innov_gate);
+		const float test_ratio = sq(innovation(axis_index)) / (sq(innov_gate) * innovation_variance(axis_index));
 
 		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])
-		    && (_aid_src_drag.test_ratio[axis_index] < 1.f)
+		    && PX4_ISFINITE(innovation_variance(axis_index)) && PX4_ISFINITE(innovation(axis_index))
+		    && (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;