/*************************************************************************** TITLE: ls_trim.c ---------------------------------------------------------------------------- FUNCTION: Trims the simulated aircraft by using certain controls to null out a similar number of outputs. This routine used modified Newton-Raphson method to find the vector of control corrections, delta_U, to drive a similar-sized vector of output errors, Y, to near-zero. Nearness to zero is to within a tolerance specified by the Criteria vector. An optional Weight vector can be used to improve the numerical properties of the Jacobian matrix (called H_Partials). Using a single-sided difference method, each control is independently perturbed and the change in each output of interest is calculated, forming a Jacobian matrix H (variable name is H_Partials): dY = H dU The columns of H correspond to the control effect; the rows of H correspond to the outputs affected. We wish to find dU such that for U = U0 + dU, Y = Y0 + dY = 0 or dY = -Y0 One solution is dU = inv(H)*(-Y0); however, inverting H directly is not numerically sound, since it may be singular (especially if one of the controls is on a limit, or the problem is poorly posed). An alternative is to either weight the elements of dU to make them more normalized; another is to multiply both sides by the transpose of H and invert the resulting [H' H]. This routine does both: -Y0 = H dU W (-Y0) = W H dU premultiply by W H' W (-Y0) = H' W H dU premultiply by H' dU = [inv(H' W H)][ H' W (-Y0)] Solve for dU As a further refinement, dU is limited to a smallish magnitude so that Y approaches 0 in small steps (to avoid an overshoot if the problem is inherently non-linear). Lastly, this routine can be easily fooled by "local minima", or depressions in the solution space that don't lead to a Y = 0 solution. The only advice we can offer is to "go somewheres else and try again"; often approaching a trim solution from a different (non-trimmed) starting point will prove beneficial. ---------------------------------------------------------------------------- MODULE STATUS: developmental ---------------------------------------------------------------------------- GENEALOGY: Created from old CASTLE SHELL$TRIM.PAS on 6 FEB 95, which was based upon an Ames CASPRE routine called BQUIET. ---------------------------------------------------------------------------- DESIGNED BY: E. B. Jackson CODED BY: same MAINTAINED BY: same ---------------------------------------------------------------------------- MODIFICATION HISTORY: DATE PURPOSE BY 950307 Modified to make use of ls_get_sym_val and ls_put_sym_val routines. EBJ 950329 Fixed bug in making use of more than 3 controls; removed call by ls_trim_get_set() to ls_trim_init(). EBJ CURRENT RCS HEADER: $Header$ $Log$ Revision 1.1 1999/04/05 21:32:45 curt Initial revision * Revision 1.9 1995/03/29 16:09:56 bjax * Fixed bug in having more than three trim controls; removed unnecessary * call to ls_trim_init in ls_trim_get_set. EBJ * * Revision 1.8 1995/03/16 12:28:40 bjax * Fixed problem where ls_trim() returns non-zero if * symbols are not loaded - implies vehicle trimmed when * actually no trim attempt is made. This results in storing of non- * trimmed initial conditions in sims without defined trim controls. * * Revision 1.7 1995/03/15 12:17:12 bjax * Added flag marker line to ls_trim_put_set() routine output. * * Revision 1.6 1995/03/08 11:49:07 bjax * Minor improvements to ls_trim_get_set; deleted weighting parameter * for output definition; added comment lines to settings file output. * * Revision 1.5 1995/03/07 22:38:04 bjax * Removed ls_generic.h; this version relies entirely on symbol table routines to * set and get variable values. Added additional fields to Control record structure; * created Output record with appropriate fields. Added ls_trim_put_set() and * ls_trim_get_set() routines. Heavily modified initialization routine; most of this * logic now resides in ls_trim_get_set(). Renamed all routines so that they being * with "ls_trim_" to avoid conflicts. * EBJ * * Revision 1.4 1995/03/07 13:04:16 bjax * Configured to use ls_get_sym_val() and ls_set_sym_val(). * * Revision 1.3 1995/03/03 01:59:53 bjax * Moved definition of SYMBOL_NAME and SYMBOL_TYPE to ls_sym.h * and removed from this module. EBJ * * Revision 1.2 1995/02/27 19:53:41 bjax * Moved symbol routines to ls_sym.c to declutter this file. EBJ * * Revision 1.1 1995/02/27 18:14:10 bjax * Initial revision * ---------------------------------------------------------------------------- REFERENCES: ---------------------------------------------------------------------------- CALLED BY: ---------------------------------------------------------------------------- CALLS TO: ---------------------------------------------------------------------------- INPUTS: ---------------------------------------------------------------------------- OUTPUTS: --------------------------------------------------------------------------*/ static char rcsid[] = "$Id$"; #ifdef __SUNPRO_CC # define _REENTRANT #endif #include #include "ls_constants.h" #include "ls_types.h" #include "ls_sym.h" #include "ls_matrix.h" #include "ls_interface.h" #ifndef TRUE #define FALSE 0 #define TRUE !FALSE #endif #define MAX_NUMBER_OF_CONTROLS 10 #define MAX_NUMBER_OF_OUTPUTS 10 #define STEP_LIMIT 0.01 #define NIL_POINTER 0L #define FACILITY_NAME_STRING "trim" #define CURRENT_VERSION 10 typedef struct { symbol_rec Symbol; double Min_Val, Max_Val, Curr_Val, Authority; double Percent, Requested_Percent, Pert_Size; int Ineffective, At_Limit; } control_rec; typedef struct { symbol_rec Symbol; double Curr_Val, Weighting, Trim_Criteria; int Uncontrollable; } output_rec; static int Symbols_loaded = 0; static int Index; static int Trim_Cycles; static int First; static int Trimmed; static double Gain; static int Number_of_Controls; static int Number_of_Outputs; static control_rec Controls[ MAX_NUMBER_OF_CONTROLS ]; static output_rec Outputs[ MAX_NUMBER_OF_OUTPUTS ]; static double **H_Partials; static double Baseline_Output[ MAX_NUMBER_OF_OUTPUTS ]; static double Saved_Control, Saved_Control_Percent; static double Cost, Previous_Cost; 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= 1.0) { Controls[i].Requested_Percent = 1.0; Controls[i].At_Limit = 1; } Controls[i].Curr_Val = Controls[i].Min_Val + (Controls[i].Max_Val - Controls[i].Min_Val) * Controls[i].Requested_Percent; } } void ls_trim_put_controls() /* Put current control requests out to controls themselves */ { int i; for (i=0;i Outputs[i].Trim_Criteria) trimmed = 0; return trimmed; } void ls_trim_calc_h_column() { int i; double delta_control, delta_output; delta_control = (Controls[Index].Curr_Val - Saved_Control)/Controls[Index].Authority; for(i=0;i EPS) Controls[j].Ineffective = 0; } /* Identify uncontrollable outputs */ for (j=0;j 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) { Baseline = TRUE; Index = -1; ls_trim_do_step(); } else { /* Save the current value & pert next control */ Baseline = FALSE; Saved_Control = Controls[Index].Curr_Val; Saved_Control_Percent = Controls[Index].Percent; if (Controls[Index].Percent < (1.0 - Controls[Index].Pert_Size) ) { Controls[Index].Requested_Percent = Controls[Index].Percent + Controls[Index].Pert_Size ; } else { Controls[Index].Requested_Percent = Controls[Index].Percent - Controls[Index].Pert_Size; } } ls_trim_move_controls(); ls_trim_put_controls(); ls_loop( 0.0, -1 ); Trim_Cycles++; } } nr_free_matrix( H_Partials, 1, Number_of_Controls, 1, Number_of_Controls ); } if (!Trimmed) fprintf(stderr, "Trim unsuccessful.\n"); return Trimmed; } char *ls_trim_get_set(char *buffer, char *eob) /* This routine parses the settings file for "trim" entries. */ { static char *fac_name = FACILITY_NAME_STRING; char *bufptr, **lasts, *nullptr, null = '\0'; char line[256]; int n, ver, i, error, abrt; enum {controls_header, controls, outputs_header, outputs, done} looking_for; nullptr = &null; lasts = &nullptr; abrt = 0; looking_for = controls_header; n = sscanf(buffer, "%s", line); if (n == 0) return 0L; if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L; bufptr = strtok_r( buffer+strlen(fac_name)+1, "\n", lasts); if (bufptr == 0) return 0L; sscanf( bufptr, "%d", &ver ); if (ver != CURRENT_VERSION) return 0L; while( !abrt && (eob > bufptr)) { bufptr = strtok_r( 0L, "\n", lasts ); if (bufptr == 0) return 0L; if (strncasecmp( bufptr, "end", 3) == 0) break; sscanf( bufptr, "%s", line ); if (line[0] != '#') /* ignore comments */ { switch (looking_for) { case controls_header: { if (strncasecmp( line, "controls", 8) == 0) { n = sscanf( bufptr, "%s%d", line, &Number_of_Controls ); if (n != 2) abrt = 1; looking_for = controls; i = 0; } break; } case controls: { n = sscanf( bufptr, "%s%s%le%le%le", Controls[i].Symbol.Mod_Name, Controls[i].Symbol.Par_Name, &Controls[i].Min_Val, &Controls[i].Max_Val, &Controls[i].Pert_Size); if (n != 5) abrt = 1; Controls[i].Symbol.Addr = NIL_POINTER; i++; if (i >= Number_of_Controls) looking_for = outputs_header; break; } case outputs_header: { if (strncasecmp( line, "outputs", 7) == 0) { n = sscanf( bufptr, "%s%d", line, &Number_of_Outputs ); if (n != 2) abrt = 1; looking_for = outputs; i = 0; } break; } case outputs: { n = sscanf( bufptr, "%s%s%le", Outputs[i].Symbol.Mod_Name, Outputs[i].Symbol.Par_Name, &Outputs[i].Trim_Criteria ); if (n != 3) abrt = 1; Outputs[i].Symbol.Addr = NIL_POINTER; i++; if (i >= Number_of_Outputs) looking_for = done; } case done: { break; } } } } if ((!abrt) && (Number_of_Controls > 0) && (Number_of_Outputs == Number_of_Controls)) { Symbols_loaded = 1; for(i=0;i