AlphaFilter: merge with PX4 implementation

I made a separate implementation of the same filter for PX4. Now
that I know it's duplicate I merge the two into one and reuse it.

EKF/AlphaFilter.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,32 +32,60 @@
  ****************************************************************************/
 
 /**
- * First order "alpha" IIR digital filter
+ * @file AlphaFilter.hpp
+ *
+ * @brief First order "alpha" IIR digital filter also known as leaky integrator or forgetting average.
+ *
  * @author Mathieu Bresciani <brescianimathieu@gmail.com>
+ * @author Matthias Grob <maetugr@gmail.com>
  */
 
 #pragma once
 
 template <typename T>
-class AlphaFilter final {
+class AlphaFilter {
 public:
 	AlphaFilter() = default;
 	~AlphaFilter() = default;
 
-	void reset(const T &val) { _x = val; }
-
-	void update(const T &input, float tau, float dt) {
-		const float alpha = dt / tau;
-		update(input, alpha);
+	/**
+	 * Set filter parameters for time abstraction
+	 *
+	 * Both parameters have to be provided in the same units.
+	 *
+	 * @param sample_interval interval between two samples
+	 * @param time_constant filter time constant determining convergence
+	 */
+	void setParameters(float sample_interval, float time_constant) {
+		setAlpha(sample_interval / (time_constant + sample_interval));
 	}
 
-	void update(const T &input, float alpha) { _x = (1.f - alpha) * _x + alpha * input; }
+	/**
+	 * Set filter parameter alpha directly without time abstraction
+	 *
+	 * @param alpha [0,1] filter weight for the previous state. High value - long time constant.
+	 */
+	void setAlpha(float alpha) { _alpha = alpha; }
 
-	// Typical 0.9/0.1 lowpass filter
-	void update(const T &input) { update(input, 0.1f); }
+	/**
+	 * Set filter state to an initial value
+	 *
+	 * @param sample new initial value
+	 */
+	void reset(const T &sample) { _filter_state = sample; }
 
-	const T &getState() const { return _x; }
+	/**
+	 * Add a new raw value to the filter
+	 *
+	 * @return retrieve the filtered result
+	 */
+	void update(const T &sample) { _filter_state = updateCalculation(sample); }
 
-private:
-	T _x{};  ///< current state of the filter
+	const T &getState() const { return _filter_state; }
+
+protected:
+	T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; }
+
+	float _alpha{0.f};
+	T _filter_state{};
 };

EKF/ekf.cpp

@@ -48,7 +48,9 @@ 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;
 }
 

EKF/ekf.h

@@ -458,11 +458,11 @@ private:
 	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
-	AlphaFilterVector3f _accel_lpf;		///< filtered accelerometer measurement used to align tilt (m/s/s)
-	AlphaFilterVector3f _gyro_lpf;		///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+	AlphaFilter<Vector3f> _accel_lpf;	///< filtered accelerometer measurement used to align tilt (m/s/s)
+	AlphaFilter<Vector3f> _gyro_lpf;	///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
 
 	// Variables used to perform in flight resets and switch between height sources
-	AlphaFilterVector3f _mag_lpf;		///< filtered magnetometer measurement for instant reset (Gauss)
+	AlphaFilter<Vector3f> _mag_lpf;		///< 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)
 

EKF/estimator_interface.h

@@ -59,8 +59,6 @@ class EstimatorInterface
 {
 
 public:
-	typedef AlphaFilter<Vector3f> AlphaFilterVector3f;
-
 	EstimatorInterface():_imu_down_sampler(FILTER_UPDATE_PERIOD_S){};
 	virtual ~EstimatorInterface() = default;