Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

EKF2: fix false pre-flight failure report #24050

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
+29 −18
Open

EKF2: fix false pre-flight failure report #24050

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,6 +112,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
// Try to initialize using measurement
if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
sample.epv)) {
ekf.resetAidSourceStatusZeroInnovation(aid_src);
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::active;
Expand Down
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
_information_events.flags.reset_hgt_to_baro = true;
resetAltitudeTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var);
bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState());
resetAidSourceStatusZeroInnovation(aid_src);

// reset vertical velocity if no valid sources available
if (!isVerticalVelocityAidingActive()) {
Expand Down Expand Up @@ -165,6 +166,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
_information_events.flags.reset_hgt_to_baro = true;
initialiseAltitudeTo(measurement, measurement_var);
bias_est.reset();
resetAidSourceStatusZeroInnovation(aid_src);

} else {
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
Expand Down
2 changes: 2 additions & 0 deletions src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)
_information_events.flags.reset_hgt_to_gps = true;
resetAltitudeTo(measurement, measurement_var);
bias_est.setBias(-_gpos.altitude() + measurement);
resetAidSourceStatusZeroInnovation(aid_src);

aid_src.time_last_fuse = _time_delayed_us;

Expand All @@ -131,6 +132,7 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample)

initialiseAltitudeTo(measurement, measurement_var);
bias_est.reset();
resetAidSourceStatusZeroInnovation(aid_src);

} else {
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
Expand Down
6 changes: 6 additions & 0 deletions src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)

if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}

} else if (do_range_aid) {
Expand All @@ -150,6 +151,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_information_events.flags.reset_hgt_to_rng = true;
resetAltitudeTo(aid_src.observation, aid_src.observation_variance);
_state.terrain = 0.f;
resetAidSourceStatusZeroInnovation(aid_src);
_control_status.flags.rng_hgt = true;
stopRngTerrFusion();

Expand All @@ -163,6 +165,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)

if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
}
}
Expand All @@ -181,6 +184,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)

_information_events.flags.reset_hgt_to_rng = true;
resetAltitudeTo(aid_src.observation - _state.terrain);
resetAidSourceStatusZeroInnovation(aid_src);

// reset vertical velocity if no valid sources available
if (!isVerticalVelocityAidingActive()) {
Expand All @@ -198,6 +202,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)

} else {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
}

Expand All @@ -218,6 +223,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
} else {
if (aid_src.innovation_rejected) {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}

_control_status.flags.rng_terrain = true;
Expand Down
15 changes: 1 addition & 14 deletions src/modules/ekf2/EKF/ekf.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1018,20 +1018,7 @@ class Ekf final : public EstimatorInterface
#endif // CONFIG_EKF2_EXTERNAL_VISION

// 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
{
status.time_last_fuse = _time_delayed_us;

status.innovation = 0.f;
status.innovation_filtered = 0.f;
status.innovation_variance = status.observation_variance;

status.test_ratio = 0.f;
status.test_ratio_filtered = 0.f;

status.innovation_rejected = false;
status.fused = true;
}
void resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const;

// helper used for populating and filtering estimator aid source struct for logging
void updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t &timestamp_sample,
Expand Down
15 changes: 15 additions & 0 deletions src/modules/ekf2/EKF/ekf_helper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1131,6 +1131,21 @@ bool Ekf::measurementUpdate(VectorState &K, const VectorState &H, const float R,
return true;
}

void Ekf::resetAidSourceStatusZeroInnovation(estimator_aid_source1d_s &status) const
{
status.time_last_fuse = _time_delayed_us;

status.innovation = 0.f;
status.innovation_filtered = 0.f;
status.innovation_variance = status.observation_variance;

status.test_ratio = 0.f;
status.test_ratio_filtered = 0.f;

status.innovation_rejected = false;
status.fused = true;
}

void Ekf::updateAidSourceStatus(estimator_aid_source1d_s &status, const uint64_t &timestamp_sample,
const float &observation, const float &observation_variance,
const float &innovation, const float &innovation_variance,
Expand Down
6 changes: 2 additions & 4 deletions src/modules/ekf2/test/test_EKF_height_fusion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -345,12 +345,10 @@ TEST_F(EkfHeightFusionTest, baroRefAllHgtFailReset)
// Also check the reset counters to make sure the reset logic triggered
reset_logging_checker.capturePostResetState();

EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));

// The velocity does not reset as baro only provides height measurement
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0));

// The height resets twice in a row as the baro innovation is not corrected after a height
// reset and triggers a new reset at the next iteration
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2));
}

TEST_F(EkfHeightFusionTest, changeEkfOriginAlt)
Expand Down
Loading