119void wls_alloc(
struct WLS_t* WLS_p,
float **
B,
float *u_guess,
float *W_init,
int imax) {
122 if (!imax) imax = 100;
124 int n_c = WLS_p->
nu + WLS_p->
nv;
126 float A[n_c][WLS_p->
nu];
127 float A_free[n_c][WLS_p->
nu];
131 float *A_free_ptr[n_c];
132 for(
int i = 0; i < n_c; i++)
133 A_free_ptr[i] = A_free[i];
138 int free_index[WLS_p->
nu];
139 int free_index_lookup[WLS_p->
nu];
144 float p_free[WLS_p->
nu];
146 float u_opt[WLS_p->
nu];
147 int infeasible_index[WLS_p->
nu]
UNUSED;
148 int n_infeasible = 0;
154 for (
int i = 0; i < WLS_p->
nu; i++) {
155 WLS_p->
u[i] = (WLS_p->
u_max[i] + WLS_p->
u_min[i]) * 0.5;
158 for (
int i = 0; i < WLS_p->
nu; i++) {
159 WLS_p->
u[i] = u_guess[i];
162 W_init ? memcpy(W, W_init, WLS_p->
nu *
sizeof(
float))
163 : memset(W, 0, WLS_p->
nu *
sizeof(
float));
165 memset(free_index_lookup, -1, WLS_p->
nu *
sizeof(
float));
168 for (
int i = 0; i < WLS_p->
nu; i++) {
170 free_index_lookup[i] = n_free;
171 free_index[n_free++] = i;
176 for (
int i = 0; i < WLS_p->
nv; i++) {
179 for (
int j = 0; j < WLS_p->
nu; j++) {
182 d[i] -=
A[i][j] * WLS_p->
u[j];
185 for (
int i = WLS_p->
nv; i < n_c; i++) {
186 memset(
A[i], 0, WLS_p->
nu *
sizeof(
float));
187 A[i][i - WLS_p->
nv] = WLS_p->
Wu[i - WLS_p->
nv];
188 b[i] = WLS_p->
Wu[i - WLS_p->
nv] * WLS_p->
u_pref[i - WLS_p->
nv];
189 d[i] =
b[i] -
A[i][i - WLS_p->
nv] * WLS_p->
u[i - WLS_p->
nv];
193 while (iter++ < imax) {
195 memset(
p, 0, WLS_p->
nu *
sizeof(
float));
196 memcpy(u_opt, WLS_p->
u, WLS_p->
nu *
sizeof(
float));
199 if (free_chk != n_free) {
200 for (
int i = 0; i < n_c; i++) {
201 for (
int j = 0; j < n_free; j++) {
202 A_free[i][j] =
A[i][free_index[j]];
220 print_in_and_outputs(n_c, n_free, A_free_ptr, d, p_free);
224 for (
int i = 0; i < n_free; i++) {
225 p[free_index[i]] = p_free[i];
226 u_opt[free_index[i]] += p_free[i];
229 if ((u_opt[free_index[i]] > WLS_p->
u_max[free_index[i]] || u_opt[free_index[i]] < WLS_p->
u_min[free_index[i]])) {
230 infeasible_index[n_infeasible++] = free_index[i];
236 if (n_infeasible == 0) {
238 memcpy(WLS_p->
u, u_opt, WLS_p->
nu *
sizeof(
float));
239 memset(
lambda, 0, WLS_p->
nu *
sizeof(
float));
242 for (
int i = 0; i < n_c; i++) {
243 for (
int k = 0; k < n_free; k++) {
244 d[i] -= A_free[i][k] * p_free[k];
246 for (
int k = 0; k < WLS_p->
nu; k++) {
250 bool break_flag =
true;
253 for (
int i = 0; i < WLS_p->
nu; i++) {
256 if (
lambda[i] < -FLT_EPSILON) {
260 if (free_index_lookup[i] < 0) {
261 free_index_lookup[i] = n_free;
262 free_index[n_free++] = i;
268 print_final_values(WLS_p,
B);
278 int id_alpha = free_index[0];
281 for (
int i = 0; i < n_free; i++) {
282 int id = free_index[i];
284 alpha_tmp = (
p[id] < 0) ? (WLS_p->
u_min[
id] - WLS_p->
u[
id]) /
p[id]
285 : (WLS_p->
u_max[id] - WLS_p->
u[id]) /
p[
id];
287 if (isnan(alpha_tmp) || alpha_tmp < 0.f) {
290 if (alpha_tmp <
alpha) {
297 for (
int i = 0; i < WLS_p->
nu; i++) {
299 Bound(WLS_p->
u[i], WLS_p->
u_min[i], WLS_p->
u_max[i]);
302 for (
int i = 0; i < n_c; i++) {
303 for (
int k = 0; k < n_free; k++) {
304 d[i] -= A_free[i][k] *
alpha * p_free[k];
308 W[id_alpha] = (
p[id_alpha] > 0) ? 1.0 : -1.0;
310 free_index[free_index_lookup[id_alpha]] = free_index[--n_free];
311 free_index_lookup[free_index[free_index_lookup[id_alpha]]] =
312 free_index_lookup[id_alpha];
313 free_index_lookup[id_alpha] = -1;
void wls_alloc(struct WLS_t *WLS_p, float **B, float *u_guess, float *W_init, int imax)
active set algorithm for control allocation