From 7f8a4dc5ddd67635adceb5ae0f8ba03125c1f987 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Thu, 4 Jan 2024 09:46:32 +0100
Subject: [PATCH] arx-rls: update RLS algorithm

Rearrange to look like a KF
---
 src/lib/system_identification/arx_rls.hpp     | 21 +++++--------------
 .../mc_autotune_attitude_control.cpp          | 11 ++++++----
 2 files changed, 12 insertions(+), 20 deletions(-)

diff --git a/src/lib/system_identification/arx_rls.hpp b/src/lib/system_identification/arx_rls.hpp
index 895832c4d4..0bc8ff73ce 100644
--- a/src/lib/system_identification/arx_rls.hpp
+++ b/src/lib/system_identification/arx_rls.hpp
@@ -90,7 +90,7 @@ public:
 		_P.setZero();
 
 		for (size_t i = 0; i < (N + M + 1); i++) {
-			_P(i, i) = 10e3f;
+			_P(i, i) = 10.f;
 		}
 
 		_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;
diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
index d02c043589..0f865829fd 100644
--- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
+++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
@@ -275,7 +275,10 @@ 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;
+	const float converged_thr = 10.f;
+	Vector<float, 5> state_init;
+	state_init(0) = -1.5f;
+	state_init(1) = 0.5f;
 
 	switch (_state) {
 	case state::idle:
@@ -296,7 +299,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if (_are_filters_initialized) {
 			_state = state::roll;
 			_state_start_time = now;
-			_sys_id.reset();
+			_sys_id.reset(state_init);
 			_gains_backup_available = false;
 		}
 
@@ -318,7 +321,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if ((now - _state_start_time) > 2_s) {
 			_state = state::pitch;
 			_state_start_time = now;
-			_sys_id.reset();
+			_sys_id.reset(state_init);
 		}
 
 		break;
@@ -337,7 +340,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		if ((now - _state_start_time) > 2_s) {
 			_state = state::yaw;
 			_state_start_time = now;
-			_sys_id.reset();
+			_sys_id.reset(state_init);
 		}
 
 		break;