Paparazzi UAS  v5.17_devel-0-g9d9a0d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
wls_alloc.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) Anton Naruta && Daniel Hoppener
3  * MAVLab Delft University of Technology
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
38 #include "wls_alloc.h"
39 #include <stdio.h>
40 #include "std.h"
41 
42 #include <string.h>
43 #include <math.h>
44 #include <float.h>
45 #include "math/qr_solve/qr_solve.h"
47 
48 void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax);
49 void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free);
50 
51 // provide loop feedback
52 #define WLS_VERBOSE FALSE
53 
54 // Problem size needs to be predefined to avoid having to use VLAs
55 #ifndef CA_N_V
56 #error CA_N_V needs to be defined!
57 #endif
58 
59 #ifndef CA_N_U
60 #error CA_N_U needs to be defined!
61 #endif
62 
63 #define CA_N_C (CA_N_U+CA_N_V)
64 
74 void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
75  float in[m * n];
76  // convert A to 1d array
77  int k = 0;
78  for (int j = 0; j < n; j++) {
79  for (int i = 0; i < m; i++) {
80  in[k++] = A[i][j];
81  }
82  }
83  // use solver
84  qr_solve(m, n, in, b, x);
85 }
86 
112 int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
113  float* u_guess, float* W_init, float* Wv, float* Wu, float* up,
114  float gamma_sq, int imax) {
115  // allocate variables, use defaults where parameters are set to 0
116  if(!gamma_sq) gamma_sq = 100000;
117  if(!imax) imax = 100;
118 
119  int n_c = CA_N_C;
120  int n_u = CA_N_U;
121  int n_v = CA_N_V;
122 
123  float A[CA_N_C][CA_N_U];
124  float A_free[CA_N_C][CA_N_U];
125 
126  // Create a pointer array to the rows of A_free
127  // such that we can pass it to a function
128  float * A_free_ptr[CA_N_C];
129  for(int i = 0; i < n_c; i++)
130  A_free_ptr[i] = A_free[i];
131 
132  float b[CA_N_C];
133  float d[CA_N_C];
134 
135  int free_index[CA_N_U];
136  int free_index_lookup[CA_N_U];
137  int n_free = 0;
138  int free_chk = -1;
139 
140  int iter = 0;
141  float p_free[CA_N_U];
142  float p[CA_N_U];
143  float u_opt[CA_N_U];
144  int infeasible_index[CA_N_U] UNUSED;
145  int n_infeasible = 0;
146  float lambda[CA_N_U];
147  float W[CA_N_U];
148 
149  // Initialize u and the working set, if provided from input
150  if (!u_guess) {
151  for (int i = 0; i < n_u; i++) {
152  u[i] = (umax[i] + umin[i]) * 0.5;
153  }
154  } else {
155  for (int i = 0; i < n_u; i++) {
156  u[i] = u_guess[i];
157  }
158  }
159  W_init ? memcpy(W, W_init, n_u * sizeof(float))
160  : memset(W, 0, n_u * sizeof(float));
161 
162  memset(free_index_lookup, -1, n_u * sizeof(float));
163 
164 
165  // find free indices
166  for (int i = 0; i < n_u; i++) {
167  if (W[i] == 0) {
168  free_index_lookup[i] = n_free;
169  free_index[n_free++] = i;
170  }
171  }
172 
173  // fill up A, A_free, b and d
174  for (int i = 0; i < n_v; i++) {
175  // If Wv is a NULL pointer, use Wv = identity
176  b[i] = Wv ? gamma_sq * Wv[i] * v[i] : gamma_sq * v[i];
177  d[i] = b[i];
178  for (int j = 0; j < n_u; j++) {
179  // If Wv is a NULL pointer, use Wv = identity
180  A[i][j] = Wv ? gamma_sq * Wv[i] * B[i][j] : gamma_sq * B[i][j];
181  d[i] -= A[i][j] * u[j];
182  }
183  }
184  for (int i = n_v; i < n_c; i++) {
185  memset(A[i], 0, n_u * sizeof(float));
186  A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0;
187  b[i] = up ? (Wu ? Wu[i-n_v] * up[i-n_v] : up[i-n_v]) : 0;
188  d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
189  }
190 
191  // -------------- Start loop ------------
192  while (iter++ < imax) {
193  // clear p, copy u to u_opt
194  memset(p, 0, n_u * sizeof(float));
195  memcpy(u_opt, u, n_u * sizeof(float));
196 
197  // Construct a matrix with the free columns of A
198  if (free_chk != n_free) {
199  for (int i = 0; i < n_c; i++) {
200  for (int j = 0; j < n_free; j++) {
201  A_free[i][j] = A[i][free_index[j]];
202  }
203  }
204  free_chk = n_free;
205  }
206 
207 
208  if (n_free) {
209  // Still free variables left, calculate corresponding solution
210 
211  // use a solver to find the solution to A_free*p_free = d
212  qr_solve_wrapper(n_c, n_free, A_free_ptr, d, p_free);
213 
214  //print results current step
215 #if WLS_VERBOSE
216  print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
217 #endif
218 
219  }
220 
221  // Set the nonzero values of p and add to u_opt
222  for (int i = 0; i < n_free; i++) {
223  p[free_index[i]] = p_free[i];
224  u_opt[free_index[i]] += p_free[i];
225  }
226  // check limits
227  n_infeasible = 0;
228  for (int i = 0; i < n_u; i++) {
229  if (u_opt[i] >= (umax[i] + 1.0) || u_opt[i] <= (umin[i] - 1.0)) {
230  infeasible_index[n_infeasible++] = i;
231  }
232  }
233 
234  // Check feasibility of the solution
235  if (n_infeasible == 0) {
236  // all variables are within limits
237  memcpy(u, u_opt, n_u * sizeof(float));
238  memset(lambda, 0, n_u * sizeof(float));
239 
240  // d = d + A_free*p_free; lambda = A*d;
241  for (int i = 0; i < n_c; i++) {
242  for (int k = 0; k < n_free; k++) {
243  d[i] -= A_free[i][k] * p_free[k];
244  }
245  for (int k = 0; k < n_u; k++) {
246  lambda[k] += A[i][k] * d[i];
247  }
248  }
249  bool break_flag = true;
250 
251  // lambda = lambda x W;
252  for (int i = 0; i < n_u; i++) {
253  lambda[i] *= W[i];
254  // if any lambdas are negative, keep looking for solution
255  if (lambda[i] < -FLT_EPSILON) {
256  break_flag = false;
257  W[i] = 0;
258  // add a free index
259  if (free_index_lookup[i] < 0) {
260  free_index_lookup[i] = n_free;
261  free_index[n_free++] = i;
262  }
263  }
264  }
265  if (break_flag) {
266 
267 #if WLS_VERBOSE
268  print_final_values(1, n_u, n_v, u, B, v, umin, umax);
269 #endif
270 
271  // if solution is found, return number of iterations
272  return iter;
273  }
274  } else {
275  float alpha = INFINITY;
276  float alpha_tmp;
277  int id_alpha = 0;
278 
279  // find the lowest distance from the limit among the free variables
280  for (int i = 0; i < n_free; i++) {
281  int id = free_index[i];
282  if(fabs(p[id]) > FLT_EPSILON) {
283  alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
284  : (umax[id] - u[id]) / p[id];
285  } else {
286  alpha_tmp = INFINITY;
287  }
288  if (alpha_tmp < alpha) {
289  alpha = alpha_tmp;
290  id_alpha = id;
291  }
292  }
293 
294  // update input u = u + alpha*p
295  for (int i = 0; i < n_u; i++) {
296  u[i] += alpha * p[i];
297  }
298  // update d = d-alpha*A*p_free
299  for (int i = 0; i < n_c; i++) {
300  for (int k = 0; k < n_free; k++) {
301  d[i] -= A_free[i][k] * alpha * p_free[k];
302  }
303  }
304  // get rid of a free index
305  W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;
306 
307  free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
308  free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
309  free_index_lookup[id_alpha];
310  free_index_lookup[id_alpha] = -1;
311  }
312  }
313  // solution failed, return negative one to indicate failure
314  return -1;
315 }
316 
317 #if WLS_VERBOSE
318 void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
319 
320  printf("n_c = %d n_free = %d\n", n_c, n_free);
321 
322  printf("A_free =\n");
323  for(int i = 0; i < n_c; i++) {
324  for (int j = 0; j < n_free; j++) {
325  printf("%f ", A_free_ptr[i][j]);
326  }
327  printf("\n");
328  }
329 
330  printf("d = ");
331  for (int j = 0; j < n_c; j++) {
332  printf("%f ", d[j]);
333  }
334 
335  printf("\noutput = ");
336  for (int j = 0; j < n_free; j++) {
337  printf("%f ", p_free[j]);
338  }
339  printf("\n\n");
340 }
341 
342 void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
343  printf("n_u = %d n_v = %d\n", n_u, n_v);
344 
345  printf("B =\n");
346  for(int i = 0; i < n_v; i++) {
347  for (int j = 0; j < n_u; j++) {
348  printf("%f ", B[i][j]);
349  }
350  printf("\n");
351  }
352 
353  printf("v = ");
354  for (int j = 0; j < n_v; j++) {
355  printf("%f ", v[j]);
356  }
357 
358  printf("\nu = ");
359  for (int j = 0; j < n_u; j++) {
360  printf("%f ", u[j]);
361  }
362  printf("\n");
363 
364  printf("\numin = ");
365  for (int j = 0; j < n_u; j++) {
366  printf("%f ", umin[j]);
367  }
368  printf("\n");
369 
370  printf("\numax = ");
371  for (int j = 0; j < n_u; j++) {
372  printf("%f ", umax[j]);
373  }
374  printf("\n\n");
375 
376 }
377 #endif
uint8_t last_wp UNUSED
Definition: navigation.c:92
float alpha
Definition: textons.c:107
void print_final_values(int n_u, int n_v, float *u, float **B, float *v, float *umin, float *umax)
#define B
uint16_t up[LIGHT_NB]
Definition: light_solar.c:48
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, float gamma_sq, int imax)
active set algorithm for control allocation
Definition: wls_alloc.c:112
void qr_solve(int m, int n, float a[], float b[], float x[])
Definition: qr_solve.c:1548
void print_in_and_outputs(int n_c, int n_free, float **A_free_ptr, float *d, float *p_free)
static float p[2][2]
#define A
#define CA_N_C
Definition: wls_alloc.c:63
void qr_solve_wrapper(int m, int n, float **A, float *b, float *x)
Wrapper for qr solve.
Definition: wls_alloc.c:74
static float Wv[INDI_OUTPUTS]