dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sys_id: parametrize number of zeros, poles and delays

This commit is contained in:
bresch 2023-12-21 16:52:10 +01:00
parent e29353c3c4
commit 933d7b9ebc
4 changed files with 43 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/lib/system_identification/system_identification.cpp
View File

@ -39,7 +39,7 @@
#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)
{
_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<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);
}

15
src/lib/system_identification/system_identification.hpp
View File

@ -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 = {});
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(); }
@ -83,7 +88,7 @@ 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};

27
src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp
View File

@ -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,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<float, 5> sys_id_init(temp);
const Vector<float, SystemIdentification::_kParameters> sys_id_init;
switch (_state) {
case state::idle:

17
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
View File

@ -169,10 +169,19 @@ void McAutotuneAttitudeControl::Run()
const hrt_abstime now = hrt_absolute_time();
updateStateMachine(now);
Vector<float, 5> coeff = _sys_id.getCoefficients();
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,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<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(now)