From 480c232bfd761784435bae4998702fada831e030 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 16 Dec 2024 12:01:36 +0100 Subject: [PATCH] ekf2: use alpha filter class --- .../ekf2/EKF/aid_sources/magnetometer/mag_control.cpp | 2 +- src/modules/ekf2/EKF/covariance.cpp | 5 +++-- src/modules/ekf2/EKF/ekf.cpp | 11 ++--------- src/modules/ekf2/EKF/ekf.h | 9 +++++++-- 4 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index 85979a2447..ae1142c86b 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -482,7 +482,7 @@ void Ekf::checkMagHeadingConsistency(const magSample &mag_sample) // Check if there has been enough change in horizontal velocity to make yaw observable const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos; - if (using_ne_aiding && (_accel_lpf_NE.norm() > _params.mag_acc_gate)) { + if (using_ne_aiding && (_accel_horiz_lpf.getState().longerThan(_params.mag_acc_gate))) { // yaw angle must be observable to consider consistency _control_status.flags.mag_heading_consistent = true; } diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index f148a99d91..0c9a5df6cb 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -202,8 +202,9 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_WIND) // wind vel: add process noise - float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); - float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; + const float height_rate = _height_rate_lpf.update(_state.vel(2), imu_delayed.delta_vel_dt); + const float wind_vel_nsd_scaled = _params.wind_vel_nsd * (1.f + _params.wind_vel_nsd_scaler * fabsf(height_rate)); + const float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; for (unsigned index = 0; index < State::wind_vel.dof; index++) { const unsigned i = State::wind_vel.idx + index; diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index a0df2a2ad7..febe46f90e 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -272,15 +272,8 @@ void Ekf::predictState(const imuSample &imu_delayed) // constrain states _state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit); - - // calculate a filtered horizontal acceleration with a 1 sec time constant - // this are used for manoeuvre detection elsewhere - const float alpha = 1.0f - imu_delayed.delta_vel_dt; - _accel_lpf_NE = _accel_lpf_NE * alpha + corrected_delta_vel_ef.xy(); - - // Calculate low pass filtered height rate - float alpha_height_rate_lpf = 0.1f * imu_delayed.delta_vel_dt; // 10 seconds time constant - _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; + // calculate a filtered horizontal acceleration this are used for manoeuvre detection elsewhere + _accel_horiz_lpf.update(corrected_delta_vel_ef.xy() / imu_delayed.delta_vel_dt, imu_delayed.delta_vel_dt); } bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude, diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index fc4aa0e41a..8df41e6ba8 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -490,8 +490,13 @@ private: Dcmf _R_to_earth{}; ///< transformation matrix from body frame to earth frame from last EKF prediction - Vector2f _accel_lpf_NE{}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) - float _height_rate_lpf{0.0f}; + static constexpr float _kAccelHorizLpfTimeConstant = 1.f; + AlphaFilter _accel_horiz_lpf{_kAccelHorizLpfTimeConstant}; ///< Low pass filtered horizontal earth frame acceleration (m/sec**2) + +#if defined(CONFIG_EKF2_WIND) + static constexpr float _kHeightRateLpfTimeConstant = 10.f; + AlphaFilter _height_rate_lpf{_kHeightRateLpfTimeConstant}; +#endif // CONFIG_EKF2_WIND SquareMatrixState P{}; ///< state covariance matrix