From 560a08018b8c392d0e798523a8758e4967759d24 Mon Sep 17 00:00:00 2001 From: Mario Goebbels Date: Mon, 30 Apr 2018 16:32:56 +0200 Subject: [PATCH 1/2] lqg: Expose control cost on LQR, remove beta offset for yaw. --- flight/Libraries/math/lqg.c | 19 +++++++++---------- flight/Libraries/math/lqg.h | 4 ++-- flight/Modules/Stabilization/stabilization.c | 11 ++++------- shared/uavobjectdefinition/lqgsettings.xml | 2 +- 4 files changed, 16 insertions(+), 20 deletions(-) diff --git a/flight/Libraries/math/lqg.c b/flight/Libraries/math/lqg.c index 43f0a3a7c7..337e8b8d07 100644 --- a/flight/Libraries/math/lqg.c +++ b/flight/Libraries/math/lqg.c @@ -252,10 +252,9 @@ bool rtkf_is_solved(rtkf_t rtkf) P = A'PA - A'PB (R+B'PB)^-1 B'PA + Q */ -void lqr_calculate_covariance_2x2(float A[2][2], float B[2], float P[2][2], float Q[2][2]) +void lqr_calculate_covariance_2x2(float A[2][2], float B[2], float P[2][2], float Q[2][2], float R) { float nP[2][2]; - const float R = 1; float B0B0 = B0*B0; float B1B1 = B1*B1; @@ -306,6 +305,7 @@ struct lqr_state { float B[2]; float K[2]; float P[2][2]; + float R; float Q[2][2]; float u; @@ -319,15 +319,12 @@ struct lqr_state { K = (R + B'PB)^-1 B'PA */ -void lqr_calculate_gains_int(float A[2][2], float B[2], float P[2][2], float K[2]) +void lqr_calculate_gains_int(float A[2][2], float B[2], float P[2][2], float K[2], float R) { - const float R = 1; - float div = (R + B1*B1*P11 + B0*(B0*P00 + B1*(P01 + P10))); K[0] = (A00*(B0*P00 + B1*P10)) / div; K[1] = (A01*(B0*P00 + B1*P10) + A11*(B0*P01 + B1*P11)) / div; - } /* @@ -353,10 +350,10 @@ void lqr_stabilize_covariance(lqr_t lqr, int iterations) PIOS_Assert(lqr); for (int i = 0; i < iterations; i++) { - lqr_calculate_covariance_2x2(lqr->A, lqr->B, lqr->P, lqr->Q); + lqr_calculate_covariance_2x2(lqr->A, lqr->B, lqr->P, lqr->Q, lqr->R); } - lqr_calculate_gains_int(lqr->A, lqr->B, lqr->P, lqr->K); + lqr_calculate_gains_int(lqr->A, lqr->B, lqr->P, lqr->K, lqr->R); lqr->solver_iterations += iterations; } @@ -393,7 +390,7 @@ void lqr_initialize_matrices_int(float A[2][2], float B[2], float beta, float ta Current workable values for 5" miniquads seems to be Q1 = 0.00001, Q2 = 0.00013333. */ -lqr_t lqr_create(float beta, float tau, float Ts, float q1, float q2) +lqr_t lqr_create(float beta, float tau, float Ts, float q1, float q2, float r) { struct lqr_state *state = PIOS_malloc_no_dma(sizeof(*state)); PIOS_Assert(state); @@ -402,6 +399,7 @@ lqr_t lqr_create(float beta, float tau, float Ts, float q1, float q2) lqr_initialize_matrices_int(state->A, state->B, expf(beta), tau, Ts); state->Q00 = q1; state->Q11 = q2*expf(beta); + state->R = r; state->beta = beta; state->tau = tau; @@ -409,12 +407,13 @@ lqr_t lqr_create(float beta, float tau, float Ts, float q1, float q2) return state; } -void lqr_update(lqr_t lqr, float q1, float q2) +void lqr_update(lqr_t lqr, float q1, float q2, float r) { PIOS_Assert(lqr); lqr->Q00 = q1; lqr->Q11 = q2*expf(lqr->beta); + lqr->R = r; lqr->solver_iterations = 0; } diff --git a/flight/Libraries/math/lqg.h b/flight/Libraries/math/lqg.h index 2dd7b7a5a7..811df39801 100644 --- a/flight/Libraries/math/lqg.h +++ b/flight/Libraries/math/lqg.h @@ -42,11 +42,11 @@ extern rtkf_t rtkf_create(float beta, float tau, float Ts, float R, float Q1, fl extern void rtkf_stabilize_covariance(rtkf_t rtkf, int iterations); extern bool rtkf_is_solved(rtkf_t rtkf); -extern lqr_t lqr_create(float beta, float tau, float Ts, float q1, float q2); +extern lqr_t lqr_create(float beta, float tau, float Ts, float q1, float q2, float r); extern void lqr_stabilize_covariance(lqr_t lqr, int iterations); extern bool lqr_is_solved(lqr_t lqr); -extern void lqr_update(lqr_t lqr, float q1, float q2); +extern void lqr_update(lqr_t lqr, float q1, float q2, float r); extern void lqr_get_gains(lqr_t lqg, float K[2]); extern lqg_t lqg_create(rtkf_t rtkf, lqr_t lqr); diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 6760eb7b3b..97b29cf3c1 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -439,18 +439,14 @@ static void initialize_lqg_controllers(float dT) lqr_t lqr = lqg_get_lqr(lqg[i]); lqr_update(lqr, lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ1 : LQGSETTINGS_LQR_Q1], - lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ2 : LQGSETTINGS_LQR_Q2] + lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ2 : LQGSETTINGS_LQR_Q2], + lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWR : LQGSETTINGS_LQR_R] ); } else { /* Initial setup. */ float beta = sysIdent.Beta[i]; float tau = (sysIdent.Tau[0] + sysIdent.Tau[1]) * 0.5f; - if (i == YAW) - { - beta += lqgSettings.LQR[LQGSETTINGS_LQR_YAWBETAOFFSET]; - } - if (tau > 0.001f && beta > 6) { rtkf_t rtkf = rtkf_create(beta, tau, dT, lqgSettings.RTKF[i == YAW ? LQGSETTINGS_RTKF_YAWR : LQGSETTINGS_RTKF_R], @@ -461,7 +457,8 @@ static void initialize_lqg_controllers(float dT) ); lqr_t lqr = lqr_create(beta, tau, dT, lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ1 : LQGSETTINGS_LQR_Q1], - lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ2 : LQGSETTINGS_LQR_Q2] + lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ2 : LQGSETTINGS_LQR_Q2], + lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWR : LQGSETTINGS_LQR_R] ); lqg[i] = lqg_create(rtkf, lqr); } diff --git a/shared/uavobjectdefinition/lqgsettings.xml b/shared/uavobjectdefinition/lqgsettings.xml index f38ac02d25..ebb29da879 100644 --- a/shared/uavobjectdefinition/lqgsettings.xml +++ b/shared/uavobjectdefinition/lqgsettings.xml @@ -8,7 +8,7 @@ Weights for the Rate-Torque Kalman filter. - + Weights for the LQ-regulator. From d1844b26aa4290e7f1e39d6be49aac797ea9a1b0 Mon Sep 17 00:00:00 2001 From: Mario Goebbels Date: Sat, 5 May 2018 19:36:51 +0200 Subject: [PATCH 2/2] lqg: Rename LQR field to LQRegulator. So that UAVO exports of earlier nightly builds don't break the LQG on import. --- flight/Modules/Stabilization/stabilization.c | 12 ++++++------ shared/uavobjectdefinition/lqgsettings.xml | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index 97b29cf3c1..fed65623c4 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -438,9 +438,9 @@ static void initialize_lqg_controllers(float dT) /* Update Q matrix. */ lqr_t lqr = lqg_get_lqr(lqg[i]); lqr_update(lqr, - lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ1 : LQGSETTINGS_LQR_Q1], - lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ2 : LQGSETTINGS_LQR_Q2], - lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWR : LQGSETTINGS_LQR_R] + lqgSettings.LQRegulator[i == YAW ? LQGSETTINGS_LQREGULATOR_YAWQ1 : LQGSETTINGS_LQREGULATOR_Q1], + lqgSettings.LQRegulator[i == YAW ? LQGSETTINGS_LQREGULATOR_YAWQ2 : LQGSETTINGS_LQREGULATOR_Q2], + lqgSettings.LQRegulator[i == YAW ? LQGSETTINGS_LQREGULATOR_YAWR : LQGSETTINGS_LQREGULATOR_R] ); } else { /* Initial setup. */ @@ -456,9 +456,9 @@ static void initialize_lqg_controllers(float dT) lqgSettings.RTKF[LQGSETTINGS_RTKF_BIASLIMIT] ); lqr_t lqr = lqr_create(beta, tau, dT, - lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ1 : LQGSETTINGS_LQR_Q1], - lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWQ2 : LQGSETTINGS_LQR_Q2], - lqgSettings.LQR[i == YAW ? LQGSETTINGS_LQR_YAWR : LQGSETTINGS_LQR_R] + lqgSettings.LQRegulator[i == YAW ? LQGSETTINGS_LQREGULATOR_YAWQ1 : LQGSETTINGS_LQREGULATOR_Q1], + lqgSettings.LQRegulator[i == YAW ? LQGSETTINGS_LQREGULATOR_YAWQ2 : LQGSETTINGS_LQREGULATOR_Q2], + lqgSettings.LQRegulator[i == YAW ? LQGSETTINGS_LQREGULATOR_YAWR : LQGSETTINGS_LQREGULATOR_R] ); lqg[i] = lqg_create(rtkf, lqr); } diff --git a/shared/uavobjectdefinition/lqgsettings.xml b/shared/uavobjectdefinition/lqgsettings.xml index ebb29da879..905a75d034 100644 --- a/shared/uavobjectdefinition/lqgsettings.xml +++ b/shared/uavobjectdefinition/lqgsettings.xml @@ -8,7 +8,7 @@ Weights for the Rate-Torque Kalman filter. - + Weights for the LQ-regulator.