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