mathlib: AlphaFilter add additional checks and updates methods

- setParameters() and setCutoffFreq() sanity check all input
 - on invalid configuration disable filter and leave in safe state
 - constrain cutoff frequency to Nyquist rather than disabling the filter
 - allow configuring only time constant directly
 - add new update() method with dt that also recomputes parameters if necessary
 - filter update don't allow invalid data (NAN, etc) to propagate

boards/px4/fmu-v5/src/toc.c

@@ -60,7 +60,7 @@ extern const int *_boot_signature;
 /* RD certificate signature follows the certificate */
 
 #define RDCTSIG_ADDR RDCT_END
-#define RDCTSIG_END ((const void *)((const uint8_t*)RDCT_ADDR+SIGNATURE_SIZE))
+#define RDCTSIG_END ((const void *)((const uint8_t*)RDCTSIG_ADDR+SIGNATURE_SIZE))
 
 /* The table of contents */
 

src/lib/mathlib/math/Functions.hpp

@@ -271,6 +271,11 @@ inline bool isFinite(const float &value)
 	return PX4_ISFINITE(value);
 }
 
+inline bool isFinite(const matrix::Vector2f &value)
+{
+	return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1));
+}
+
 inline bool isFinite(const matrix::Vector3f &value)
 {
 	return PX4_ISFINITE(value(0)) && PX4_ISFINITE(value(1)) && PX4_ISFINITE(value(2));

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

@@ -56,6 +56,30 @@ public:
 
 	~AlphaFilter() = default;
 
+	// 0.01 to 10,000 Hz
+	static constexpr float MIN_FREQ_HZ = 0.01f;    // 0.01 Hz or 100s interval
+	static constexpr float MAX_FREQ_HZ = 10'000.f; // 10,000 Hz or 0.0001s interval
+
+	static constexpr float min_interval_s() { return 1.f / MAX_FREQ_HZ; }
+	static constexpr float max_interval_s() { return 1.f / MIN_FREQ_HZ; }
+
+	/**
+	 * Set filter parameter alpha directly without time abstraction
+	 *
+	 * @param alpha [0,1] filter weight for the previous state. High value - long time constant.
+	 */
+	bool setAlpha(const float alpha)
+	{
+		if (alpha < 0.f || alpha > 1.f || !PX4_ISFINITE(alpha)) {
+			// Invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		_alpha = alpha;
+		return true;
+	}
+
 	/**
 	 * Set filter parameters for time abstraction
 	 *
@@ -64,42 +88,86 @@ public:
 	 * @param sample_interval interval between two samples
 	 * @param time_constant filter time constant determining convergence
 	 */
-	void setParameters(float sample_interval, float time_constant)
+	bool setParameters(const float sample_interval, const float time_constant)
 	{
-		const float denominator = time_constant + sample_interval;
-
-		if (denominator > FLT_EPSILON) {
-			setAlpha(sample_interval / denominator);
-		}
-	}
-
-	bool setCutoffFreq(float sample_freq, float cutoff_freq)
-	{
-		if ((sample_freq <= 0.f) || (cutoff_freq <= 0.f) || (cutoff_freq >= sample_freq / 2.f)
-		    || !isFinite(sample_freq) || !isFinite(cutoff_freq)) {
-
-			// Invalid parameters
+		if ((sample_interval <= 0.f) || !PX4_ISFINITE(sample_interval)
+		    || (time_constant <= 0.f) || !PX4_ISFINITE(time_constant)) {
+			// invalid parameters, disable filter
+			disable();
 			return false;
 		}
 
-		setParameters(1.f / sample_freq, 1.f / (2.f * M_PI_F * cutoff_freq));
-		_cutoff_freq = cutoff_freq;
-		return true;
+		_sample_interval = math::constrain(sample_interval, min_interval_s(), max_interval_s());
+		_time_constant = time_constant;
+
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
 	}
 
-	/**
-	 * 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; }
+	bool setSampleAndCutoffFrequency(const float sample_freq, const float cutoff_freq)
+	{
+		if ((sample_freq <= 0.f) || !PX4_ISFINITE(sample_freq)
+		    || (cutoff_freq <= 0.f) || !PX4_ISFINITE(cutoff_freq)) {
+			// invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		const float sample_freq_constrained = math::constrain(sample_freq, MIN_FREQ_HZ, MAX_FREQ_HZ);
+		const float cutoff_freq_constrained = math::constrain(cutoff_freq, MIN_FREQ_HZ, sample_freq_constrained / 2.f);
+
+		_sample_interval = 1.f / sample_freq_constrained;
+		_time_constant = 1.f / (2.f * M_PI_F * cutoff_freq_constrained);
+
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
+	}
+
+	bool setTimeConstant(const float time_constant)
+	{
+		if ((time_constant <= 0.f) || !PX4_ISFINITE(time_constant)) {
+			// Invalid parameters, disable filter
+			disable();
+			return false;
+		}
+
+		if ((fabsf(time_constant - _time_constant) / _time_constant) < 0.01f) {
+			// no change, do nothing
+			return true;
+		}
+
+		// time constant changed, update alpha
+		_time_constant = time_constant;
+		const float alpha = _sample_interval / (_sample_interval + _time_constant);
+
+		return setAlpha(alpha);
+	}
+
+	bool setCutoffFrequency(const float cutoff_freq)
+	{
+		const float time_constant = 1.f / (2.f * M_PI_F * cutoff_freq);
+		return setTimeConstant(time_constant);
+	}
 
 	/**
 	 * Set filter state to an initial value
 	 *
 	 * @param sample new initial value
 	 */
-	void reset(const T &sample) { _filter_state = sample; }
+	const T &reset(const T &sample = {})
+	{
+		_filter_state = sample;
+		return _filter_state;
+	}
+
+	void disable()
+	{
+		_alpha = 1.f;
+		_sample_interval = 1.f;
+		_time_constant = 0.f;
+	}
 
 	/**
 	 * Add a new raw value to the filter
@@ -112,13 +180,43 @@ public:
 		return _filter_state;
 	}
 
+	const T &update(const T &sample, const float dt)
+	{
+		if ((fabsf(dt - _sample_interval) / _sample_interval) > 0.01f) {
+			// update parameters if changed by more than 1%
+			if (!setParameters(dt, _time_constant)) {
+				// invalid new parameters, reset filter
+				return reset(sample);
+			}
+		}
+
+		_filter_state = updateCalculation(sample);
+		return _filter_state;
+	}
+
 	const T &getState() const { return _filter_state; }
-	float getCutoffFreq() const { return _cutoff_freq; }
 
-protected:
-	T updateCalculation(const T &sample) { return (1.f - _alpha) * _filter_state + _alpha * sample; }
+	float getCutoffFreq() const { return 1.f / (2.f * M_PI_F * _time_constant); }
+
+	const float &getSampleInterval() const { return _sample_interval; }
+	const float &getTimeConstant() const { return _time_constant; }
+
+private:
+	T updateCalculation(const T &sample)
+	{
+		// don't allow bad updates to propagate
+		const T ret = (1.f - _alpha) * _filter_state + _alpha * sample;
+
+		if (isFinite(ret)) {
+			return ret;
+		}
+
+		return {};
+	}
 
-	float _cutoff_freq{0.f};
-	float _alpha{0.f};
 	T _filter_state{};
+	float _alpha{1.f};
+
+	float _sample_interval{1.f};
+	float _time_constant{0.f};
 };

src/lib/mathlib/math/test/AlphaFilterTest.cpp

@@ -246,15 +246,19 @@ TEST(AlphaFilterTest, SetFrequencyTest)
 	AlphaFilter<float> _alpha_filter;
 	const float fs = .1f;
 
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, 0.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, fs)); // Cutoff above Nyquist freq
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(0.f, fs / 4.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(0.f, 0.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(-fs, fs / 4.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(-fs, -fs / 4.f));
-	EXPECT_FALSE(_alpha_filter.setCutoffFreq(fs, -fs / 4.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(fs, 0.f));
 
-	EXPECT_TRUE(_alpha_filter.setCutoffFreq(fs, fs / 4.f));
+	// Cutoff above Nyquist freq
+	EXPECT_TRUE(_alpha_filter.setSampleAndCutoffFrequency(fs, fs));
+	EXPECT_FLOAT_EQ(_alpha_filter.getCutoffFreq(), fs / 2.f);
+
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(0.f, fs / 4.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(0.f, 0.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(-fs, fs / 4.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(-fs, -fs / 4.f));
+	EXPECT_FALSE(_alpha_filter.setSampleAndCutoffFrequency(fs, -fs / 4.f));
+
+	EXPECT_TRUE(_alpha_filter.setSampleAndCutoffFrequency(fs, fs / 4.f));
 }
 
 TEST(AlphaFilterTest, ConvergenceTest)

src/modules/ekf2/EKF/ekf.cpp

@@ -176,13 +176,6 @@ bool Ekf::initialiseFilter()
 		}
 	}
 
-	if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
-		if (_mag_counter < _obs_buffer_length) {
-			// not enough mag samples accumulated
-			return false;
-		}
-	}
-
 	if (_baro_counter < _obs_buffer_length) {
 		// not enough baro samples accumulated
 		return false;
@@ -198,18 +191,25 @@ bool Ekf::initialiseFilter()
 	// calculate the initial magnetic field and yaw alignment
 	// but do not mark the yaw alignement complete as it needs to be
 	// reset once the leveling phase is done
-	if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) && (_mag_counter != 0)) {
-		// rotate the magnetometer measurements into earth frame using a zero yaw angle
-		// the angle of the projection onto the horizontal gives the yaw angle
-		const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
-		float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+	if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
+		if (_mag_counter > 1) {
+			// rotate the magnetometer measurements into earth frame using a zero yaw angle
+			// the angle of the projection onto the horizontal gives the yaw angle
+			const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
+			float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 
-		// update quaternion states and corresponding covarainces
-		resetQuatStateYaw(yaw_new, 0.f, false);
+			// update the rotation matrix using the new yaw value
+			_R_to_earth = updateYawInRotMat(yaw_new, Dcmf(_state.quat_nominal));
+			_state.quat_nominal = _R_to_earth;
 
-		// set the earth magnetic field states using the updated rotation
-		_state.mag_I = _R_to_earth * _mag_lpf.getState();
-		_state.mag_B.zero();
+			// set the earth magnetic field states using the updated rotation
+			_state.mag_I = _R_to_earth * _mag_lpf.getState();
+			_state.mag_B.zero();
+
+		} else {
+			// not enough mag samples accumulated
+			return false;
+		}
 	}
 
 	// initialise the state covariance matrix now we have starting values for all the states

src/modules/ekf2/EKF/ekf.h

@@ -1071,8 +1071,7 @@ private:
 	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
 	// yaw : Euler yaw angle (rad)
 	// yaw_variance : yaw error variance (rad^2)
-	// update_buffer : true if the state change should be also applied to the output observer buffer
-	void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer = true);
+	void resetQuatStateYaw(float yaw, float yaw_variance);
 
 	// Declarations used to control use of the EKF-GSF yaw estimator
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -370,7 +370,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 		const float yaw_variance_new = sq(asinf(sineYawError));
 
 		// Apply updated yaw and yaw variance to states and covariances
-		resetQuatStateYaw(yaw_new, yaw_variance_new, true);
+		resetQuatStateYaw(yaw_new, yaw_variance_new);
 
 		// Use the last magnetometer measurements to reset the field states
 		_state.mag_B.zero();
@@ -458,7 +458,7 @@ bool Ekf::resetYawToEv()
 	const float yaw_new = getEulerYaw(_ev_sample_delayed.quat);
 	const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
 
-	resetQuatStateYaw(yaw_new, yaw_new_variance, true);
+	resetQuatStateYaw(yaw_new, yaw_new_variance);
 	_R_ev_to_ekf.setIdentity();
 
 	return true;
@@ -1567,7 +1567,7 @@ void Ekf::stopFlowFusion()
 	}
 }
 
-void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
+void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 {
 	// save a copy of the quaternion state for later use in calculating the amount of reset change
 	const Quatf quat_before_reset = _state.quat_nominal;
@@ -1593,16 +1593,14 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 	}
 
 	// add the reset amount to the output observer buffered data
-	if (update_buffer) {
-		for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-			_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
-		}
-
-		// apply the change in attitude quaternion to our newest quaternion estimate
-		// which was already taken out from the output buffer
-		_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
+	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
+		_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
 	}
 
+	// apply the change in attitude quaternion to our newest quaternion estimate
+	// which was already taken out from the output buffer
+	_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
+
 	_last_static_yaw = NAN;
 
 	// capture the reset event
@@ -1618,7 +1616,7 @@ bool Ekf::resetYawToEKFGSF()
 		return false;
 	}
 
-	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar(), true);
+	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
 
 	// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -216,7 +216,7 @@ bool Ekf::resetYawToGps()
 	const float measured_yaw = _gps_sample_delayed.yaw;
 
 	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
-	resetQuatStateYaw(measured_yaw, yaw_variance, true);
+	resetQuatStateYaw(measured_yaw, yaw_variance);
 
 	_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
 	_gnss_yaw_signed_test_ratio_lpf.reset(0.f);

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -183,7 +183,7 @@ void VehicleAngularVelocity::ResetFilters(const hrt_abstime &time_now_us)
 
 			// angular acceleration low pass
 			if ((_param_imu_dgyro_cutoff.get() > 0.f)
-			    && (_lp_filter_acceleration[axis].setCutoffFreq(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()))) {
+			    && (_lp_filter_acceleration[axis].setSampleAndCutoffFrequency(_filter_sample_rate_hz, _param_imu_dgyro_cutoff.get()))) {
 				_lp_filter_acceleration[axis].reset(angular_acceleration_uncalibrated(axis));
 
 			} else {