diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index a863a5b3e6..c604e416b8 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -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(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); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 89b3fb6f63..d265a0bc26 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/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(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) { diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index 1943f5015e..2d9a88f3ae 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/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(GnssMode::kDeadReckoning); +} + void EkfWrapper::enableGpsHeadingFusion() { _ekf_params->ekf2_gps_ctrl |= static_cast(GnssCtrl::YAW); diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 69b72475dc..6601670eaf 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -78,6 +78,8 @@ public: void enableGpsFusion(); void disableGpsFusion(); bool isIntendingGpsFusion() const; + bool isGnssFaultDetected() const; + void setGnssDeadReckonMode(); void enableGpsHeadingFusion(); void disableGpsHeadingFusion(); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 47c0d3a0e2..debfec6560 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/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()); +}