diff --git a/AlphaFilter/AlphaFilter.hpp b/AlphaFilter/AlphaFilter.hpp index 507fffb905..ae2c344cc6 100644 --- a/AlphaFilter/AlphaFilter.hpp +++ b/AlphaFilter/AlphaFilter.hpp @@ -45,9 +45,12 @@ #include template -class AlphaFilter { +class AlphaFilter +{ public: AlphaFilter() = default; + explicit AlphaFilter(float alpha) : _alpha(alpha) {} + ~AlphaFilter() = default; /** @@ -58,7 +61,8 @@ class AlphaFilter { * @param sample_interval interval between two samples * @param time_constant filter time constant determining convergence */ - void setParameters(float sample_interval, float time_constant) { + void setParameters(float sample_interval, float time_constant) + { const float denominator = time_constant + sample_interval; if (denominator > FLT_EPSILON) { @@ -85,7 +89,11 @@ class AlphaFilter { * * @return retrieve the filtered result */ - void update(const T &sample) { _filter_state = updateCalculation(sample); } + const T &update(const T &sample) + { + _filter_state = updateCalculation(sample); + return _filter_state; + } const T &getState() const { return _filter_state; } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 50444868dc..059042437c 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -48,9 +48,6 @@ bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); reset(); - _accel_lpf.setAlpha(.1f); - _gyro_lpf.setAlpha(.1f); - _mag_lpf.setAlpha(.1f); return ret; } diff --git a/EKF/ekf.h b/EKF/ekf.h index eecf719090..19f3522258 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -468,11 +468,11 @@ class Ekf final : public EstimatorInterface bool _is_first_imu_sample{true}; uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation - AlphaFilter _accel_lpf; ///< filtered accelerometer measurement used to align tilt (m/s/s) - AlphaFilter _gyro_lpf; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) + AlphaFilter _accel_lpf{0.1f}; ///< filtered accelerometer measurement used to align tilt (m/s/s) + AlphaFilter _gyro_lpf{0.1f}; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) // Variables used to perform in flight resets and switch between height sources - AlphaFilter _mag_lpf; ///< filtered magnetometer measurement for instant reset (Gauss) + AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m) float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)