1 /***************************************************************************
5 ----------------------------------------------------------------------------
7 FUNCTION: Trims the simulated aircraft by using certain
8 controls to null out a similar number of outputs.
10 This routine used modified Newton-Raphson method to find the vector
11 of control corrections, delta_U, to drive a similar-sized vector of
12 output errors, Y, to near-zero. Nearness to zero is to within a
13 tolerance specified by the Criteria vector. An optional Weight
14 vector can be used to improve the numerical properties of the
15 Jacobian matrix (called H_Partials).
17 Using a single-sided difference method, each control is
18 independently perturbed and the change in each output of
19 interest is calculated, forming a Jacobian matrix H (variable
25 The columns of H correspond to the control effect; the rows of
26 H correspond to the outputs affected.
28 We wish to find dU such that for U = U0 + dU,
33 One solution is dU = inv(H)*(-Y0); however, inverting H
34 directly is not numerically sound, since it may be singular
35 (especially if one of the controls is on a limit, or the
36 problem is poorly posed). An alternative is to either weight
37 the elements of dU to make them more normalized; another is to
38 multiply both sides by the transpose of H and invert the
39 resulting [H' H]. This routine does both:
42 W (-Y0) = W H dU premultiply by W
43 H' W (-Y0) = H' W H dU premultiply by H'
45 dU = [inv(H' W H)][ H' W (-Y0)] Solve for dU
47 As a further refinement, dU is limited to a smallish magnitude
48 so that Y approaches 0 in small steps (to avoid an overshoot
49 if the problem is inherently non-linear).
51 Lastly, this routine can be easily fooled by "local minima",
52 or depressions in the solution space that don't lead to a Y =
53 0 solution. The only advice we can offer is to "go somewheres
54 else and try again"; often approaching a trim solution from a
55 different (non-trimmed) starting point will prove beneficial.
58 ----------------------------------------------------------------------------
60 MODULE STATUS: developmental
62 ----------------------------------------------------------------------------
64 GENEALOGY: Created from old CASTLE SHELL$TRIM.PAS
65 on 6 FEB 95, which was based upon an Ames
66 CASPRE routine called BQUIET.
68 ----------------------------------------------------------------------------
70 DESIGNED BY: E. B. Jackson
76 ----------------------------------------------------------------------------
82 950307 Modified to make use of ls_get_sym_val and ls_put_sym_val
84 950329 Fixed bug in making use of more than 3 controls;
85 removed call by ls_trim_get_set() to ls_trim_init(). EBJ
91 Revision 1.2 2001/07/30 20:53:54 curt
92 Various MSVC tweaks and warning fixes.
94 Revision 1.1.1.1 1999/06/17 18:07:33 curt
97 Revision 1.1.1.1 1999/04/05 21:32:45 curt
98 Start of 0.6.x branch.
100 * Revision 1.9 1995/03/29 16:09:56 bjax
101 * Fixed bug in having more than three trim controls; removed unnecessary
102 * call to ls_trim_init in ls_trim_get_set. EBJ
104 * Revision 1.8 1995/03/16 12:28:40 bjax
105 * Fixed problem where ls_trim() returns non-zero if
106 * symbols are not loaded - implies vehicle trimmed when
107 * actually no trim attempt is made. This results in storing of non-
108 * trimmed initial conditions in sims without defined trim controls.
110 * Revision 1.7 1995/03/15 12:17:12 bjax
111 * Added flag marker line to ls_trim_put_set() routine output.
113 * Revision 1.6 1995/03/08 11:49:07 bjax
114 * Minor improvements to ls_trim_get_set; deleted weighting parameter
115 * for output definition; added comment lines to settings file output.
117 * Revision 1.5 1995/03/07 22:38:04 bjax
118 * Removed ls_generic.h; this version relies entirely on symbol table routines to
119 * set and get variable values. Added additional fields to Control record structure;
120 * created Output record with appropriate fields. Added ls_trim_put_set() and
121 * ls_trim_get_set() routines. Heavily modified initialization routine; most of this
122 * logic now resides in ls_trim_get_set(). Renamed all routines so that they being
123 * with "ls_trim_" to avoid conflicts.
126 * Revision 1.4 1995/03/07 13:04:16 bjax
127 * Configured to use ls_get_sym_val() and ls_set_sym_val().
129 * Revision 1.3 1995/03/03 01:59:53 bjax
130 * Moved definition of SYMBOL_NAME and SYMBOL_TYPE to ls_sym.h
131 * and removed from this module. EBJ
133 * Revision 1.2 1995/02/27 19:53:41 bjax
134 * Moved symbol routines to ls_sym.c to declutter this file. EBJ
136 * Revision 1.1 1995/02/27 18:14:10 bjax
140 ----------------------------------------------------------------------------
144 ----------------------------------------------------------------------------
148 ----------------------------------------------------------------------------
152 ----------------------------------------------------------------------------
156 ----------------------------------------------------------------------------
160 --------------------------------------------------------------------------*/
162 static char rcsid[] = "$Id$";
169 #include "ls_constants.h"
170 #include "ls_types.h"
172 #include "ls_matrix.h"
173 #include "ls_interface.h"
181 #define MAX_NUMBER_OF_CONTROLS 10
182 #define MAX_NUMBER_OF_OUTPUTS 10
183 #define STEP_LIMIT 0.01
184 #define NIL_POINTER 0L
186 #define FACILITY_NAME_STRING "trim"
187 #define CURRENT_VERSION 10
193 double Min_Val, Max_Val, Curr_Val, Authority;
194 double Percent, Requested_Percent, Pert_Size;
195 int Ineffective, At_Limit;
201 double Curr_Val, Weighting, Trim_Criteria;
206 static int Symbols_loaded = 0;
208 static int Trim_Cycles;
213 static int Number_of_Controls;
214 static int Number_of_Outputs;
215 static control_rec Controls[ MAX_NUMBER_OF_CONTROLS ];
216 static output_rec Outputs[ MAX_NUMBER_OF_OUTPUTS ];
218 static double **H_Partials;
220 static double Baseline_Output[ MAX_NUMBER_OF_OUTPUTS ];
221 static double Saved_Control, Saved_Control_Percent;
223 static double Cost, Previous_Cost;
229 /* Initialize partials matrix */
241 for (i=0;i<Number_of_Controls;i++)
243 Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
244 if (error) Controls[i].Symbol.Addr = NIL_POINTER;
245 Controls[i].Requested_Percent =
246 (Controls[i].Curr_Val - Controls[i].Min_Val)
247 /Controls[i].Authority;
250 H_Partials = nr_matrix( 1, Number_of_Controls, 1, Number_of_Controls );
251 if (H_Partials == 0) return -1;
256 void ls_trim_get_vals()
257 /* Load the Output vector, and calculate control percent used */
261 for (i=0;i<Number_of_Outputs;i++)
263 Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
264 if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
268 for (i=0;i<Number_of_Controls;i++)
270 Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
271 if (error) Controls[i].Symbol.Addr = NIL_POINTER;
272 Controls[i].Percent =
273 (Controls[i].Curr_Val - Controls[i].Min_Val)
274 /Controls[i].Authority;
280 void ls_trim_move_controls()
281 /* This routine moves the current control to specified percent of authority */
285 for(i=0;i<Number_of_Controls;i++)
287 Controls[i].At_Limit = 0;
288 if (Controls[i].Requested_Percent <= 0.0)
290 Controls[i].Requested_Percent = 0.0;
291 Controls[i].At_Limit = 1;
293 if (Controls[i].Requested_Percent >= 1.0)
295 Controls[i].Requested_Percent = 1.0;
296 Controls[i].At_Limit = 1;
298 Controls[i].Curr_Val = Controls[i].Min_Val +
299 (Controls[i].Max_Val - Controls[i].Min_Val) *
300 Controls[i].Requested_Percent;
304 void ls_trim_put_controls()
305 /* Put current control requests out to controls themselves */
309 for (i=0;i<Number_of_Controls;i++)
310 if (Controls[i].Symbol.Addr)
311 ls_set_sym_val( &Controls[i].Symbol, Controls[i].Curr_Val );
314 void ls_trim_calc_cost()
315 /* This routine calculates the current distance, or cost, from trim */
320 for(i=0;i<Number_of_Outputs;i++)
321 Cost += pow((Outputs[i].Curr_Val/Outputs[i].Trim_Criteria),2.0);
324 void ls_trim_save_baseline_outputs()
328 for (i=0;i<Number_of_Outputs;i++)
329 Baseline_Output[i] = ls_get_sym_val( &Outputs[i].Symbol, &error );
332 int ls_trim_eval_outputs()
337 for(i=0;i<Number_of_Outputs;i++)
338 if( fabs(Outputs[i].Curr_Val) > Outputs[i].Trim_Criteria) trimmed = 0;
342 void ls_trim_calc_h_column()
345 double delta_control, delta_output;
347 delta_control = (Controls[Index].Curr_Val - Saved_Control)/Controls[Index].Authority;
349 for(i=0;i<Number_of_Outputs;i++)
351 delta_output = Outputs[i].Curr_Val - Baseline_Output[i];
352 H_Partials[i+1][Index+1] = delta_output/delta_control;
356 void ls_trim_do_step()
358 int i, j, l, singular;
359 double **h_trans_w_h;
360 double delta_req_mag, scaling;
361 double delta_U_requested[ MAX_NUMBER_OF_CONTROLS ];
362 double temp[ MAX_NUMBER_OF_CONTROLS ];
364 /* Identify ineffective controls (whose partials are all near zero) */
366 for (j=0;j<Number_of_Controls;j++)
368 Controls[j].Ineffective = 1;
369 for(i=0;i<Number_of_Outputs;i++)
370 if (fabs(H_Partials[i+1][j+1]) > EPS) Controls[j].Ineffective = 0;
373 /* Identify uncontrollable outputs */
375 for (j=0;j<Number_of_Outputs;j++)
377 Outputs[j].Uncontrollable = 1;
378 for(i=0;i<Number_of_Controls;i++)
379 if (fabs(H_Partials[j+1][i+1]) > EPS) Outputs[j].Uncontrollable = 0;
382 /* Calculate well-conditioned partials matrix [ H' W H ] */
384 h_trans_w_h = nr_matrix(1, Number_of_Controls, 1, Number_of_Controls);
385 if (h_trans_w_h == 0)
387 fprintf(stderr, "Memory error in ls_trim().\n");
390 for (l=1;l<=Number_of_Controls;l++)
391 for (j=1;j<=Number_of_Controls;j++)
393 h_trans_w_h[l][j] = 0.0;
394 for (i=1;i<=Number_of_Outputs;i++)
396 H_Partials[i][l]*H_Partials[i][j]*Outputs[i-1].Weighting;
399 /* Invert the partials array [ inv( H' W H ) ]; note: h_trans_w_h is replaced
400 with its inverse during this function call */
402 singular = nr_gaussj( h_trans_w_h, Number_of_Controls, 0, 0 );
404 if (singular) /* Can't invert successfully */
406 nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
407 1, Number_of_Controls );
408 fprintf(stderr, "Singular matrix in ls_trim().\n");
412 /* Form right hand side of equality: temp = [ H' W (-Y) ] */
414 for(i=0;i<Number_of_Controls;i++)
417 for(j=0;j<Number_of_Outputs;j++)
418 temp[i] -= H_Partials[j+1][i+1]*Baseline_Output[j]*Outputs[j].Weighting;
421 /* Solve for dU = [inv( H' W H )][ H' W (-Y)] */
422 for(i=0;i<Number_of_Controls;i++)
424 delta_U_requested[i] = 0.0;
425 for(j=0;j<Number_of_Controls;j++)
426 delta_U_requested[i] += h_trans_w_h[i+1][j+1]*temp[j];
429 /* limit step magnitude to certain size, but not direction */
432 for(i=0;i<Number_of_Controls;i++)
433 delta_req_mag += delta_U_requested[i]*delta_U_requested[i];
434 delta_req_mag = sqrt(delta_req_mag);
435 scaling = STEP_LIMIT/delta_req_mag;
437 for(i=0;i<Number_of_Controls;i++)
438 delta_U_requested[i] *= scaling;
440 /* Convert deltas to percent of authority */
442 for(i=0;i<Number_of_Controls;i++)
443 Controls[i].Requested_Percent = Controls[i].Percent + delta_U_requested[i];
445 /* free up temporary matrix */
447 nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
448 1, Number_of_Controls );
456 const int Max_Cycles = 100;
460 if (Symbols_loaded) {
462 ls_trim_init(); /* Initialize Outputs & controls */
463 ls_trim_get_vals(); /* Limit the current control settings */
465 ls_trim_move_controls(); /* Write out the new values of controls */
466 ls_trim_put_controls();
467 ls_loop( 0.0, -1 ); /* Cycle the simulation once with new limited
470 /* Main trim cycle loop follows */
472 while((!Trimmed) && (Trim_Cycles < Max_Cycles))
479 ls_trim_save_baseline_outputs();
480 Trimmed = ls_trim_eval_outputs();
484 ls_trim_calc_h_column();
485 Controls[Index].Curr_Val = Saved_Control;
486 Controls[Index].Percent = Saved_Control_Percent;
487 Controls[Index].Requested_Percent = Saved_Control_Percent;
492 if (Index >= Number_of_Controls)
499 { /* Save the current value & pert next control */
501 Saved_Control = Controls[Index].Curr_Val;
502 Saved_Control_Percent = Controls[Index].Percent;
504 if (Controls[Index].Percent <
505 (1.0 - Controls[Index].Pert_Size) )
507 Controls[Index].Requested_Percent =
508 Controls[Index].Percent +
509 Controls[Index].Pert_Size ;
513 Controls[Index].Requested_Percent =
514 Controls[Index].Percent -
515 Controls[Index].Pert_Size;
518 ls_trim_move_controls();
519 ls_trim_put_controls();
525 nr_free_matrix( H_Partials, 1, Number_of_Controls, 1, Number_of_Controls );
528 if (!Trimmed) fprintf(stderr, "Trim unsuccessful.\n");
534 char *ls_trim_get_set(char *buffer, char *eob)
535 /* This routine parses the settings file for "trim" entries. */
538 static char *fac_name = FACILITY_NAME_STRING;
539 char *bufptr, **lasts, *nullptr, null = '\0';
541 int n, ver, i, error, abrt;
542 enum {controls_header, controls, outputs_header, outputs, done} looking_for;
547 looking_for = controls_header;
550 n = sscanf(buffer, "%s", line);
551 if (n == 0) return 0L;
552 if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
554 bufptr = strtok_r( buffer+strlen(fac_name)+1, "\n", lasts);
555 if (bufptr == 0) return 0L;
557 sscanf( bufptr, "%d", &ver );
558 if (ver != CURRENT_VERSION) return 0L;
560 while( !abrt && (eob > bufptr))
562 bufptr = strtok_r( 0L, "\n", lasts );
563 if (bufptr == 0) return 0L;
564 if (strncasecmp( bufptr, "end", 3) == 0) break;
566 sscanf( bufptr, "%s", line );
567 if (line[0] != '#') /* ignore comments */
571 case controls_header:
573 if (strncasecmp( line, "controls", 8) == 0)
575 n = sscanf( bufptr, "%s%d", line, &Number_of_Controls );
576 if (n != 2) abrt = 1;
577 looking_for = controls;
584 n = sscanf( bufptr, "%s%s%le%le%le",
585 Controls[i].Symbol.Mod_Name,
586 Controls[i].Symbol.Par_Name,
587 &Controls[i].Min_Val,
588 &Controls[i].Max_Val,
589 &Controls[i].Pert_Size);
590 if (n != 5) abrt = 1;
591 Controls[i].Symbol.Addr = NIL_POINTER;
593 if (i >= Number_of_Controls) looking_for = outputs_header;
598 if (strncasecmp( line, "outputs", 7) == 0)
600 n = sscanf( bufptr, "%s%d", line, &Number_of_Outputs );
601 if (n != 2) abrt = 1;
602 looking_for = outputs;
609 n = sscanf( bufptr, "%s%s%le",
610 Outputs[i].Symbol.Mod_Name,
611 Outputs[i].Symbol.Par_Name,
612 &Outputs[i].Trim_Criteria );
613 if (n != 3) abrt = 1;
614 Outputs[i].Symbol.Addr = NIL_POINTER;
616 if (i >= Number_of_Outputs) looking_for = done;
628 (Number_of_Controls > 0) &&
629 (Number_of_Outputs == Number_of_Controls))
633 for(i=0;i<Number_of_Controls;i++) /* Initialize fields in Controls data */
635 Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
637 Controls[i].Symbol.Addr = NIL_POINTER;
638 Controls[i].Authority = Controls[i].Max_Val - Controls[i].Min_Val;
639 if (Controls[i].Authority == 0.0)
640 Controls[i].Authority = 1.0;
641 Controls[i].Requested_Percent =
642 (Controls[i].Curr_Val - Controls[i].Min_Val)
643 /Controls[i].Authority;
644 Controls[i].Pert_Size = Controls[i].Pert_Size/Controls[i].Authority;
647 for (i=0;i<Number_of_Outputs;i++) /* Initialize fields in Outputs data */
649 Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
650 if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
651 Outputs[i].Weighting =
652 Outputs[0].Trim_Criteria/Outputs[i].Trim_Criteria;
662 void ls_trim_put_set( FILE *fp )
668 fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
670 fprintf(fp, FACILITY_NAME_STRING);
672 fprintf(fp, "%04d\n", CURRENT_VERSION);
673 fprintf(fp, " controls: %d\n", Number_of_Controls);
674 fprintf(fp, "# module parameter min_val max_val pert_size\n");
675 for (i=0; i<Number_of_Controls; i++)
676 fprintf(fp, " %s\t%s\t%E\t%E\t%E\n",
677 Controls[i].Symbol.Mod_Name,
678 Controls[i].Symbol.Par_Name,
681 Controls[i].Pert_Size*Controls[i].Authority);
682 fprintf(fp, " outputs: %d\n", Number_of_Outputs);
683 fprintf(fp, "# module parameter trim_criteria\n");
684 for (i=0;i<Number_of_Outputs;i++)
685 fprintf(fp, " %s\t%s\t%E\n",
686 Outputs[i].Symbol.Mod_Name,
687 Outputs[i].Symbol.Par_Name,
688 Outputs[i].Trim_Criteria );
689 fprintf(fp, "end\n");