fly/flightgear
1
0
Fork 0
Code Activity
flightgear/Simulator/FDM/LaRCsim/ls_trim.c
curt 29f6d05875 Initial revision
1999-04-05 21:32:32 +00:00

685 lines
18 KiB
C
Raw Blame History

/***************************************************************************
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 <string.h>
#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<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;
}
void ls_trim_get_vals()
/* Load the Output vector, and calculate control percent used */
{
int i, error;
for (i=0;i<Number_of_Outputs;i++)
{
Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
}
Cost = 0.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].Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
}
}
void ls_trim_move_controls()
/* This routine moves the current control to specified percent of authority */
{
int i;
for(i=0;i<Number_of_Controls;i++)
{
Controls[i].At_Limit = 0;
if (Controls[i].Requested_Percent <= 0.0)
{
Controls[i].Requested_Percent = 0.0;
Controls[i].At_Limit = 1;
}
if (Controls[i].Requested_Percent >= 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<Number_of_Controls;i++)
if (Controls[i].Symbol.Addr)
ls_set_sym_val( &Controls[i].Symbol, Controls[i].Curr_Val );
}
void ls_trim_calc_cost()
/* This routine calculates the current distance, or cost, from trim */
{
int i;
Cost = 0.0;
for(i=0;i<Number_of_Outputs;i++)
Cost += pow((Outputs[i].Curr_Val/Outputs[i].Trim_Criteria),2.0);
}
void ls_trim_save_baseline_outputs()
{
int i, error;
for (i=0;i<Number_of_Outputs;i++)
Baseline_Output[i] = ls_get_sym_val( &Outputs[i].Symbol, &error );
}
int ls_trim_eval_outputs()
{
int i, trimmed;
trimmed = 1;
for(i=0;i<Number_of_Outputs;i++)
if( fabs(Outputs[i].Curr_Val) > 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<Number_of_Outputs;i++)
{
delta_output = Outputs[i].Curr_Val - Baseline_Output[i];
H_Partials[i+1][Index+1] = delta_output/delta_control;
}
}
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 );
}
int ls_trim()
{
const int Max_Cycles = 100;
int Baseline;
Trimmed = 0;
if (Symbols_loaded) {
ls_trim_init(); /* Initialize Outputs & controls */
ls_trim_get_vals(); /* Limit the current control settings */
Baseline = TRUE;
ls_trim_move_controls(); /* Write out the new values of controls */
ls_trim_put_controls();
ls_loop( 0.0, -1 ); /* Cycle the simulation once with new limited
controls */
/* Main trim cycle loop follows */
while((!Trimmed) && (Trim_Cycles < Max_Cycles))
{
ls_trim_get_vals();
if (Index == -1)
{
ls_trim_calc_cost();
/*Adjust_Gain(); */
ls_trim_save_baseline_outputs();
Trimmed = ls_trim_eval_outputs();
}
else
{
ls_trim_calc_h_column();
Controls[Index].Curr_Val = Saved_Control;
Controls[Index].Percent = Saved_Control_Percent;
Controls[Index].Requested_Percent = Saved_Control_Percent;
}
Index++;
if (!Trimmed)
{
if (Index >= 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<Number_of_Controls;i++) /* Initialize fields in Controls data */
{
Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
if (error)
Controls[i].Symbol.Addr = NIL_POINTER;
Controls[i].Authority = Controls[i].Max_Val - Controls[i].Min_Val;
if (Controls[i].Authority == 0.0)
Controls[i].Authority = 1.0;
Controls[i].Requested_Percent =
(Controls[i].Curr_Val - Controls[i].Min_Val)
/Controls[i].Authority;
Controls[i].Pert_Size = Controls[i].Pert_Size/Controls[i].Authority;
}
for (i=0;i<Number_of_Outputs;i++) /* Initialize fields in Outputs data */
{
Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
Outputs[i].Weighting =
Outputs[0].Trim_Criteria/Outputs[i].Trim_Criteria;
}
}
bufptr = *lasts;
return bufptr;
}
void ls_trim_put_set( FILE *fp )
{
int i;
if (fp==0) return;
fprintf(fp, "\n");
fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
fprintf(fp, "\n");
fprintf(fp, FACILITY_NAME_STRING);
fprintf(fp, "\n");
fprintf(fp, "%04d\n", CURRENT_VERSION);
fprintf(fp, " controls: %d\n", Number_of_Controls);
fprintf(fp, "# module parameter min_val max_val pert_size\n");
for (i=0; i<Number_of_Controls; i++)
fprintf(fp, " %s\t%s\t%E\t%E\t%E\n",
Controls[i].Symbol.Mod_Name,
Controls[i].Symbol.Par_Name,
Controls[i].Min_Val,
Controls[i].Max_Val,
Controls[i].Pert_Size*Controls[i].Authority);
fprintf(fp, " outputs: %d\n", Number_of_Outputs);
fprintf(fp, "# module parameter trim_criteria\n");
for (i=0;i<Number_of_Outputs;i++)
fprintf(fp, " %s\t%s\t%E\n",
Outputs[i].Symbol.Mod_Name,
Outputs[i].Symbol.Par_Name,
Outputs[i].Trim_Criteria );
fprintf(fp, "end\n");
return;
}
View git blame Copy permalink