ekf2: stop GNSS altittude and velocity aiding when gnss_fault is set

Author: bresch

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

@@ -76,18 +76,11 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 						measurement_var + bias_est.getBiasVar(),
 						math::max(_params.ekf2_gps_p_gate, 1.f));
 
-		// update the bias estimator before updating the main filter but after
-		// using its current state to compute the vertical position innovation
-		if (measurement_valid) {
-			bias_est.setMaxStateNoise(sqrtf(measurement_var));
-			bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
-			bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
-		}
-
 		// determine if we should use height aiding
 		const bool common_conditions_passing = measurement_valid
 						       && _local_origin_lat_lon.isInitialized()
-						       && _gnss_checks.passed();
+						       && _gnss_checks.passed()
+						       && !_control_status.flags.gnss_fault;
 
 		const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VPOS))
 				&& common_conditions_passing;
@@ -103,6 +96,12 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
 		if (_control_status.flags.gps_hgt) {
 			if (continuing_conditions_passing) {
 
+				// update the bias estimator before updating the main filter but after
+				// using its current state to compute the vertical position innovation
+				bias_est.setMaxStateNoise(sqrtf(measurement_var));
+				bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
+				bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
+
 				fuseVerticalPosition(aid_src);
 
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);

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

@@ -123,7 +123,8 @@ void Ekf::controlGnssVelFusion(estimator_aid_source3d_s &aid_src, const bool for
 {
 	const bool continuing_conditions_passing = (_params.ekf2_gps_ctrl & static_cast<int32_t>(GnssCtrl::VEL))
 			&& _control_status.flags.tilt_align
-			&& _control_status.flags.yaw_align;
+			&& _control_status.flags.yaw_align
+			&& !_control_status.flags.gnss_fault;
 	const bool starting_conditions_passing = continuing_conditions_passing && _gnss_checks.passed();
 
 	if (_control_status.flags.gnss_vel) {

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp

@@ -120,6 +120,16 @@ bool EkfWrapper::isIntendingGpsFusion() const
 	return _ekf->control_status_flags().gnss_vel || _ekf->control_status_flags().gnss_pos;
 }
 
+bool EkfWrapper::isGnssFaultDetected() const
+{
+	return _ekf->control_status_flags().gnss_fault;
+}
+
+void EkfWrapper::setGnssDeadReckonMode()
+{
+	_ekf_params->ekf2_gps_mode = static_cast<int32_t>(GnssMode::kDeadReckoning);
+}
+
 void EkfWrapper::enableGpsHeadingFusion()
 {
 	_ekf_params->ekf2_gps_ctrl |= static_cast<int32_t>(GnssCtrl::YAW);

src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h

@@ -78,6 +78,8 @@ class EkfWrapper
 	void enableGpsFusion();
 	void disableGpsFusion();
 	bool isIntendingGpsFusion() const;
+	bool isGnssFaultDetected() const;
+	void setGnssDeadReckonMode();
 
 	void enableGpsHeadingFusion();
 	void disableGpsHeadingFusion();

src/modules/ekf2/test/test_EKF_gps.cpp

@@ -228,3 +228,54 @@ TEST_F(EkfGpsTest, altitudeDrift)
 	// THEN: the baro and local position should follow it
 	EXPECT_LT(fabsf(baro_innov), 0.1f);
 }
+
+TEST_F(EkfGpsTest, gnssJumpDetectionDRMode)
+{
+	// Dead-reckoning mode allows the EKF to reject GNSS data if another source
+	// of horizontal aiding is used (e.g.: airspped)
+	_ekf_wrapper.setGnssDeadReckonMode();
+	_ekf_wrapper.enableGpsHeightFusion();
+
+	// GIVEN:EKF that fuses GNSS and airspeed
+	const Vector3f previous_position = _ekf->getPosition();
+	const Vector3f simulated_velocity_earth(1.f, 0.5f, 0.0f);
+	const Vector2f airspeed_body(15.f, 0.0f);
+	_sensor_simulator._gps.setVelocity(simulated_velocity_earth);
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_is_fixed_wing(true);
+
+	_sensor_simulator.startAirspeedSensor();
+	_sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0));
+	_sensor_simulator.runSeconds(1);
+
+	EXPECT_TRUE(_ekf_wrapper.isWindVelocityEstimated());
+
+	// AND: simulate jump in position
+	const Vector3f simulated_position_change(500.0f, -1.0f, 0.f);
+	_sensor_simulator._gps.stepHorizontalPositionByMeters(
+		Vector2f(simulated_position_change));
+	_sensor_simulator.runSeconds(15);
+
+	// THEN: the GNSS jump should trigger the fault detection
+	// and stop the fusion (including height and velocity)
+	const Vector3f estimated_position = _ekf->getPosition();
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+	EXPECT_TRUE(_ekf_wrapper.isGnssFaultDetected());
+
+	// The position is obtained through dead-reckoning
+	EXPECT_TRUE(isEqual(estimated_position,
+			    previous_position, 25.f));
+
+	// BUT WHEN: the position data goes back to normal
+	_sensor_simulator._gps.stepHorizontalPositionByMeters(
+		-Vector2f(simulated_position_change) + Vector2f(20.f, 10.f));
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: the fault is cleared an dfusion restarts
+	EXPECT_FALSE(_ekf_wrapper.isGnssFaultDetected());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+}