fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
bresch 2025-08-19 11:59:59 +02:00 committed by Mathieu Bresciani
parent db3f33760e
commit 4a697d0191
5 changed files with 73 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View File

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

3
src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp
View File

@ -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) {

10
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
View File

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

2
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
View File

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

51
src/modules/ekf2/test/test_EKF_gps.cpp
View File

@ -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());
}