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

fix(ekf2): prefilter baro/mag samples before rate-limited fusion

Browse Source 
The EKF2 rate limiter was decimating (picking one sample, dropping the
rest) which loses the noise reduction that the old publish-side
accumulate-and-average provided. Accumulate all incoming samples between
fusion intervals and push the average to the EKF instead.
This commit is contained in:
Jacob Dahl
2026-03-31 23:21:21 -08:00
parent dc4cf031f9
commit 2a59d2e1d2
2 changed files with 31 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF2.cpp
+23 -4
View File
@@ -2179,7 +2179,12 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
_ekf.set_air_density(airdata.rho);
// Rate-limit baro data to reduce EKF fusion CPU load
// Accumulate baro samples and average before pushing to EKF at the
// configured fusion rate. This preserves the noise reduction of the
// old publish-side averaging while fixing the rate-aliasing bug.
_baro_prefilter_sum += airdata.baro_alt_meter;
_baro_prefilter_count++;
const float baro_rate_hz = _param_ekf2_baro_rate.get();
const hrt_abstime baro_interval_us = (baro_rate_hz > 0.f)
? static_cast<hrt_abstime>(1e6f / baro_rate_hz)
@@ -2187,8 +2192,13 @@ void EKF2::UpdateBaroSample(ekf2_timestamps_s &ekf2_timestamps)
if (reset || (baro_interval_us == 0)
|| (airdata.timestamp_sample >= _last_baro_ekf_timestamp + baro_interval_us)) {
_ekf.setBaroData(baroSample{airdata.timestamp_sample, airdata.baro_alt_meter, reset});
const float baro_avg = _baro_prefilter_sum / _baro_prefilter_count;
_ekf.setBaroData(baroSample{airdata.timestamp_sample, baro_avg, reset});
_last_baro_ekf_timestamp = airdata.timestamp_sample;
_baro_prefilter_sum = 0.f;
_baro_prefilter_count = 0;
}
ekf2_timestamps.vehicle_air_data_timestamp_rel = (int16_t)((int64_t)airdata.timestamp / 100 -
@@ -2535,7 +2545,11 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
_mag_cal = {};
}
// Rate-limit mag data to reduce EKF fusion CPU load (3-axis fusion is expensive)
// Accumulate mag samples and average before pushing to EKF at the
// configured fusion rate.
_mag_prefilter_sum += Vector3f{magnetometer.magnetometer_ga};
_mag_prefilter_count++;
const float mag_rate_hz = _param_ekf2_mag_rate.get();
const hrt_abstime mag_interval_us = (mag_rate_hz > 0.f)
? static_cast<hrt_abstime>(1e6f / mag_rate_hz)
@@ -2543,8 +2557,13 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
if (reset || (mag_interval_us == 0)
|| (magnetometer.timestamp_sample >= _last_mag_ekf_timestamp + mag_interval_us)) {
_ekf.setMagData(magSample{magnetometer.timestamp_sample, Vector3f{magnetometer.magnetometer_ga}, reset});
const Vector3f mag_avg = _mag_prefilter_sum / _mag_prefilter_count;
_ekf.setMagData(magSample{magnetometer.timestamp_sample, mag_avg, reset});
_last_mag_ekf_timestamp = magnetometer.timestamp_sample;
_mag_prefilter_sum.zero();
_mag_prefilter_count = 0;
}
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = (int16_t)((int64_t)magnetometer.timestamp / 100 -
src/modules/ekf2/EKF2.hpp
+8
View File
@@ -311,6 +311,11 @@ private:
hrt_abstime _status_mag_pub_last{0};
hrt_abstime _last_mag_ekf_timestamp{0};
// Prefilter accumulators: average samples between fusion intervals to
// preserve noise reduction without the aliasing of the old publish-side rate limiter
Vector3f _mag_prefilter_sum{};
int _mag_prefilter_count{0};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_mag_pub{ORB_ID(estimator_aid_src_mag)};
@@ -355,6 +360,9 @@ private:
hrt_abstime _status_baro_hgt_pub_last{0};
hrt_abstime _last_baro_ekf_timestamp{0};
float _baro_prefilter_sum{0.f};
int _baro_prefilter_count{0};
float _last_baro_bias_published{};
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};