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

arx-rls: update RLS algorithm 

Rearrange to look like a KF
This commit is contained in:
bresch 2024-01-04 09:46:32 +01:00
parent 936caafc06
commit 7f8a4dc5dd
2 changed files with 12 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/lib/system_identification/arx_rls.hpp
View File

@ -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;

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

@ -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;