Fix indi wu initializer (#3032)

* update WU init and always return iter * add another test case for WLS
paparazzi · May 16, 2023 · 5dacd6a · 5dacd6a
1 parent b9564dd
commit 5dacd6a
Show file tree

Hide file tree

Showing 3 changed files with 81 additions and 8 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -139,7 +139,7 @@ static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
 #ifdef STABILIZATION_INDI_WLS_WU
 float indi_Wu[INDI_NUM_ACT] = STABILIZATION_INDI_WLS_WU;
 #else
-float indi_Wu[INDI_NUM_ACT] = {1.0};
+float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT-1] = 1.0};
 #endif
 
 // variables needed for control

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c b/sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c
@@ -107,7 +107,7 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
  * control vector (sqare root of gamma)
  * @param imax Max number of iterations
  *
- * @return Number of iterations, -1 upon failure
+ * @return Number of iterations
  */
 int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
     float* u_guess, float* W_init, float* Wv, float* Wu, float* up,
@@ -226,7 +226,7 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
     // check limits
     n_infeasible = 0;
     for (int i = 0; i < n_u; i++) {
-      if (u_opt[i] >= (umax[i] + 1.0) || u_opt[i] <= (umin[i] - 1.0)) {
+      if (u_opt[i] >= (umax[i] + 0.01) || u_opt[i] <= (umin[i] - 0.01)) {
         infeasible_index[n_infeasible++] = i;
       }
     }
@@ -265,7 +265,7 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
       if (break_flag) {
 
 #if WLS_VERBOSE
-        print_final_values(1, n_u, n_v, u, B, v, umin, umax);
+        print_final_values(n_u, n_v, u, B, v, umin, umax);
 #endif
 
         // if solution is found, return number of iterations
@@ -311,7 +311,7 @@ int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
     }
   }
   // solution failed, return negative one to indicate failure
-  return -1;
+  return iter;
 }
 
 #if WLS_VERBOSE

diff --git a/sw/airborne/test/test_alloc.c b/sw/airborne/test/test_alloc.c
@@ -35,12 +35,18 @@
 #define INDI_OUTPUTS 4
 
 void test_overdetermined(void);
+void test_overdetermined2(void);
 void calc_nu_out(float** Bwls, float* du, float* nu_out);
 
 int main(int argc, char **argv)
 {
-#define INDI_NUM_ACT 6
-  test_overdetermined();
+// #define INDI_NUM_ACT 6
+  // test_overdetermined();
+
+  // Make sure to set CA_N_U to 8 in the test makefile!
+  #define INDI_NUM_ACT 8
+  test_overdetermined2();
+
 /*#define INDI_NUM_ACT 4*/
   /*test_four_by_four();*/
 }
@@ -153,12 +159,79 @@ void test_overdetermined(void)
   printf("nu_out = %f, %f, %f, %f\n", nu_out[0], nu_out[1], nu_out[2], nu_out[3]);
 }
 
+/*
+ * function to test wls for an overdetermined 4x6 (outputs x inputs) system
+ */
+void test_overdetermined2(void)
+{
+  float u_min[INDI_NUM_ACT] = {0};
+  float u_max[INDI_NUM_ACT] = {0};
+  float du_min[INDI_NUM_ACT] = {1500.0, 1500.0, 1500.0, 1500.0, -9600.0, -9600.0, -9600.0, -9600.0};
+  float du_max[INDI_NUM_ACT] = {9600.0, 9600.0, 9600.0, 9600.0, 9600.0, 9600.0, 9600.0, 9600.0};
+
+  float u_p[INDI_NUM_ACT] = {0};
+
+  printf("lower and upper bounds for du:\n");
+
+  uint8_t k;
+  for(k=0; k<INDI_NUM_ACT; k++) {
+    printf("%f ", du_min[k]);
+    printf("%f \n", du_max[k]);
+  }
+
+  printf("\n");
+
+float g1g2[INDI_OUTPUTS][INDI_NUM_ACT] = {
+   { 0.3310,   -0.3310,    0.8930,   -0.8930,         0,         0,         0,         0},
+   { 0.4650,    0.4650,   -0.9300,   -0.9300,   -0.2240,   -0.2240,   -0.2240,   -0.2240},
+   {      0,         0,         0,         0,    0.2150,   -0.2150,   -0.2150,    0.2150},
+   {-0.3700,   -0.3700,   -0.5500,   -0.5500,         0,         0,         0,         0}
+};
+uint8_t i,j;
+for (i=0; i< INDI_OUTPUTS;i++) {
+  for (j=0; j< INDI_NUM_ACT;j++) {
+    g1g2[i][j] = g1g2[i][j]/1000.0f;
+  }
+}
+
+  //State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
+  static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
+
+  // The control objective in array format
+  float indi_v[INDI_OUTPUTS] = {0.0712,    0.5877,    0.1185,   -0.2238};
+  // float indi_v[INDI_OUTPUTS] = {0.0738,    0.5876,    0.1187,   -0.2237};
+  float indi_du[INDI_NUM_ACT];
+
+  // Initialize the array of pointers to the rows of g1g2
+  float *Bwls[INDI_OUTPUTS];
+  for (i = 0; i < INDI_OUTPUTS; i++) {
+    Bwls[i] = g1g2[i];
+  }
+
+  float indi_Wu[INDI_NUM_ACT] = {[0 ... INDI_NUM_ACT-1] = 1.0};
+
+  // WLS Control Allocator
+  int num_iter =
+    wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, indi_Wu, u_p, 0, 15);
+
+  printf("finished in %d iterations\n", num_iter);
+
+  float nu_out[4] = {0.0f};
+  calc_nu_out(Bwls, indi_du, nu_out);
+
+  printf("du                 = %f, %f, %f, %f, %f, %f, %f, %f\n", indi_du[0], indi_du[1], indi_du[2], indi_du[3], indi_du[4], indi_du[5], indi_du[6], indi_du[7]);
+  // Precomputed solution' in Matlab for this problem using lsqlin:
+  // printf("du (matlab_lsqlin) = %f, %f, %f, %f, %f, %f\n", -4614.0, 426.064612091305, 5390.0, -4614.0, -4210.0, 5390.0);
+    printf("nu_in = %f, %f, %f, %f\n", indi_v[0], indi_v[1], indi_v[2], indi_v[3]);
+  printf("nu_out = %f, %f, %f, %f\n", nu_out[0], nu_out[1], nu_out[2], nu_out[3]);
+}
+
 /*
  * Calculate the achieved control objective for some calculated control input
  */
 void calc_nu_out(float** Bwls, float* du, float* nu_out) {
 
-  for(int i=0; i<4; i++) {
+  for(int i=0; i<INDI_OUTPUTS; i++) {
     nu_out[i] = 0;
     for(int j=0; j<INDI_NUM_ACT; j++) {
       nu_out[i] += Bwls[i][j] * du[j];