atune: paremeterize intial RLS variance

Rearrange to look like a KF

src/lib/system_identification/arx_rls.hpp

@@ -84,13 +84,13 @@ public:
 	float getInnovation() const { return _innovation; }
 	const matrix::Vector < float, N + M + 1 > &getDiffEstimate() const { return _diff_theta_hat; }
 
-	void reset(const matrix::Vector < float, N + M + 1 > &theta_init = {})
+	void reset(const matrix::Vector < float, N + M + 1 > &theta_init = {}, const float var_init = 100.f)
 	{
 		/* _P.uncorrelateCovarianceSetVariance<N + M + 1>(0, 10e3f); // does not work */
 		_P.setZero();
 
 		for (size_t i = 0; i < (N + M + 1); i++) {
-			_P(i, i) = 10e3f;
+			_P(i, i) = var_init;
 		}
 
 		_diff_theta_hat.setZero();
@@ -115,18 +115,14 @@ public:
 
 		addInputOutput(u, y);
 
-		if (!isBufferFull()) {
-			// Do not start to update the RLS algorithm when the
-			// buffer still contains zeros
-			return;
-		}
-
 		const matrix::Vector < float, N + M + 1 > phi = constructDesignVector();
 		const matrix::Matrix < float, 1, N + M + 1 > phi_t = phi.transpose();
 
-		_P = (_P - _P * phi * phi_t * _P / (_lambda + (phi_t * _P * phi)(0, 0))) / _lambda;
+		auto K = _P * phi / ((phi_t * _P * phi)(0, 0) + _lambda);
 		_innovation = _y[N] - (phi_t * _theta_hat)(0, 0);
-		_theta_hat = _theta_hat + _P * phi * _innovation;
+
+		_theta_hat = _theta_hat + K * _innovation;
+		_P = (_P - K * (phi_t * _P)) / _lambda;
 
 		for (size_t i = 0; i < N + M + 1; i++) {
 			_diff_theta_hat(i) = fabsf(_theta_hat(i) - theta_prev(i));
@@ -135,16 +131,11 @@ public:
 		/* fixCovarianceErrors(); // TODO: this could help against ill-conditioned matrix but needs more testing*/
 	}
 
-private:
 	void addInputOutput(float u, float y)
 	{
 		shiftRegisters();
 		_u[M + D] = u;
 		_y[N] = y;
-
-		if (!isBufferFull()) {
-			_nb_samples++;
-		}
 	}
 
 	void shiftRegisters()
@@ -158,8 +149,6 @@ private:
 		}
 	}
 
