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: rng don't allow bad measurement in bad_acc_vertical (#25636)

Browse Source 
- if bad vertical acceleration is detected there's an emergency case
   where rejected range finder observations are allowed to be used, but
   this still can't happen if the sample itself is known to be bag
This commit is contained in:
Daniel Agar 2025-09-30 15:14:04 -04:00 committed by GitHub
parent 4d2170c13e
commit e3309b9f87
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 25 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -100,9 +100,33 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
if (rng_data_ready && _range_sensor.getSampleAddress()) {
updateRangeHagl(aid_src);
const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
const float measurement_variance = getRngVar();
float innovation_variance;
sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
updateAidSourceStatus(aid_src,
_range_sensor.getSampleAddress()->time_us, // sample timestamp
measurement, // observation
measurement_variance, // observation variance
getHagl() - measurement, // innovation
innovation_variance, // innovation variance
innov_gate); // innovation gate
const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance);
// z special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected
&& measurement_valid && _range_sensor.isDataHealthy()
) {
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
aid_src.innovation_rejected = false;
}
const bool continuing_conditions_passing = ((_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED))
|| (_params.ekf2_rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)))
&& _control_status.flags.tilt_align
@ -247,32 +271,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
}
}
void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src)
{
const float measurement = math::max(_range_sensor.getDistBottom(), _params.ekf2_min_rng);
const float measurement_variance = getRngVar();
float innovation_variance;
sym::ComputeHaglInnovVar(P, measurement_variance, &innovation_variance);
const float innov_gate = math::max(_params.ekf2_rng_gate, 1.f);
updateAidSourceStatus(aid_src,
_range_sensor.getSampleAddress()->time_us, // sample timestamp
measurement, // observation
measurement_variance, // observation variance
getHagl() - measurement, // innovation
innovation_variance, // innovation variance
innov_gate); // innovation gate
// z special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && aid_src.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(aid_src.innovation_variance);
aid_src.innovation = math::constrain(aid_src.innovation, -innov_limit, innov_limit);
aid_src.innovation_rejected = false;
}
}
float Ekf::getRngVar() const
{
return fmaxf(

1
src/modules/ekf2/EKF/ekf.h
View File

@ -765,7 +765,6 @@ private:
# if defined(CONFIG_EKF2_RANGE_FINDER)
// update the terrain vertical position estimate using a height above ground measurement from the range finder
bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain);
void updateRangeHagl(estimator_aid_source1d_s &aid_src);
void resetTerrainToRng(estimator_aid_source1d_s &aid_src);
float getRngVar() const;
# endif // CONFIG_EKF2_RANGE_FINDER