From 933d7b9ebc42d51b464172be043eafa19163f7c1 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 21 Dec 2023 16:52:10 +0100 Subject: [PATCH] sys_id: parametrize number of zeros, poles and delays --- .../system_identification.cpp | 6 ++--- .../system_identification.hpp | 15 +++++++---- .../fw_autotune_attitude_control.cpp | 27 ++++++++++++------- .../mc_autotune_attitude_control.cpp | 17 +++++++++--- 4 files changed, 43 insertions(+), 22 deletions(-) diff --git a/src/lib/system_identification/system_identification.cpp b/src/lib/system_identification/system_identification.cpp index c95bea6bac..9c9ea0933b 100644 --- a/src/lib/system_identification/system_identification.cpp +++ b/src/lib/system_identification/system_identification.cpp @@ -39,7 +39,7 @@ #include "system_identification.hpp" -void SystemIdentification::reset(const matrix::Vector &id_state_init) +void SystemIdentification::reset(const matrix::Vector &id_state_init) { _rls.reset(id_state_init); _u_lpf.reset(0.f); @@ -88,10 +88,10 @@ void SystemIdentification::updateFilters(float u, float y) void SystemIdentification::updateFitness() { - const matrix::Vector &diff = _rls.getDiffEstimate(); + const matrix::Vector &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); } diff --git a/src/lib/system_identification/system_identification.hpp b/src/lib/system_identification/system_identification.hpp index 10712ed7e0..4773f9b3aa 100644 --- a/src/lib/system_identification/system_identification.hpp +++ b/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 &id_state_init = {}); + void reset(const matrix::Vector &id_state_init = {}); 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 &getCoefficients() const { return _rls.getCoefficients(); } - const matrix::Vector getVariances() const { return _rls.getVariances(); } - const matrix::Vector &getDiffEstimate() const { return _rls.getDiffEstimate(); } + const matrix::Vector getCoefficients() const { return _rls.getCoefficients(); } + const matrix::Vector getVariances() const { return _rls.getVariances(); } + const matrix::Vector getDiffEstimate() const { return _rls.getDiffEstimate(); } float getFitness() const { return _fitness_lpf.getState(); } float getInnovation() const { return _rls.getInnovation(); } @@ -83,7 +88,7 @@ public: float getFilteredOutputData() const { return _y_hpf; } private: - ArxRls<2, 2, 1> _rls; + ArxRls<_kPoles, _kZeros, _kDelays> _rls; math::LowPassFilter2p _u_lpf{400.f, 30.f}; math::LowPassFilter2p _y_lpf{400.f, 30.f}; diff --git a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp b/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp index 7f6e5b9c10..f14f488418 100644 --- a/src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp +++ b/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 coeff = _sys_id.getCoefficients(); - coeff(2) *= _input_scale; - coeff(3) *= _input_scale; - coeff(4) *= _input_scale; + Vector 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 &coeff_var = _sys_id.getVariances(); + const Vector &coeff_var = _sys_id.getVariances(); const Vector3f rate_sp = _sys_id.areFiltersInitialized() ? getIdentificationSignal() : Vector3f(); @@ -291,8 +299,7 @@ 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 sys_id_init(temp); + const Vector sys_id_init; switch (_state) { case state::idle: 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 8cad06f6f1..5882d032ea 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 @@ -169,10 +169,19 @@ void McAutotuneAttitudeControl::Run() const hrt_abstime now = hrt_absolute_time(); updateStateMachine(now); - Vector coeff = _sys_id.getCoefficients(); + Vector 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(_model_update_scaler) * _filter_dt; @@ -190,7 +199,7 @@ void McAutotuneAttitudeControl::Run() // 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 &coeff_var = _sys_id.getVariances(); + const Vector &coeff_var = _sys_id.getVariances(); const Vector3f rate_sp = _sys_id.areFiltersInitialized() ? getIdentificationSignal(now)