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

EKF2: reset aid src when resetting state to measurement

Browse Source 
Filtered innovations and test ratios can be large before the reset and
would trigger pre-flight warnings
This commit is contained in:
bresch
2024-12-13 14:26:57 +01:00
committed by Mathieu Bresciani
parent eb829676b0
commit fad9ae855d
7 changed files with 29 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp
+1
View File
@@ -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, pos_var,
sq(sample.epv))) {
ekf.resetAidSourceStatusZeroInnovation(aid_src);
ekf.enableControlStatusAuxGpos();
_reset_counters.lat_lon = sample.lat_lon_reset_counter;
_state = State::active;
src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp
+2
View File
@@ -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()) {
@@ -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);
src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp
+2
View File
@@ -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;
@@ -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);
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+6
View File
@@ -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) {
@@ -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();
@@ -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);
}
}
}
@@ -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()) {
@@ -198,6 +202,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
} else {
resetTerrainToRng(aid_src);
resetAidSourceStatusZeroInnovation(aid_src);
}
}
@@ -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;
src/modules/ekf2/EKF/ekf.h
+1 -14
View File
@@ -1016,20 +1016,7 @@ private:
#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,
src/modules/ekf2/EKF/ekf_helper.cpp
+15
View File
@@ -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,
src/modules/ekf2/test/test_EKF_height_fusion.cpp
+2 -4
View File
@@ -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)