-	bool isBufferFull() const { return _nb_samples > (M + N + D); }
-
 	matrix::Vector < float, N + M + 1 > constructDesignVector() const
 	{
 		matrix::Vector < float, N + M + 1 > phi;

src/lib/system_identification/signal_generator.hpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file signal_generator.hpp
+ */
+
+#pragma once
+
+namespace signal_generator
+{
+
+float getLinearSineSweep(float f_start, float f_end, float duration, float t)
+{
+	if (t > duration) {
+		return 0.f;
+	}
+
+	const float w_start = f_start * M_TWOPI_F;
+	const float w_end = f_end * M_TWOPI_F;
+
+	return sinf(w_start * t + 0.5f * (w_end - w_start) * t * t / duration);
+}
+
+float getLogSineSweep(float f_start, float f_end, float duration, float t)
+{
+	if (t > duration) {
+		return 0.f;
+
+	}
+
+	float w_start = f_start * M_TWOPI_F;
+	float w_end = f_end * M_TWOPI_F;
+
+	if (f_start > f_end) {
+		// Handle high-to-low sweep correctly
+		w_start = f_end * M_TWOPI_F;
+		w_end = f_start * M_TWOPI_F;
+		t = duration - t;
+	}
+
+	return sinf(t * powf(10.f, log10f(w_start) + (log10f(w_end) - log10f(w_start)) * t / duration));
+}
+
+} /* namespace signal_generator */

src/lib/system_identification/system_identification.cpp

@@ -39,9 +39,9 @@
 
 #include "system_identification.hpp"
 
-void SystemIdentification::reset(const matrix::Vector<float, 5> &id_state_init)
+void SystemIdentification::reset(const matrix::Vector<float, _kParameters> &id_state_init, const float var_init)
 {
-	_rls.reset(id_state_init);
+	_rls.reset(id_state_init, var_init);
 	_u_lpf.reset(0.f);
 	_u_lpf.reset(0.f);
 	_u_hpf = 0.f;
@@ -79,8 +79,8 @@ void SystemIdentification::updateFilters(float u, float y)
 
 	const float u_lpf = _u_lpf.apply(u);
 	const float y_lpf = _y_lpf.apply(y);
-	_u_hpf = _alpha_hpf * _u_hpf + _alpha_hpf * (u_lpf - _u_prev);
-	_y_hpf = _alpha_hpf * _y_hpf + _alpha_hpf * (y_lpf - _y_prev);
+	_u_hpf = u_lpf - _u_prev - (_gamma_hpf - 1.f) * _u_hpf;
+	_y_hpf = y_lpf - _y_prev - (_gamma_hpf - 1.f) * _y_hpf;
 
 	_u_prev = u_lpf;
 	_y_prev = y_lpf;
@@ -88,10 +88,10 @@ void SystemIdentification::updateFilters(float u, float y)
 
 void SystemIdentification::updateFitness()
 {
-	const matrix::Vector<float, 5> &diff = _rls.getDiffEstimate();
+	const matrix::Vector<float, _kParameters> &diff = _rls.getDiffEstimate();
 	float sum = 0.f;
 
-	for (size_t i = 0; i < 5; i++) {
+	for (size_t i = 0; i < _kParameters; i++) {
 		sum += diff(i);
 	}
 

src/lib/system_identification/system_identification.hpp

@@ -50,18 +50,23 @@
 class SystemIdentification final
 {
 public:
+	static constexpr int _kPoles = 2;
+	static constexpr int _kZeros = 2;
+	static constexpr int _kDelays = 1;
+	static constexpr int _kParameters = _kPoles + _kZeros + 1;
+
 	SystemIdentification() = default;
 	~SystemIdentification() = default;
 
-	void reset(const matrix::Vector<float, 5> &id_state_init = {});
+	void reset(const matrix::Vector<float, _kParameters> &id_state_init = {}, float var_init = 100.f);
 	void update(float u, float y); // update filters and model
 	void update(); // update model only (to be called after updateFilters)
 	void updateFilters(float u, float y);
 	bool areFiltersInitialized() const { return _are_filters_initialized; }
 	void updateFitness();
-	const matrix::Vector<float, 5> &getCoefficients() const { return _rls.getCoefficients(); }
-	const matrix::Vector<float, 5> getVariances() const { return _rls.getVariances(); }
-	const matrix::Vector<float, 5> &getDiffEstimate() const { return _rls.getDiffEstimate(); }
+	const matrix::Vector<float, _kParameters> getCoefficients() const { return  _rls.getCoefficients(); }
+	const matrix::Vector<float, _kParameters> getVariances() const { return _rls.getVariances(); }
+	const matrix::Vector<float, _kParameters> getDiffEstimate() const { return _rls.getDiffEstimate(); }
 	float getFitness() const { return _fitness_lpf.getState(); }
 	float getInnovation() const { return _rls.getInnovation(); }
 
@@ -70,7 +75,7 @@ public:
 		_u_lpf.set_cutoff_frequency(sample_freq, cutoff);
 		_y_lpf.set_cutoff_frequency(sample_freq, cutoff);
 	}
-	void setHpfCutoffFrequency(float sample_freq, float cutoff) { _alpha_hpf = sample_freq / (sample_freq + 2.f * M_PI_F * cutoff); }
+	void setHpfCutoffFrequency(float sample_freq, float cutoff) { _gamma_hpf = tanf(M_PI_F * cutoff / sample_freq); }
 
 	void setForgettingFactor(float time_constant, float dt) { _rls.setForgettingFactor(time_constant, dt); }
 	void setFitnessLpfTimeConstant(float time_constant, float dt)
@@ -83,12 +88,12 @@ public:
 	float getFilteredOutputData() const { return _y_hpf; }
 
 private:
-	ArxRls<2, 2, 1> _rls;
+	ArxRls<_kPoles, _kZeros, _kDelays> _rls;
 	math::LowPassFilter2p<float> _u_lpf{400.f, 30.f};
 	math::LowPassFilter2p<float> _y_lpf{400.f, 30.f};
 
 	//TODO: replace by HighPassFilter class
-	float _alpha_hpf{0.f};
+	float _gamma_hpf{0.f};
 	float _u_hpf{0.f};
 	float _y_hpf{0.f};
 

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -169,14 +169,22 @@ void FwAutotuneAttitudeControl::Run()
 		const hrt_abstime now = hrt_absolute_time();
 		updateStateMachine(now);
 
-		Vector<float, 5> coeff = _sys_id.getCoefficients();
-		coeff(2) *= _input_scale;
-		coeff(3) *= _input_scale;
-		coeff(4) *= _input_scale;
+		Vector<float, SystemIdentification::_kParameters> coeff = _sys_id.getCoefficients();
 
-		const Vector3f num(coeff(2), coeff(3), coeff(4));
-		const Vector3f den(1.f, coeff(0), coeff(1));
-		_kiff(2) = (1.f + coeff(0) + coeff(1)) / (coeff(2) + coeff(3) + coeff(4)); // inverse of the static gain
+		Vector3f den(1.f, 0.f, 0.f);
+
+		for (int i = 0; i < SystemIdentification::_kPoles; i++) {
+			den(i + 1) = coeff(i);
+		}
+
+		Vector3f num;
+
+		for (int i = 0; i < SystemIdentification::_kZeros + 1; i++) {
+			coeff(SystemIdentification::_kPoles + i) *= _input_scale;
+			num(i) = coeff(SystemIdentification::_kPoles + i);
+		}
+
+		_kiff(2) = (den(0) + den(1) + den(2)) / (num(0) + num(1) + num(2)); // inverse of the static gain
 		const Vector3f num_design = num * _kiff(2); // PID algorithm design works better with systems having unit static gain
 		Vector3f kid = pid_design::computePidGmvc(num_design, den, _sample_interval_avg, 0.2f, 0.f, 0.4f);
 		_kiff(0) = kid(0);
@@ -187,7 +195,7 @@ void FwAutotuneAttitudeControl::Run()
 		// or K_att * (K_rate + K_ff) * rad(60) = 1
 		_attitude_p = math::constrain(1.f / (math::radians(60.f) * (_kiff(0) + _kiff(2))), 1.f, 5.f);
 
-		const Vector<float, 5> &coeff_var = _sys_id.getVariances();
+		const Vector<float, SystemIdentification::_kParameters> &coeff_var = _sys_id.getVariances();
 		const Vector3f rate_sp = _sys_id.areFiltersInitialized()
 					 ? getIdentificationSignal()
 					 : Vector3f();
@@ -291,8 +299,9 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 	// when identifying an axis, check if the estimate has converged
 	const float converged_thr = 1.f;
 
-	const float temp[5] = {0.f, 0.f, 0.f, 0.f, 0.f};
-	const Vector<float, 5> sys_id_init(temp);
+	Vector<float, 5> sys_id_init;
+	sys_id_init(0) = -1.5f;
+	sys_id_init(1) = 0.5f;
 
 	switch (_state) {
 	case state::idle:

src/modules/logger/logged_topics.cpp

@@ -50,7 +50,7 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic("actuator_controls_status_0", 300);
 	add_topic("airspeed", 1000);
 	add_optional_topic("airspeed_validated", 200);
-	add_optional_topic("autotune_attitude_control_status", 100);
+	add_optional_topic("autotune_attitude_control_status");
 	add_optional_topic("camera_capture");
 	add_optional_topic("camera_trigger");
 	add_topic("cellular_status", 200);
@@ -391,6 +391,7 @@ void LoggedTopics::add_system_identification_topics()
 	add_topic("sensor_combined");
 	add_topic("vehicle_angular_velocity");
 	add_topic("vehicle_torque_setpoint");
+	add_optional_topic("autotune_attitude_control_status");
 }
 
 void LoggedTopics::add_mavlink_tunnel()

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp

@@ -61,8 +61,6 @@ bool McAutotuneAttitudeControl::init()
 		return false;
 	}
 
-	_signal_filter.setParameters(_publishing_dt_s, .2f); // runs in the slow publishing loop
-
 	return true;
 }
 
@@ -91,9 +89,7 @@ void McAutotuneAttitudeControl::Run()
 		updateStateMachine(hrt_absolute_time());
 	}
 
-	// new control data needed every iteration
-	if (_state == state::idle
-	    || !_vehicle_torque_setpoint_sub.updated()) {
+	if (_state == state::idle) {
 		return;
 	}
 
@@ -113,17 +109,23 @@ void McAutotuneAttitudeControl::Run()
 		}
 	}
 
