diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 6e9c9bb540..445203e7a0 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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(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(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 - diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 18de839293..108716feef 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -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_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)};