int ls_trim_init()
/* Initialize partials matrix */
{
int i, error;
// int result;
Index = -1;
Trim_Cycles = 0;
Gain = 1;
First = 1;
Previous_Cost = 0.0;
Trimmed = 0;
for (i=0;i<Number_of_Controls;i++)
{
Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
if (error) Controls[i].Symbol.Addr = NIL_POINTER;
Controls[i].Requested_Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
}
H_Partials = nr_matrix( 1, Number_of_Controls, 1, Number_of_Controls );
if (H_Partials == 0) return -1;
return 0;
}
int svdsolve(eusfloat_t **a, int m, int n, eusfloat_t *b, eusfloat_t *x)
{
int j;
eusfloat_t **v, *w, wmax, wmin;
v = nr_matrix(1,n,1,n);
w = nr_vector(1,n);
if ( svdcmp(a,m,n,w,v) < 0 ) {
free_nr_vector(w,1,n);
free_nr_matrix(v,1,n,1,n);
return -1;
}
wmax = 0.0;
for (j=1; j<=n; j++) if (w[j] > wmax) wmax = w[j];
wmin = wmax*1.0e-6;
for (j=1; j<=n; j++) if (w[j] < wmin) w[j] = 0.0;
svbksb(a,w,v,m,n,b,x);
free_nr_vector(w,1,n);
free_nr_matrix(v,1,n,1,n);
return 1;
}
void ls_trim_do_step()
{
int i, j, l, singular;
double **h_trans_w_h;
double delta_req_mag, scaling;
double delta_U_requested[ MAX_NUMBER_OF_CONTROLS ];
double temp[ MAX_NUMBER_OF_CONTROLS ];
/* Identify ineffective controls (whose partials are all near zero) */
for (j=0;j<Number_of_Controls;j++)
{
Controls[j].Ineffective = 1;
for(i=0;i<Number_of_Outputs;i++)
if (fabs(H_Partials[i+1][j+1]) > EPS) Controls[j].Ineffective = 0;
}
/* Identify uncontrollable outputs */
for (j=0;j<Number_of_Outputs;j++)
{
Outputs[j].Uncontrollable = 1;
for(i=0;i<Number_of_Controls;i++)
if (fabs(H_Partials[j+1][i+1]) > EPS) Outputs[j].Uncontrollable = 0;
}
/* Calculate well-conditioned partials matrix [ H' W H ] */
h_trans_w_h = nr_matrix(1, Number_of_Controls, 1, Number_of_Controls);
if (h_trans_w_h == 0)
{
fprintf(stderr, "Memory error in ls_trim().\n");
exit(1);
}
for (l=1;l<=Number_of_Controls;l++)
for (j=1;j<=Number_of_Controls;j++)
{
h_trans_w_h[l][j] = 0.0;
for (i=1;i<=Number_of_Outputs;i++)
h_trans_w_h[l][j] +=
H_Partials[i][l]*H_Partials[i][j]*Outputs[i-1].Weighting;
}
/* Invert the partials array [ inv( H' W H ) ]; note: h_trans_w_h is replaced
with its inverse during this function call */
singular = nr_gaussj( h_trans_w_h, Number_of_Controls, 0, 0 );
if (singular) /* Can't invert successfully */
{
nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
1, Number_of_Controls );
fprintf(stderr, "Singular matrix in ls_trim().\n");
return;
}
/* Form right hand side of equality: temp = [ H' W (-Y) ] */
for(i=0;i<Number_of_Controls;i++)
{
temp[i] = 0.0;
for(j=0;j<Number_of_Outputs;j++)
temp[i] -= H_Partials[j+1][i+1]*Baseline_Output[j]*Outputs[j].Weighting;
}
/* Solve for dU = [inv( H' W H )][ H' W (-Y)] */
for(i=0;i<Number_of_Controls;i++)
{
delta_U_requested[i] = 0.0;
for(j=0;j<Number_of_Controls;j++)
delta_U_requested[i] += h_trans_w_h[i+1][j+1]*temp[j];
}
/* limit step magnitude to certain size, but not direction */
delta_req_mag = 0.0;
for(i=0;i<Number_of_Controls;i++)
delta_req_mag += delta_U_requested[i]*delta_U_requested[i];
delta_req_mag = sqrt(delta_req_mag);
scaling = STEP_LIMIT/delta_req_mag;
if (scaling < 1.0)
for(i=0;i<Number_of_Controls;i++)
delta_U_requested[i] *= scaling;
/* Convert deltas to percent of authority */
for(i=0;i<Number_of_Controls;i++)
Controls[i].Requested_Percent = Controls[i].Percent + delta_U_requested[i];
/* free up temporary matrix */
nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
1, Number_of_Controls );
}
