49 #define WLS_VERBOSE FALSE
54 static void print_final_values(
int n_u,
int n_v,
float* u,
float**
B,
float* v,
float* umin,
float* umax);
55 static void print_in_and_outputs(
int n_c,
int n_free,
float** A_free_ptr,
float* d,
float* p_free);
59 #define WLS_N_C ((WLS_N_U)+(WLS_N_V))
74 for (
int j = 0; j < n; j++) {
75 for (
int i = 0; i <
m; i++) {
108 int wls_alloc(
float* u,
float* v,
float* umin,
float* umax,
float**
B,
109 float* u_guess,
float* W_init,
float*
Wv,
float*
Wu,
float*
up,
110 float gamma_sq,
int imax,
int n_u,
int n_v) {
112 if(!gamma_sq) gamma_sq = 100000;
113 if(!imax) imax = 100;
123 for(
int i = 0; i < n_c; i++)
124 A_free_ptr[i] = A_free[i];
130 int free_index_lookup[
WLS_N_U];
139 int n_infeasible = 0;
145 for (
int i = 0; i < n_u; i++) {
146 u[i] = (umax[i] + umin[i]) * 0.5;
149 for (
int i = 0; i < n_u; i++) {
153 W_init ? memcpy(W, W_init, n_u *
sizeof(
float))
154 : memset(W, 0, n_u *
sizeof(
float));
156 memset(free_index_lookup, -1, n_u *
sizeof(
float));
159 for (
int i = 0; i < n_u; i++) {
161 free_index_lookup[i] = n_free;
162 free_index[n_free++] = i;
167 for (
int i = 0; i < n_v; i++) {
169 b[i] =
Wv ? gamma_sq *
Wv[i] * v[i] : gamma_sq * v[i];
171 for (
int j = 0; j < n_u; j++) {
173 A[i][j] =
Wv ? gamma_sq *
Wv[i] *
B[i][j] : gamma_sq *
B[i][j];
174 d[i] -=
A[i][j] * u[j];
177 for (
int i = n_v; i < n_c; i++) {
178 memset(
A[i], 0, n_u *
sizeof(
float));
179 A[i][i - n_v] =
Wu ?
Wu[i - n_v] : 1.0;
180 b[i] =
up ? (
Wu ?
Wu[i-n_v] *
up[i-n_v] :
up[i-n_v]) : 0;
181 d[i] =
b[i] -
A[i][i - n_v] * u[i - n_v];
185 while (iter++ < imax) {
187 memset(
p, 0, n_u *
sizeof(
float));
188 memcpy(u_opt, u, n_u *
sizeof(
float));
191 if (free_chk != n_free) {
192 for (
int i = 0; i < n_c; i++) {
193 for (
int j = 0; j < n_free; j++) {
194 A_free[i][j] =
A[i][free_index[j]];
212 print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
216 for (
int i = 0; i < n_free; i++) {
217 p[free_index[i]] = p_free[i];
218 u_opt[free_index[i]] += p_free[i];
221 if ( (u_opt[free_index[i]] > umax[free_index[i]] || u_opt[free_index[i]] < umin[free_index[i]])) {
222 infeasible_index[n_infeasible++] = free_index[i];
228 if (n_infeasible == 0) {
230 memcpy(u, u_opt, n_u *
sizeof(
float));
231 memset(
lambda, 0, n_u *
sizeof(
float));
234 for (
int i = 0; i < n_c; i++) {
235 for (
int k = 0; k < n_free; k++) {
236 d[i] -= A_free[i][k] * p_free[k];
238 for (
int k = 0; k < n_u; k++) {
242 bool break_flag =
true;
245 for (
int i = 0; i < n_u; i++) {
248 if (
lambda[i] < -FLT_EPSILON) {
252 if (free_index_lookup[i] < 0) {
253 free_index_lookup[i] = n_free;
254 free_index[n_free++] = i;
261 print_final_values(n_u, n_v, u,
B, v, umin, umax);
271 int id_alpha = free_index[0];
274 for (
int i = 0; i < n_free; i++) {
275 int id = free_index[i];
277 alpha_tmp = (
p[id] < 0) ? (umin[
id] - u[
id]) /
p[id]
278 : (umax[id] - u[id]) /
p[
id];
280 if (isnan(alpha_tmp) || alpha_tmp < 0.f) {
283 if (alpha_tmp <
alpha) {
290 for (
int i = 0; i < n_u; i++) {
292 Bound(u[i],umin[i],umax[i]);
295 for (
int i = 0; i < n_c; i++) {
296 for (
int k = 0; k < n_free; k++) {
297 d[i] -= A_free[i][k] *
alpha * p_free[k];
301 W[id_alpha] = (
p[id_alpha] > 0) ? 1.0 : -1.0;
303 free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
304 free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
305 free_index_lookup[id_alpha];
306 free_index_lookup[id_alpha] = -1;
313 static void print_in_and_outputs(
int n_c,
int n_free,
float** A_free_ptr,
float* d,
float* p_free) {
315 printf(
"n_c = %d n_free = %d\n", n_c, n_free);
317 printf(
"A_free =\n");
318 for(
int i = 0; i < n_c; i++) {
319 for (
int j = 0; j < n_free; j++) {
320 printf(
"%f ", A_free_ptr[i][j]);
326 for (
int j = 0; j < n_c; j++) {
330 printf(
"\noutput = ");
331 for (
int j = 0; j < n_free; j++) {
332 printf(
"%f ", p_free[j]);
337 static void print_final_values(
int n_u,
int n_v,
float* u,
float**
B,
float* v,
float* umin,
float* umax) {
338 printf(
"n_u = %d n_v = %d\n", n_u, n_v);
341 for(
int i = 0; i < n_v; i++) {
342 for (
int j = 0; j < n_u; j++) {
343 printf(
"%f ",
B[i][j]);
349 for (
int j = 0; j < n_v; j++) {
354 for (
int j = 0; j < n_u; j++) {
360 for (
int j = 0; j < n_u; j++) {
361 printf(
"%f ", umin[j]);
366 for (
int j = 0; j < n_u; j++) {
367 printf(
"%f ", umax[j]);
static float Wv[ANDI_OUTPUTS]
static float Wu[ANDI_NUM_ACT_TOT]
void qr_solve(int m, int n, float a[], float b[], float x[])
static void qr_solve_wrapper(int m, int n, float **A, float *b, float *x)
Wrapper for qr solve.
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, int n_u, int n_v)
active set algorithm for control allocation
#define WLS_N_U
active set algorithm for control allocation