Paparazzi UAS  v5.15_devel-88-gb3ad7fe
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 
63 void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
64  float in[m * n];
65  // convert A to 1d array
66  int k = 0;
67  for (int j = 0; j < n; j++) {
68  for (int i = 0; i < m; i++) {
69  in[k++] = A[i][j];
70  }
71  }
72  // use solver
73  qr_solve(m, n, in, b, x);
74 }
75 
101 int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
102  int n_u, int n_v, float* u_guess, float* W_init, float* Wv,
103  float* Wu, float* up, float gamma_sq, int imax) {
104  // allocate variables, use defaults where parameters are set to 0
105  if(!gamma_sq) gamma_sq = 100000;
106  if(!imax) imax = 100;
107  int n_c = n_u + n_v;
108 
109  float A[n_c][n_u];
110  float A_free[n_c][n_u];
111 
112  // Create a pointer array to the rows of A_free
113  // such that we can pass it to a function
114  float * A_free_ptr[n_c];
115  for(int i = 0; i < n_c; i++)
116  A_free_ptr[i] = A_free[i];
117 
118  float b[n_c];
119  float d[n_c];
120 
121  int free_index[n_u];
122  int free_index_lookup[n_u];
123  int n_free = 0;
124  int free_chk = -1;
125 
126  int iter = 0;
127  float p_free[n_u];
128  float p[n_u];
129  float u_opt[n_u];
130  int infeasible_index[n_u] UNUSED;
131  int n_infeasible = 0;
132  float lambda[n_u];
133  float W[n_u];
134 
135  // Initialize u and the working set, if provided from input
136  if (!u_guess) {
137  for (int i = 0; i < n_u; i++) {
138  u[i] = (umax[i] + umin[i]) * 0.5;
139  }
140  } else {
141  for (int i = 0; i < n_u; i++) {
142  u[i] = u_guess[i];
143  }
144  }
145  W_init ? memcpy(W, W_init, n_u * sizeof(float))
146  : memset(W, 0, n_u * sizeof(float));
147 
148  memset(free_index_lookup, -1, n_u * sizeof(float));
149 
150 
151  // find free indices
152  for (int i = 0; i < n_u; i++) {
153  if (W[i] == 0) {
154  free_index_lookup[i] = n_free;
155  free_index[n_free++] = i;
156  }
157  }
158 
159  // fill up A, A_free, b and d
160  for (int i = 0; i < n_v; i++) {
161  // If Wv is a NULL pointer, use Wv = identity
162  b[i] = Wv ? gamma_sq * Wv[i] * v[i] : gamma_sq * v[i];
163  d[i] = b[i];
164  for (int j = 0; j < n_u; j++) {
165  // If Wv is a NULL pointer, use Wv = identity
166  A[i][j] = Wv ? gamma_sq * Wv[i] * B[i][j] : gamma_sq * B[i][j];
167  d[i] -= A[i][j] * u[j];
168  }
169  }
170  for (int i = n_v; i < n_c; i++) {
171  memset(A[i], 0, n_u * sizeof(float));
172  A[i][i - n_v] = Wu ? Wu[i - n_v] : 1.0;
173  b[i] = up ? (Wu ? Wu[i] * up[i] : up[i]) : 0;
174  d[i] = b[i] - A[i][i - n_v] * u[i - n_v];
175  }
176 
177  // -------------- Start loop ------------
178  while (iter++ < imax) {
179  // clear p, copy u to u_opt
180  memset(p, 0, n_u * sizeof(float));
181  memcpy(u_opt, u, n_u * sizeof(float));
182 
183  // Construct a matrix with the free columns of A
184  if (free_chk != n_free) {
185  for (int i = 0; i < n_c; i++) {
186  for (int j = 0; j < n_free; j++) {
187  A_free[i][j] = A[i][free_index[j]];
188  }
189  }
190  free_chk = n_free;
191  }
192 
193 
194  if (n_free) {
195  // Still free variables left, calculate corresponding solution
196 
197  // use a solver to find the solution to A_free*p_free = d
198  qr_solve_wrapper(n_c, n_free, A_free_ptr, d, p_free);
199 
200  //print results current step
201 #if WLS_VERBOSE
202  print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
203 #endif
204 
205  }
206 
207  // Set the nonzero values of p and add to u_opt
208  for (int i = 0; i < n_free; i++) {
209  p[free_index[i]] = p_free[i];
210  u_opt[free_index[i]] += p_free[i];
211  }
212  // check limits
213  n_infeasible = 0;
214  for (int i = 0; i < n_u; i++) {
215  if (u_opt[i] >= (umax[i] + 1.0) || u_opt[i] <= (umin[i] - 1.0)) {
216  infeasible_index[n_infeasible++] = i;
217  }
218  }
219 
220  // Check feasibility of the solution
221  if (n_infeasible == 0) {
222  // all variables are within limits
223  memcpy(u, u_opt, n_u * sizeof(float));
224  memset(lambda, 0, n_u * sizeof(float));
225 
226  // d = d + A_free*p_free; lambda = A*d;
227  for (int i = 0; i < n_c; i++) {
228  for (int k = 0; k < n_free; k++) {
229  d[i] -= A_free[i][k] * p_free[k];
230  }
231  for (int k = 0; k < n_u; k++) {
232  lambda[k] += A[i][k] * d[i];
233  }
234  }
235  bool break_flag = true;
236 
237  // lambda = lambda x W;
238  for (int i = 0; i < n_u; i++) {
239  lambda[i] *= W[i];
240  // if any lambdas are negative, keep looking for solution
241  if (lambda[i] < -FLT_EPSILON) {
242  break_flag = false;
243  W[i] = 0;
244  // add a free index
245  if (free_index_lookup[i] < 0) {
246  free_index_lookup[i] = n_free;
247  free_index[n_free++] = i;
248  }
249  }
250  }
251  if (break_flag) {
252 
253 #if WLS_VERBOSE
254  print_final_values(1, n_u, n_v, u, B, v, umin, umax);
255 #endif
256 
257  // if solution is found, return number of iterations
258  return iter;
259  }
260  } else {
261  float alpha = INFINITY;
262  float alpha_tmp;
263  int id_alpha = 0;
264 
265  // find the lowest distance from the limit among the free variables
266  for (int i = 0; i < n_free; i++) {
267  int id = free_index[i];
268  if(fabs(p[id]) > FLT_EPSILON) {
269  alpha_tmp = (p[id] < 0) ? (umin[id] - u[id]) / p[id]
270  : (umax[id] - u[id]) / p[id];
271  } else {
272  alpha_tmp = INFINITY;
273  }
274  if (alpha_tmp < alpha) {
275  alpha = alpha_tmp;
276  id_alpha = id;
277  }
278  }
279 
280  // update input u = u + alpha*p
281  for (int i = 0; i < n_u; i++) {
282  u[i] += alpha * p[i];
283  }
284  // update d = d-alpha*A*p_free
285  for (int i = 0; i < n_c; i++) {
286  for (int k = 0; k < n_free; k++) {
287  d[i] -= A_free[i][k] * alpha * p_free[k];
288  }
289  }
290  // get rid of a free index
291  W[id_alpha] = (p[id_alpha] > 0) ? 1.0 : -1.0;
292 
293  free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
294  free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
295  free_index_lookup[id_alpha];
296  free_index_lookup[id_alpha] = -1;
297  }
298  }
299  // solution failed, return negative one to indicate failure
300  return -1;
301 }
302 
303 #if WLS_VERBOSE
304 void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, float* p_free) {
305 
306  printf("n_c = %d n_free = %d\n", n_c, n_free);
307 
308  printf("A_free =\n");
309  for(int i = 0; i < n_c; i++) {
310  for (int j = 0; j < n_free; j++) {
311  printf("%f ", A_free_ptr[i][j]);
312  }
313  printf("\n");
314  }
315 
316  printf("d = ");
317  for (int j = 0; j < n_c; j++) {
318  printf("%f ", d[j]);
319  }
320 
321  printf("\noutput = ");
322  for (int j = 0; j < n_free; j++) {
323  printf("%f ", p_free[j]);
324  }
325  printf("\n\n");
326 }
327 
328 void print_final_values(int n_u, int n_v, float* u, float** B, float* v, float* umin, float* umax) {
329  printf("n_u = %d n_v = %d\n", n_u, n_v);
330 
331  printf("B =\n");
332  for(int i = 0; i < n_v; i++) {
333  for (int j = 0; j < n_u; j++) {
334  printf("%f ", B[i][j]);
335  }
336  printf("\n");
337  }
338 
339  printf("v = ");
340  for (int j = 0; j < n_v; j++) {
341  printf("%f ", v[j]);
342  }
343 
344  printf("\nu = ");
345  for (int j = 0; j < n_u; j++) {
346  printf("%f ", u[j]);
347  }
348  printf("\n");
349 
350  printf("\numin = ");
351  for (int j = 0; j < n_u; j++) {
352  printf("%f ", umin[j]);
353  }
354  printf("\n");
355 
356  printf("\numax = ");
357  for (int j = 0; j < n_u; j++) {
358  printf("%f ", umax[j]);
359  }
360  printf("\n\n");
361 
362 }
363 #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
void qr_solve(int m, int n, float a[], float b[], float x[])
Definition: qr_solve.c:1548
int wls_alloc(float *u, float *v, float *umin, float *umax, float **B, int n_u, int n_v, 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:101
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
void qr_solve_wrapper(int m, int n, float **A, float *b, float *x)
Wrapper for qr solve.
Definition: wls_alloc.c:63
static float Wv[INDI_OUTPUTS]