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