AlphaFilter: set dt during update

src/lib/mathlib/math/filter/AlphaFilter.hpp

@@ -52,7 +52,8 @@ class AlphaFilter
 {
 public:
 	AlphaFilter() = default;
-	explicit AlphaFilter(float alpha) : _alpha(alpha) {}
+	explicit AlphaFilter(float sample_interval, float time_constant) { setParameters(sample_interval, time_constant); }
+	explicit AlphaFilter(float time_constant) : _time_constant(time_constant) {};
 
 	~AlphaFilter() = default;
 
@@ -71,6 +72,8 @@ class AlphaFilter
 		if (denominator > FLT_EPSILON) {
 			setAlpha(sample_interval / denominator);
 		}
+
+		_time_constant = time_constant;
 	}
 
 	bool setCutoffFreq(float sample_freq, float cutoff_freq)
@@ -82,8 +85,7 @@ class AlphaFilter
 			return false;
 		}
 
-		setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq));
-		_cutoff_freq = cutoff_freq;
+		setParameters(1.f / sample_freq, 1.f / (M_TWOPI_F * cutoff_freq));
 		return true;
 	}
 
@@ -112,14 +114,19 @@ class AlphaFilter
 		return _filter_state;
 	}
 
+	const T update(const T &sample, float dt)
+	{
+		setParameters(dt, _time_constant);
+		return update(sample);
+	}
+
 	const T &getState() const { return _filter_state; }
-	float getCutoffFreq() const { return _cutoff_freq; }
+	float getCutoffFreq() const { return 1.f / (M_TWOPI_F * _time_constant); }
 
 protected:
 	T updateCalculation(const T &sample);
 
-
-	float _cutoff_freq{0.f};
+	float _time_constant{0.f};
 	float _alpha{0.f};
 	T _filter_state{};
 };

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;
 		}

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;

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,

src/modules/ekf2/EKF/ekf.h

@@ -490,8 +490,13 @@ class Ekf final : public EstimatorInterface
 
 	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<Vector2f> _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<float> _height_rate_lpf{_kHeightRateLpfTimeConstant};
+#endif // CONFIG_EKF2_WIND
 
 	SquareMatrixState P{};	///< state covariance matrix
 
@@ -518,7 +523,7 @@ class Ekf final : public EstimatorInterface
 	Vector3f _ref_body_rate{};
 
 	Vector2f _flow_vel_body{};                      ///< velocity from corrected flow measurement (body frame)(m/s)
-	AlphaFilter<Vector2f> _flow_vel_body_lpf{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s)
+	AlphaFilter<Vector2f> _flow_vel_body_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant}; ///< filtered velocity from corrected flow measurement (body frame)(m/s)
 	uint32_t _flow_counter{0};                      ///< number of flow samples read for initialization
 
 	Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
@@ -583,14 +588,15 @@ class Ekf final : public EstimatorInterface
 
 	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
-	AlphaFilter<Vector3f> _accel_lpf{0.1f};	///< filtered accelerometer measurement used to align tilt (m/s/s)
-	AlphaFilter<Vector3f> _gyro_lpf{0.1f};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+	static constexpr float _kSensorLpfTimeConstant = 0.09f;
+	AlphaFilter<Vector3f> _accel_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant};	///< filtered accelerometer measurement used to align tilt (m/s/s)
+	AlphaFilter<Vector3f> _gyro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant};	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
 
 #if defined(CONFIG_EKF2_BAROMETER)
 	estimator_aid_source1d_s _aid_src_baro_hgt {};
 
 	// Variables used to perform in flight resets and switch between height sources
-	AlphaFilter<float> _baro_lpf{0.1f};	///< filtered barometric height measurement (m)
+	AlphaFilter<float> _baro_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant};	///< filtered barometric height measurement (m)
 	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
 
 	HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref};
@@ -600,12 +606,12 @@ class Ekf final : public EstimatorInterface
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 	// used by magnetometer fusion mode selection
-	AlphaFilter<float> _mag_heading_innov_lpf{0.1f};
+	AlphaFilter<float> _mag_heading_innov_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant};
 	uint32_t _min_mag_health_time_us{1'000'000}; ///< magnetometer is marked as healthy only after this amount of time
 
 	estimator_aid_source3d_s _aid_src_mag{};
 
-	AlphaFilter<Vector3f> _mag_lpf{0.1f};	///< filtered magnetometer measurement for instant reset (Gauss)
+	AlphaFilter<Vector3f> _mag_lpf{_dt_ekf_avg, _kSensorLpfTimeConstant};	///< filtered magnetometer measurement for instant reset (Gauss)
 	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
 
 	// Variables used to control activation of post takeoff functionality
@@ -1011,7 +1017,8 @@ class Ekf final : public EstimatorInterface
 #if defined(CONFIG_EKF2_EXTERNAL_VISION)
 	HeightBiasEstimator _ev_hgt_b_est {HeightSensor::EV, _height_sensor_ref};
 	PositionBiasEstimator _ev_pos_b_est{PositionSensor::EV, _position_sensor_ref};
-	AlphaFilter<Quatf> _ev_q_error_filt{0.001f};
+	static constexpr float _kQuatErrorLpfTimeConstant = 10.f;
+	AlphaFilter<Quatf> _ev_q_error_filt{_dt_ekf_avg, _kQuatErrorLpfTimeConstant};
 	bool _ev_q_error_initialized{false};
 #endif // CONFIG_EKF2_EXTERNAL_VISION