Merge pull request #2189 from glowtape/glowtape-lqg-r

lqg: Expose control cost on LQR, remove beta offset for yaw.
d-ronin · May 11, 2018 · e60463a · e60463a
2 parents 6e01839 + d1844b2
commit e60463a
Show file tree

Hide file tree

Showing 4 changed files with 18 additions and 22 deletions.
diff --git 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,19 +399,21 @@ 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;
 
 	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
@@ -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
@@ -438,19 +438,15 @@ 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.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. */
 				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],
@@ -460,8 +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.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
@@ -8,7 +8,7 @@
     <field defaultvalue="25, 75, 1, 0.000003, 0.000001, 0.3" name="RTKF" type="float" units="" elementnames="R, YawR, Q1, Q2, Q3, BiasLimit">
         <description>Weights for the Rate-Torque Kalman filter.</description>
     </field>
-    <field defaultvalue="0.000025, 0.0001, 0, 0.000025, 0.0001" name="LQR" type="float" units="" elementnames="Q1, Q2, YawBetaOffset, YawQ1, YawQ2">
+    <field defaultvalue="1, 0.000025, 0.0001, 1, 0.000025, 0.0001" name="LQRegulator" type="float" units="" elementnames="R, Q1, Q2, YawR, YawQ1, YawQ2">
         <description>Weights for the LQ-regulator.</description>
     </field>
   </object>