1 /***************************************************************************
2
3 TITLE: ls_trim.c
4
5 ----------------------------------------------------------------------------
6
7 FUNCTION: Trims the simulated aircraft by using certain
8 controls to null out a similar number of outputs.
9
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).
16
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
20 name is H_Partials):
21
22 dY = H dU
23
24
25 The columns of H correspond to the control effect; the rows of
26 H correspond to the outputs affected.
27
28 We wish to find dU such that for U = U0 + dU,
29
30 Y = Y0 + dY = 0
31 or dY = -Y0
32
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:
40
41 -Y0 = H dU
42 W (-Y0) = W H dU premultiply by W
43 H' W (-Y0) = H' W H dU premultiply by H'
44
45 dU = [inv(H' W H)][ H' W (-Y0)] Solve for dU
46
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).
50
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.
56
57
58 ----------------------------------------------------------------------------
59
60 MODULE STATUS: developmental
61
62 ----------------------------------------------------------------------------
63
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.
67
68 ----------------------------------------------------------------------------
69
70 DESIGNED BY: E. B. Jackson
71
72 CODED BY: same
73
74 MAINTAINED BY: same
75
76 ----------------------------------------------------------------------------
77
78 MODIFICATION HISTORY:
79
80 DATE PURPOSE BY
81
82 950307 Modified to make use of ls_get_sym_val and ls_put_sym_val
83 routines. EBJ
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
86
87 CURRENT RCS HEADER:
88
89 $Header$
90 $Log$
91 Revision 1.1 2002/09/10 01:14:02 curt
92 Initial revision
93
94 Revision 1.2 2001/07/30 20:53:54 curt
95 Various MSVC tweaks and warning fixes.
96
97 Revision 1.1.1.1 1999/06/17 18:07:33 curt
98 Start of 0.7.x branch
99
100 Revision 1.1.1.1 1999/04/05 21:32:45 curt
101 Start of 0.6.x branch.
102
103 * Revision 1.9 1995/03/29 16:09:56 bjax
104 * Fixed bug in having more than three trim controls; removed unnecessary
105 * call to ls_trim_init in ls_trim_get_set. EBJ
106 *
107 * Revision 1.8 1995/03/16 12:28:40 bjax
108 * Fixed problem where ls_trim() returns non-zero if
109 * symbols are not loaded - implies vehicle trimmed when
110 * actually no trim attempt is made. This results in storing of non-
111 * trimmed initial conditions in sims without defined trim controls.
112 *
113 * Revision 1.7 1995/03/15 12:17:12 bjax
114 * Added flag marker line to ls_trim_put_set() routine output.
115 *
116 * Revision 1.6 1995/03/08 11:49:07 bjax
117 * Minor improvements to ls_trim_get_set; deleted weighting parameter
118 * for output definition; added comment lines to settings file output.
119 *
120 * Revision 1.5 1995/03/07 22:38:04 bjax
121 * Removed ls_generic.h; this version relies entirely on symbol table routines to
122 * set and get variable values. Added additional fields to Control record structure;
123 * created Output record with appropriate fields. Added ls_trim_put_set() and
124 * ls_trim_get_set() routines. Heavily modified initialization routine; most of this
125 * logic now resides in ls_trim_get_set(). Renamed all routines so that they being
126 * with "ls_trim_" to avoid conflicts.
127 * EBJ
128 *
129 * Revision 1.4 1995/03/07 13:04:16 bjax
130 * Configured to use ls_get_sym_val() and ls_set_sym_val().
131 *
132 * Revision 1.3 1995/03/03 01:59:53 bjax
133 * Moved definition of SYMBOL_NAME and SYMBOL_TYPE to ls_sym.h
134 * and removed from this module. EBJ
135 *
136 * Revision 1.2 1995/02/27 19:53:41 bjax
137 * Moved symbol routines to ls_sym.c to declutter this file. EBJ
138 *
139 * Revision 1.1 1995/02/27 18:14:10 bjax
140 * Initial revision
141 *
142
143 ----------------------------------------------------------------------------
144
145 REFERENCES:
146
147 ----------------------------------------------------------------------------
148
149 CALLED BY:
150
151 ----------------------------------------------------------------------------
152
153 CALLS TO:
154
155 ----------------------------------------------------------------------------
156
157 INPUTS:
158
159 ----------------------------------------------------------------------------
160
161 OUTPUTS:
162
163 --------------------------------------------------------------------------*/
164
165 static char rcsid[] = "$Id$";
166
167 #ifdef __SUNPRO_CC
168 # define _REENTRANT
169 #endif
170
171 #include <string.h>
172 #include "ls_constants.h"
173 #include "ls_types.h"
174 #include "ls_sym.h"
175 #include "ls_matrix.h"
176 #include "ls_interface.h"
177
178
179 #ifndef TRUE
180 #define FALSE 0
181 #define TRUE !FALSE
182 #endif
183
184 #define MAX_NUMBER_OF_CONTROLS 10
185 #define MAX_NUMBER_OF_OUTPUTS 10
186 #define STEP_LIMIT 0.01
187 #define NIL_POINTER 0L
188
189 #define FACILITY_NAME_STRING "trim"
190 #define CURRENT_VERSION 10
191
192
193 typedef struct
194 {
195 symbol_rec Symbol;
196 double Min_Val, Max_Val, Curr_Val, Authority;
197 double Percent, Requested_Percent, Pert_Size;
198 int Ineffective, At_Limit;
199 } control_rec;
200
201 typedef struct
202 {
203 symbol_rec Symbol;
204 double Curr_Val, Weighting, Trim_Criteria;
205 int Uncontrollable;
206 } output_rec;
207
208
209 static int Symbols_loaded = 0;
210 static int Index;
211 static int Trim_Cycles;
212 static int First;
213 static int Trimmed;
214 static double Gain;
215
216 static int Number_of_Controls;
217 static int Number_of_Outputs;
218 static control_rec Controls[ MAX_NUMBER_OF_CONTROLS ];
219 static output_rec Outputs[ MAX_NUMBER_OF_OUTPUTS ];
220
221 static double **H_Partials;
222
223 static double Baseline_Output[ MAX_NUMBER_OF_OUTPUTS ];
224 static double Saved_Control, Saved_Control_Percent;
225
226 static double Cost, Previous_Cost;
227
228
229
230
ls_trim_init()231 int ls_trim_init()
232 /* Initialize partials matrix */
233 {
234 int i, error;
235 // int result;
236
237 Index = -1;
238 Trim_Cycles = 0;
239 Gain = 1;
240 First = 1;
241 Previous_Cost = 0.0;
242 Trimmed = 0;
243
244 for (i=0;i<Number_of_Controls;i++)
245 {
246 Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
247 if (error) Controls[i].Symbol.Addr = NIL_POINTER;
248 Controls[i].Requested_Percent =
249 (Controls[i].Curr_Val - Controls[i].Min_Val)
250 /Controls[i].Authority;
251 }
252
253 H_Partials = nr_matrix( 1, Number_of_Controls, 1, Number_of_Controls );
254 if (H_Partials == 0) return -1;
255
256 return 0;
257 }
258
ls_trim_get_vals()259 void ls_trim_get_vals()
260 /* Load the Output vector, and calculate control percent used */
261 {
262 int i, error;
263
264 for (i=0;i<Number_of_Outputs;i++)
265 {
266 Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
267 if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
268 }
269
270 Cost = 0.0;
271 for (i=0;i<Number_of_Controls;i++)
272 {
273 Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
274 if (error) Controls[i].Symbol.Addr = NIL_POINTER;
275 Controls[i].Percent =
276 (Controls[i].Curr_Val - Controls[i].Min_Val)
277 /Controls[i].Authority;
278 }
279
280
281 }
282
ls_trim_move_controls()283 void ls_trim_move_controls()
284 /* This routine moves the current control to specified percent of authority */
285 {
286 int i;
287
288 for(i=0;i<Number_of_Controls;i++)
289 {
290 Controls[i].At_Limit = 0;
291 if (Controls[i].Requested_Percent <= 0.0)
292 {
293 Controls[i].Requested_Percent = 0.0;
294 Controls[i].At_Limit = 1;
295 }
296 if (Controls[i].Requested_Percent >= 1.0)
297 {
298 Controls[i].Requested_Percent = 1.0;
299 Controls[i].At_Limit = 1;
300 }
301 Controls[i].Curr_Val = Controls[i].Min_Val +
302 (Controls[i].Max_Val - Controls[i].Min_Val) *
303 Controls[i].Requested_Percent;
304 }
305 }
306
ls_trim_put_controls()307 void ls_trim_put_controls()
308 /* Put current control requests out to controls themselves */
309 {
310 int i;
311
312 for (i=0;i<Number_of_Controls;i++)
313 if (Controls[i].Symbol.Addr)
314 ls_set_sym_val( &Controls[i].Symbol, Controls[i].Curr_Val );
315 }
316
ls_trim_calc_cost()317 void ls_trim_calc_cost()
318 /* This routine calculates the current distance, or cost, from trim */
319 {
320 int i;
321
322 Cost = 0.0;
323 for(i=0;i<Number_of_Outputs;i++)
324 Cost += pow((Outputs[i].Curr_Val/Outputs[i].Trim_Criteria),2.0);
325 }
326
ls_trim_save_baseline_outputs()327 void ls_trim_save_baseline_outputs()
328 {
329 int i, error;
330
331 for (i=0;i<Number_of_Outputs;i++)
332 Baseline_Output[i] = ls_get_sym_val( &Outputs[i].Symbol, &error );
333 }
334
ls_trim_eval_outputs()335 int ls_trim_eval_outputs()
336 {
337 int i, trimmed;
338
339 trimmed = 1;
340 for(i=0;i<Number_of_Outputs;i++)
341 if( fabs(Outputs[i].Curr_Val) > Outputs[i].Trim_Criteria) trimmed = 0;
342 return trimmed;
343 }
344
ls_trim_calc_h_column()345 void ls_trim_calc_h_column()
346 {
347 int i;
348 double delta_control, delta_output;
349
350 delta_control = (Controls[Index].Curr_Val - Saved_Control)/Controls[Index].Authority;
351
352 for(i=0;i<Number_of_Outputs;i++)
353 {
354 delta_output = Outputs[i].Curr_Val - Baseline_Output[i];
355 H_Partials[i+1][Index+1] = delta_output/delta_control;
356 }
357 }
358
ls_trim_do_step()359 void ls_trim_do_step()
360 {
361 int i, j, l, singular;
362 double **h_trans_w_h;
363 double delta_req_mag, scaling;
364 double delta_U_requested[ MAX_NUMBER_OF_CONTROLS ];
365 double temp[ MAX_NUMBER_OF_CONTROLS ];
366
367 /* Identify ineffective controls (whose partials are all near zero) */
368
369 for (j=0;j<Number_of_Controls;j++)
370 {
371 Controls[j].Ineffective = 1;
372 for(i=0;i<Number_of_Outputs;i++)
373 if (fabs(H_Partials[i+1][j+1]) > EPS) Controls[j].Ineffective = 0;
374 }
375
376 /* Identify uncontrollable outputs */
377
378 for (j=0;j<Number_of_Outputs;j++)
379 {
380 Outputs[j].Uncontrollable = 1;
381 for(i=0;i<Number_of_Controls;i++)
382 if (fabs(H_Partials[j+1][i+1]) > EPS) Outputs[j].Uncontrollable = 0;
383 }
384
385 /* Calculate well-conditioned partials matrix [ H' W H ] */
386
387 h_trans_w_h = nr_matrix(1, Number_of_Controls, 1, Number_of_Controls);
388 if (h_trans_w_h == 0)
389 {
390 fprintf(stderr, "Memory error in ls_trim().\n");
391 exit(1);
392 }
393 for (l=1;l<=Number_of_Controls;l++)
394 for (j=1;j<=Number_of_Controls;j++)
395 {
396 h_trans_w_h[l][j] = 0.0;
397 for (i=1;i<=Number_of_Outputs;i++)
398 h_trans_w_h[l][j] +=
399 H_Partials[i][l]*H_Partials[i][j]*Outputs[i-1].Weighting;
400 }
401
402 /* Invert the partials array [ inv( H' W H ) ]; note: h_trans_w_h is replaced
403 with its inverse during this function call */
404
405 singular = nr_gaussj( h_trans_w_h, Number_of_Controls, 0, 0 );
406
407 if (singular) /* Can't invert successfully */
408 {
409 nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
410 1, Number_of_Controls );
411 fprintf(stderr, "Singular matrix in ls_trim().\n");
412 return;
413 }
414
415 /* Form right hand side of equality: temp = [ H' W (-Y) ] */
416
417 for(i=0;i<Number_of_Controls;i++)
418 {
419 temp[i] = 0.0;
420 for(j=0;j<Number_of_Outputs;j++)
421 temp[i] -= H_Partials[j+1][i+1]*Baseline_Output[j]*Outputs[j].Weighting;
422 }
423
424 /* Solve for dU = [inv( H' W H )][ H' W (-Y)] */
425 for(i=0;i<Number_of_Controls;i++)
426 {
427 delta_U_requested[i] = 0.0;
428 for(j=0;j<Number_of_Controls;j++)
429 delta_U_requested[i] += h_trans_w_h[i+1][j+1]*temp[j];
430 }
431
432 /* limit step magnitude to certain size, but not direction */
433
434 delta_req_mag = 0.0;
435 for(i=0;i<Number_of_Controls;i++)
436 delta_req_mag += delta_U_requested[i]*delta_U_requested[i];
437 delta_req_mag = sqrt(delta_req_mag);
438 scaling = STEP_LIMIT/delta_req_mag;
439 if (scaling < 1.0)
440 for(i=0;i<Number_of_Controls;i++)
441 delta_U_requested[i] *= scaling;
442
443 /* Convert deltas to percent of authority */
444
445 for(i=0;i<Number_of_Controls;i++)
446 Controls[i].Requested_Percent = Controls[i].Percent + delta_U_requested[i];
447
448 /* free up temporary matrix */
449
450 nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
451 1, Number_of_Controls );
452
453 }
454
455
456
ls_trim()457 int ls_trim()
458 {
459 const int Max_Cycles = 100;
460 int Baseline;
461
462 Trimmed = 0;
463 if (Symbols_loaded) {
464
465 ls_trim_init(); /* Initialize Outputs & controls */
466 ls_trim_get_vals(); /* Limit the current control settings */
467 Baseline = TRUE;
468 ls_trim_move_controls(); /* Write out the new values of controls */
469 ls_trim_put_controls();
470 ls_loop( 0.0, -1 ); /* Cycle the simulation once with new limited
471 controls */
472
473 /* Main trim cycle loop follows */
474
475 while((!Trimmed) && (Trim_Cycles < Max_Cycles))
476 {
477 ls_trim_get_vals();
478 if (Index == -1)
479 {
480 ls_trim_calc_cost();
481 /*Adjust_Gain(); */
482 ls_trim_save_baseline_outputs();
483 Trimmed = ls_trim_eval_outputs();
484 }
485 else
486 {
487 ls_trim_calc_h_column();
488 Controls[Index].Curr_Val = Saved_Control;
489 Controls[Index].Percent = Saved_Control_Percent;
490 Controls[Index].Requested_Percent = Saved_Control_Percent;
491 }
492 Index++;
493 if (!Trimmed)
494 {
495 if (Index >= Number_of_Controls)
496 {
497 Baseline = TRUE;
498 Index = -1;
499 ls_trim_do_step();
500 }
501 else
502 { /* Save the current value & pert next control */
503 Baseline = FALSE;
504 Saved_Control = Controls[Index].Curr_Val;
505 Saved_Control_Percent = Controls[Index].Percent;
506
507 if (Controls[Index].Percent <
508 (1.0 - Controls[Index].Pert_Size) )
509 {
510 Controls[Index].Requested_Percent =
511 Controls[Index].Percent +
512 Controls[Index].Pert_Size ;
513 }
514 else
515 {
516 Controls[Index].Requested_Percent =
517 Controls[Index].Percent -
518 Controls[Index].Pert_Size;
519 }
520 }
521 ls_trim_move_controls();
522 ls_trim_put_controls();
523 ls_loop( 0.0, -1 );
524 Trim_Cycles++;
525 }
526 }
527
528 nr_free_matrix( H_Partials, 1, Number_of_Controls, 1, Number_of_Controls );
529 }
530
531 if (!Trimmed) fprintf(stderr, "Trim unsuccessful.\n");
532 return Trimmed;
533
534 }
535
536
ls_trim_get_set(char * buffer,char * eob)537 char *ls_trim_get_set(char *buffer, char *eob)
538 /* This routine parses the settings file for "trim" entries. */
539 {
540
541 static char *fac_name = FACILITY_NAME_STRING;
542 char *bufptr, **lasts, *nullptr, null = '\0';
543 char line[256];
544 int n, ver, i, error, abrt;
545 enum {controls_header, controls, outputs_header, outputs, done} looking_for;
546
547 nullptr = &null;
548 lasts = &nullptr;
549 abrt = 0;
550 looking_for = controls_header;
551
552
553 n = sscanf(buffer, "%s", line);
554 if (n == 0) return 0L;
555 if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
556
557 bufptr = strtok_r( buffer+strlen(fac_name)+1, "\n", lasts);
558 if (bufptr == 0) return 0L;
559
560 sscanf( bufptr, "%d", &ver );
561 if (ver != CURRENT_VERSION) return 0L;
562
563 while( !abrt && (eob > bufptr))
564 {
565 bufptr = strtok_r( 0L, "\n", lasts );
566 if (bufptr == 0) return 0L;
567 if (strncasecmp( bufptr, "end", 3) == 0) break;
568
569 sscanf( bufptr, "%s", line );
570 if (line[0] != '#') /* ignore comments */
571 {
572 switch (looking_for)
573 {
574 case controls_header:
575 {
576 if (strncasecmp( line, "controls", 8) == 0)
577 {
578 n = sscanf( bufptr, "%s%d", line, &Number_of_Controls );
579 if (n != 2) abrt = 1;
580 looking_for = controls;
581 i = 0;
582 }
583 break;
584 }
585 case controls:
586 {
587 n = sscanf( bufptr, "%s%s%le%le%le",
588 Controls[i].Symbol.Mod_Name,
589 Controls[i].Symbol.Par_Name,
590 &Controls[i].Min_Val,
591 &Controls[i].Max_Val,
592 &Controls[i].Pert_Size);
593 if (n != 5) abrt = 1;
594 Controls[i].Symbol.Addr = NIL_POINTER;
595 i++;
596 if (i >= Number_of_Controls) looking_for = outputs_header;
597 break;
598 }
599 case outputs_header:
600 {
601 if (strncasecmp( line, "outputs", 7) == 0)
602 {
603 n = sscanf( bufptr, "%s%d", line, &Number_of_Outputs );
604 if (n != 2) abrt = 1;
605 looking_for = outputs;
606 i = 0;
607 }
608 break;
609 }
610 case outputs:
611 {
612 n = sscanf( bufptr, "%s%s%le",
613 Outputs[i].Symbol.Mod_Name,
614 Outputs[i].Symbol.Par_Name,
615 &Outputs[i].Trim_Criteria );
616 if (n != 3) abrt = 1;
617 Outputs[i].Symbol.Addr = NIL_POINTER;
618 i++;
619 if (i >= Number_of_Outputs) looking_for = done;
620 }
621 case done:
622 {
623 break;
624 }
625 }
626
627 }
628 }
629
630 if ((!abrt) &&
631 (Number_of_Controls > 0) &&
632 (Number_of_Outputs == Number_of_Controls))
633 {
634 Symbols_loaded = 1;
635
636 for(i=0;i<Number_of_Controls;i++) /* Initialize fields in Controls data */
637 {
638 Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
639 if (error)
640 Controls[i].Symbol.Addr = NIL_POINTER;
641 Controls[i].Authority = Controls[i].Max_Val - Controls[i].Min_Val;
642 if (Controls[i].Authority == 0.0)
643 Controls[i].Authority = 1.0;
644 Controls[i].Requested_Percent =
645 (Controls[i].Curr_Val - Controls[i].Min_Val)
646 /Controls[i].Authority;
647 Controls[i].Pert_Size = Controls[i].Pert_Size/Controls[i].Authority;
648 }
649
650 for (i=0;i<Number_of_Outputs;i++) /* Initialize fields in Outputs data */
651 {
652 Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
653 if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
654 Outputs[i].Weighting =
655 Outputs[0].Trim_Criteria/Outputs[i].Trim_Criteria;
656 }
657 }
658
659 bufptr = *lasts;
660 return bufptr;
661 }
662
663
664
ls_trim_put_set(FILE * fp)665 void ls_trim_put_set( FILE *fp )
666 {
667 int i;
668
669 if (fp==0) return;
670 fprintf(fp, "\n");
671 fprintf(fp, "#============================== %s\n", FACILITY_NAME_STRING);
672 fprintf(fp, "\n");
673 fprintf(fp, FACILITY_NAME_STRING);
674 fprintf(fp, "\n");
675 fprintf(fp, "%04d\n", CURRENT_VERSION);
676 fprintf(fp, " controls: %d\n", Number_of_Controls);
677 fprintf(fp, "# module parameter min_val max_val pert_size\n");
678 for (i=0; i<Number_of_Controls; i++)
679 fprintf(fp, " %s\t%s\t%E\t%E\t%E\n",
680 Controls[i].Symbol.Mod_Name,
681 Controls[i].Symbol.Par_Name,
682 Controls[i].Min_Val,
683 Controls[i].Max_Val,
684 Controls[i].Pert_Size*Controls[i].Authority);
685 fprintf(fp, " outputs: %d\n", Number_of_Outputs);
686 fprintf(fp, "# module parameter trim_criteria\n");
687 for (i=0;i<Number_of_Outputs;i++)
688 fprintf(fp, " %s\t%s\t%E\n",
689 Outputs[i].Symbol.Mod_Name,
690 Outputs[i].Symbol.Par_Name,
691 Outputs[i].Trim_Criteria );
692 fprintf(fp, "end\n");
693 return;
694 }
695