-	vehicle_torque_setpoint_s vehicle_torque_setpoint;
 	vehicle_angular_velocity_s angular_velocity;
 
-	if (!_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint)
-	    || !_vehicle_angular_velocity_sub.copy(&angular_velocity)) {
+	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
+		_angular_velocity(0) = angular_velocity.xyz[0];
+		_angular_velocity(1) = angular_velocity.xyz[1];
+		_angular_velocity(2) = angular_velocity.xyz[2];
+	}
+
+	vehicle_torque_setpoint_s torque_setpoint;
+
+	if (!_vehicle_torque_setpoint_sub.update(&torque_setpoint)) {
 		return;
 	}
 
 	perf_begin(_cycle_perf);
 
-	const hrt_abstime timestamp_sample = vehicle_torque_setpoint.timestamp;
+	const hrt_abstime timestamp_sample = torque_setpoint.timestamp;
 
 	// collect sample interval average for filters
 	if (_last_run > 0) {
@@ -143,16 +145,16 @@ void McAutotuneAttitudeControl::Run()
 
 	// Send data to the filters at maximum frequency
 	if (_state == state::roll) {
-		_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[0],
-				      angular_velocity.xyz[0]);
+		_sys_id.updateFilters(torque_setpoint.xyz[0],
+				      _angular_velocity(0));
 
 	} else if (_state == state::pitch) {
-		_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[1],
-				      angular_velocity.xyz[1]);
+		_sys_id.updateFilters(torque_setpoint.xyz[1],
+				      _angular_velocity(1));
 
 	} else if (_state == state::yaw) {
-		_sys_id.updateFilters(_input_scale * vehicle_torque_setpoint.xyz[2],
-				      angular_velocity.xyz[2]);
+		_sys_id.updateFilters(torque_setpoint.xyz[2],
+				      _angular_velocity(2));
 	}
 
 	// Update the model at a lower frequency
