atune: paremeterize intial RLS variance

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) = 10.f;
+			_P(i, i) = var_init;
 		}
 
 		_diff_theta_hat.setZero();

src/lib/system_identification/system_identification.cpp

@@ -39,9 +39,9 @@
 
 #include "system_identification.hpp"
 
-void SystemIdentification::reset(const matrix::Vector<float, _kParameters> &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;

src/lib/system_identification/system_identification.hpp

@@ -58,7 +58,7 @@ public:
 	SystemIdentification() = default;
 	~SystemIdentification() = default;
 
-	void reset(const matrix::Vector<float, _kParameters> &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);

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -299,7 +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 Vector<float, SystemIdentification::_kParameters> sys_id_init;
+	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/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp

@@ -274,8 +274,6 @@ void McAutotuneAttitudeControl::checkFilters()
 
 void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 {
-	// when identifying an axis, check if the estimate has converged
-	const float converged_thr = 10.f;
 	Vector<float, 5> state_init;
 	state_init(0) = -1.5f;
 	state_init(1) = 0.5f;
@@ -299,14 +297,14 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if (_are_filters_initialized) {
 			_state = state::roll;
 			_state_start_time = now;
-			_sys_id.reset(state_init);
+			_sys_id.reset(state_init, _kInitVar);
 			_gains_backup_available = false;
 		}
 
 		break;
 
 	case state::roll:
-		if ((_sys_id.getVariances().max() < converged_thr)
+		if ((_sys_id.getVariances().max() < _kInitVar)
 		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(0);
 
@@ -321,13 +319,13 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if ((now - _state_start_time) > 2_s) {
 			_state = state::pitch;
 			_state_start_time = now;
-			_sys_id.reset(state_init);
+			_sys_id.reset(state_init, _kInitVar);
 		}
 
 		break;
 
 	case state::pitch:
-		if ((_sys_id.getVariances().max() < converged_thr)
+		if ((_sys_id.getVariances().max() < _kInitVar)
 		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(1);
 			_state = state::pitch_pause;
@@ -340,13 +338,13 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if ((now - _state_start_time) > 2_s) {
 			_state = state::yaw;
 			_state_start_time = now;
-			_sys_id.reset(state_init);
+			_sys_id.reset(state_init, _kInitVar);
 		}
 
 		break;
 
 	case state::yaw:
-		if ((_sys_id.getVariances().max() < converged_thr)
+		if ((_sys_id.getVariances().max() < _kInitVar)
 		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(2);
 			_state = state::yaw_pause;

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp

@@ -200,4 +200,5 @@ private:
 	)
 
 	static constexpr float _publishing_dt_s = 5e-3f;
+	static constexpr float _kInitVar = 10.f;
 };