@@ -167,17 +169,23 @@ void McAutotuneAttitudeControl::Run()
 		_model_update_counter = 0;
 	}
 
-	if (hrt_elapsed_time(&_last_publish) > _publishing_dt_hrt || _last_publish == 0) {
+	if (hrt_elapsed_time(&_last_publish) > (_publishing_dt_s * 1_s) || _last_publish == 0) {
 		const hrt_abstime now = hrt_absolute_time();
 		updateStateMachine(now);
 
-		Vector<float, 5> coeff = _sys_id.getCoefficients();
-		coeff(2) *= _input_scale;
-		coeff(3) *= _input_scale;
-		coeff(4) *= _input_scale;
+		Vector<float, SystemIdentification::_kParameters> coeff = _sys_id.getCoefficients();
 
-		const Vector3f num(coeff(2), coeff(3), coeff(4));
-		const Vector3f den(1.f, coeff(0), coeff(1));
+		Vector3f den(1.f, 0.f, 0.f);
+
+		for (int i = 0; i < SystemIdentification::_kPoles; i++) {
+			den(i + 1) = coeff(i);
+		}
+
+		Vector3f num;
+
+		for (int i = 0; i < SystemIdentification::_kZeros + 1; i++) {
+			num(i) = coeff(SystemIdentification::_kPoles + i);
+		}
 
 		const float model_dt = static_cast<float>(_model_update_scaler) * _filter_dt;
 
@@ -190,15 +198,20 @@ void McAutotuneAttitudeControl::Run()
 			_kid(2) = 0.f;
 		}
 
+		// Do not use derivative on the yaw axis as it often only amplifies noise
+		if ((_state == state::yaw) || (_state == state::yaw_pause)) {
+			_kid(2) = 0.f;
+		}
+
 		// To compute the attitude gain, use the following empirical rule:
 		// "An error of 60 degrees should produce the maximum control output"
 		// or K_att * K_rate * rad(60) = 1
 		_attitude_p = math::constrain(1.f / (math::radians(60.f) * _kid(0)), 2.f, 6.5f);
 
-		const Vector<float, 5> &coeff_var = _sys_id.getVariances();
+		const Vector<float, SystemIdentification::_kParameters> &coeff_var = _sys_id.getVariances();
 
 		const Vector3f rate_sp = _sys_id.areFiltersInitialized()
-					 ? getIdentificationSignal()
+					 ? getIdentificationSignal(now)
 					 : Vector3f();
 
 		autotune_attitude_control_status_s status{};
@@ -226,38 +239,33 @@ void McAutotuneAttitudeControl::Run()
 
 void McAutotuneAttitudeControl::checkFilters()
 {
-	if (_interval_count > 1000) {
-		// calculate sensor update rate
-		_sample_interval_avg = _interval_sum / _interval_count;
+	if (_interval_count < 1000) {
+		return;
+	}
 
-		// check if sample rate error is greater than 1%
-		bool reset_filters = false;
+	// calculate sensor update rate
+	const float sample_interval = _interval_sum / static_cast<float>(_interval_count);
 
-		if ((fabsf(_filter_dt - _sample_interval_avg) / _filter_dt) > 0.01f) {
-			reset_filters = true;
-		}
+	// check if sample rate error is greater than 1%
+	const bool reset_filters = (fabsf(_filter_dt - sample_interval) / _filter_dt) > 0.01f;
 
-		if (reset_filters || !_are_filters_initialized) {
-			_filter_dt = _sample_interval_avg;
+	if (reset_filters || !_are_filters_initialized) {
+		_filter_dt = sample_interval;
 
-			const float filter_rate_hz = 1.f / _filter_dt;
+		const float filter_rate_hz = 1.f / _filter_dt;
 
-			_sys_id.setLpfCutoffFrequency(filter_rate_hz, _param_imu_gyro_cutoff.get());
-			_sys_id.setHpfCutoffFrequency(filter_rate_hz, .5f);
+		_sys_id.setLpfCutoffFrequency(filter_rate_hz, 30.f);
+		_sys_id.setHpfCutoffFrequency(filter_rate_hz, .1f);
 
-			// Set the model sampling time depending on the gyro cutoff frequency
-			// as this is a good indicator of the maximum control loop bandwidth
-			float model_dt = math::constrain(math::max(1.f / (2.f * _param_imu_gyro_cutoff.get()), _filter_dt), _model_dt_min,
-							 _model_dt_max);
+		// Make sure the model dt is a integer multiple of the data update rate
+		float model_dt = 0.01f;
+		_model_update_scaler = math::max(int(model_dt / _filter_dt), 1);
+		model_dt = _model_update_scaler * _filter_dt;
 
-			_model_update_scaler = math::max(int(model_dt / _filter_dt), 1);
-			model_dt = _model_update_scaler * _filter_dt;
+		_sys_id.setForgettingFactor(60.f, model_dt);
+		_sys_id.setFitnessLpfTimeConstant(1.f, model_dt);
 
-			_sys_id.setForgettingFactor(60.f, model_dt);
-			_sys_id.setFitnessLpfTimeConstant(1.f, model_dt);
-
-			_are_filters_initialized = true;
-		}
+		_are_filters_initialized = true;
 
 		// reset sample interval accumulator
 		_last_run = 0;
@@ -266,8 +274,9 @@ void McAutotuneAttitudeControl::checkFilters()
 
 void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 {
-	// when identifying an axis, check if the estimate has converged
-	const float converged_thr = 50.f;
+	Vector<float, 5> state_init;
+	state_init(0) = -1.5f;
+	state_init(1) = 0.5f;
 
 	switch (_state) {
 	case state::idle:
@@ -288,21 +297,15 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if (_are_filters_initialized) {
 			_state = state::roll;
 			_state_start_time = now;
-			_sys_id.reset();
-			// first step needs to be shorter to keep the drone centered
-			_steps_counter = 5;
-			_max_steps = 10;
-			_signal_sign = 1;
-			_input_scale = 1.f / (_param_mc_rollrate_p.get() * _param_mc_rollrate_k.get());
-			_signal_filter.reset(0.f);
+			_sys_id.reset(state_init, _kInitVar);
 			_gains_backup_available = false;
 		}
 
 		break;
 
 	case state::roll:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
-		    && ((now - _state_start_time) > 5_s)) {
+		if ((_sys_id.getVariances().max() < _kInitVar)
+		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(0);
 
 			// wait for the drone to stabilize
@@ -316,20 +319,14 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if ((now - _state_start_time) > 2_s) {
 			_state = state::pitch;
 			_state_start_time = now;
-			_sys_id.reset();
-			_input_scale = 1.f / (_param_mc_pitchrate_p.get() * _param_mc_pitchrate_k.get());
-			_signal_filter.reset(0.f);
-			_signal_sign = 1;
-			// first step needs to be shorter to keep the drone centered
-			_steps_counter = 5;
-			_max_steps = 10;
+			_sys_id.reset(state_init, _kInitVar);
 		}
 
 		break;
 
 	case state::pitch:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
-		    && ((now - _state_start_time) > 5_s)) {
+		if ((_sys_id.getVariances().max() < _kInitVar)
+		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(1);
 			_state = state::pitch_pause;
 			_state_start_time = now;
@@ -341,20 +338,14 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if ((now - _state_start_time) > 2_s) {
 			_state = state::yaw;
 			_state_start_time = now;
-			_sys_id.reset();
-			_input_scale = 1.f / (_param_mc_yawrate_p.get() * _param_mc_yawrate_k.get());
-			_signal_filter.reset(0.f);
-			_signal_sign = 1;
-			// first step needs to be shorter to keep the drone centered
-			_steps_counter = 5;
-			_max_steps = 10;
+			_sys_id.reset(state_init, _kInitVar);
 		}
 
 		break;
 
 	case state::yaw:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
-		    && ((now - _state_start_time) > 5_s)) {
+		if ((_sys_id.getVariances().max() < _kInitVar)
+		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(2);
 			_state = state::yaw_pause;
 			_state_start_time = now;
@@ -367,10 +358,6 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 			_state = state::verification;
 			_state_start_time = now;
 			_sys_id.reset();
-			_signal_filter.reset(0.f);
-			_signal_sign = 1;
-			_steps_counter = 5;
-			_max_steps = 10;
 		}
 
 		break;
@@ -445,7 +432,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 
 	if (_state != state::wait_for_disarm
 	    && _state != state::idle
-	    && (((now - _state_start_time) > 20_s)
+	    && (((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s + 2_s))
 		|| (fabsf(manual_control_setpoint.roll) > 0.05f)
 		|| (fabsf(manual_control_setpoint.pitch) > 0.05f))) {
 		_state = state::fail;
@@ -507,15 +494,6 @@ bool McAutotuneAttitudeControl::registerActuatorControlsCallback()
 	return true;
 }
 
-bool McAutotuneAttitudeControl::areAllSmallerThan(const Vector<float, 5> &vect, float threshold) const
-{
-	return (vect(0) < threshold)
-	       && (vect(1) < threshold)
-	       && (vect(2) < threshold)
-	       && (vect(3) < threshold)
-	       && (vect(4) < threshold);
-}
-
 void McAutotuneAttitudeControl::copyGains(int index)
 {
 	if (index <= 2) {
@@ -585,27 +563,27 @@ void McAutotuneAttitudeControl::stopAutotune()
 	_vehicle_torque_setpoint_sub.unregisterCallback();
 }
 
-const Vector3f McAutotuneAttitudeControl::getIdentificationSignal()
+const Vector3f McAutotuneAttitudeControl::getIdentificationSignal(hrt_abstime now)
 {
-	if (_steps_counter > _max_steps) {
-		_signal_sign = (_signal_sign == 1) ? 0 : 1;
-		_steps_counter = 0;
+	const float t = static_cast<float>(now - _state_start_time) * 1e-6f;
 
-		if (_max_steps > 1) {
-			_max_steps--;
+	float signal;
 
-		} else {
-			_max_steps = 5;
-		}
+	if (_param_mc_at_sysid_type.get() == static_cast<int32_t>(SignalType::kLinearSineSweep)) {
+		const float f_max = (_state == state::yaw) ? _param_mc_at_sysid_fyaw.get() : _param_mc_at_sysid_f1.get();
+		signal = signal_generator::getLinearSineSweep(_param_mc_at_sysid_f0.get(), f_max,
+				_param_mc_at_sysid_time.get(), t);
+
+	} else if (_param_mc_at_sysid_type.get() == static_cast<int32_t>(SignalType::kLogSineSweep)) {
+		const float f_max = (_state == state::yaw) ? _param_mc_at_sysid_fyaw.get() : _param_mc_at_sysid_f1.get();
+		signal = signal_generator::getLogSineSweep(_param_mc_at_sysid_f0.get(), f_max,
+				_param_mc_at_sysid_time.get(), t);
+
+	} else {
+		signal = 0.f;
 	}
 
-	_steps_counter++;
-
-	const float step = float(_signal_sign) * _param_mc_at_sysid_amp.get();
-
-	Vector3f rate_sp{};
-
-	const float signal = step - _signal_filter.getState();
+	Vector3f rate_sp;
 
 	if (_state == state::roll) {
 		rate_sp(0) = signal;
@@ -621,8 +599,6 @@ const Vector3f McAutotuneAttitudeControl::getIdentificationSignal()
 		rate_sp(1) = signal;
 	}
 
-	_signal_filter.update(step);
-
 	return rate_sp;
 }
 

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp

@@ -43,6 +43,7 @@
 #include <lib/perf/perf_counter.h>
 #include <lib/pid_design/pid_design.hpp>
 #include <lib/system_identification/system_identification.hpp>
+#include <lib/system_identification/signal_generator.hpp>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
@@ -93,14 +94,13 @@ private:
 	void updateStateMachine(hrt_abstime now);
 	bool registerActuatorControlsCallback();
 	void stopAutotune();
-	bool areAllSmallerThan(const matrix::Vector<float, 5> &vect, float threshold) const;
 	void copyGains(int index);
 	bool areGainsGood() const;
 	void saveGainsToParams();
 	void backupAndSaveGainsToParams();
 	void revertParamGains();
 
-	const matrix::Vector3f getIdentificationSignal();
+	const matrix::Vector3f getIdentificationSignal(hrt_abstime now);
 
 	uORB::SubscriptionCallbackWorkItem _vehicle_torque_setpoint_sub{this, ORB_ID(vehicle_torque_setpoint)};
 	uORB::SubscriptionCallbackWorkItem _parameter_update_sub{this, ORB_ID(parameter_update)};
@@ -131,13 +131,17 @@ private:
 		wait_for_disarm = autotune_attitude_control_status_s::STATE_WAIT_FOR_DISARM
 	} _state{state::idle};
 
+	enum class SignalType : uint8_t {
+		kLinearSineSweep = 0,
+		kLogSineSweep
+	};
+
 	hrt_abstime _state_start_time{0};
-	uint8_t _steps_counter{0};
-	uint8_t _max_steps{5};
-	int8_t _signal_sign{0};
 
 	bool _armed{false};
 
+	matrix::Vector3f _angular_velocity{};
+
 	matrix::Vector3f _kid{};
 	matrix::Vector3f _rate_k{};
 	matrix::Vector3f _rate_i{};
@@ -150,27 +154,16 @@ private:
 
 	bool _gains_backup_available{false}; // true if a backup of the parameters has been done
 
-	/**
-	 * Scale factor applied to the input data to have the same input/output range
-	 * When input and output scales are a lot different, some elements of the covariance
-	 * matrix will collapse much faster than other ones, creating an ill-conditionned matrix
-	 */
-	float _input_scale{1.f};
-
 	hrt_abstime _last_run{0};
 	hrt_abstime _last_publish{0};
 	hrt_abstime _last_model_update{0};
 
 	float _interval_sum{0.f};
-	float _interval_count{0.f};
+	uint32_t _interval_count{0};
 	float _sample_interval_avg{0.01f};
 	float _filter_dt{0.01f};
 	bool _are_filters_initialized{false};
 
-	AlphaFilter<float> _signal_filter; ///< used to create a wash-out filter
-
-	static constexpr float _model_dt_min{2e-3f}; // 2ms = 500Hz
-	static constexpr float _model_dt_max{10e-3f}; // 10ms = 100Hz
 	int _model_update_scaler{1};
 	int _model_update_counter{0};
 
@@ -179,6 +172,11 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamBool<px4::params::MC_AT_START>) _param_mc_at_start,
 		(ParamFloat<px4::params::MC_AT_SYSID_AMP>) _param_mc_at_sysid_amp,
+		(ParamFloat<px4::params::MC_AT_SYSID_F0>) _param_mc_at_sysid_f0,
+		(ParamFloat<px4::params::MC_AT_SYSID_F1>) _param_mc_at_sysid_f1,
+		(ParamFloat<px4::params::MC_AT_SYSID_FYAW>) _param_mc_at_sysid_fyaw,
+		(ParamFloat<px4::params::MC_AT_SYSID_TIME>) _param_mc_at_sysid_time,
+		(ParamInt<px4::params::MC_AT_SYSID_TYPE>) _param_mc_at_sysid_type,
 		(ParamInt<px4::params::MC_AT_APPLY>) _param_mc_at_apply,
 		(ParamFloat<px4::params::MC_AT_RISE_TIME>) _param_mc_at_rise_time,
 
@@ -201,6 +199,6 @@ private:
 		(ParamFloat<px4::params::MC_YAW_P>) _param_mc_yaw_p
 	)
 
-	static constexpr float _publishing_dt_s = 100e-3f;
-	static constexpr hrt_abstime _publishing_dt_hrt = 100_ms;
+	static constexpr float _publishing_dt_s = 5e-3f;
+	static constexpr float _kInitVar = 10.f;
 };

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control_params.c

@@ -76,6 +76,71 @@ PARAM_DEFINE_INT32(MC_AT_START, 0);
  */
 PARAM_DEFINE_FLOAT(MC_AT_SYSID_AMP, 0.7);
 
+/**
+ * Start frequency of the injected signal
+ *
+ * Can be set lower or higher than the end frequency
+ *
+ * @min 0.1
+ * @max 30.0
+ * @decimal 1
+ * @unit Hz
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(MC_AT_SYSID_F0, 1.f);
+
+/**
+ * End frequency of the injected signal
+ *
+ * Can be set lower or higher than the start frequency
+ *
+ * @min 0.1
+ * @max 30.0
+ * @decimal 1
+ * @unit Hz
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(MC_AT_SYSID_F1, 20.f);
+
+/**
+ * Yaw axis maximum frequency
+ *
+ * End frequency of the identification for the yaw axis.
+ * This is usually set lower than the roll and pitch axes
+ * to only consider the yaw actuation produced by propeller drag.
+ *
+ * @min 0.1
+ * @max 30.0
+ * @decimal 1
+ * @unit Hz
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(MC_AT_SYSID_FYAW, 5.f);
+
+/**
+ * Maneuver time for each axis
+ *
+ * Duration of the input signal sent on each axis during system identification
+ *
+ * @min 5
+ * @max 120
+ * @decimal 0
+ * @unit s
+ * @group Autotune
+ */
+PARAM_DEFINE_FLOAT(MC_AT_SYSID_TIME, 10.f);
+
+/**
+ * Input signal type
+ *
+ * Type of signal used during system identification to excite the system.
+ *
+ * @value 0 Linear sine sweep
+ * @value 1 Logarithmic sine sweep
+ * @group Autotune
+ */
+PARAM_DEFINE_INT32(MC_AT_SYSID_TYPE, 1);
+
 /**
  * Controls when to apply the new gains
  *