]> git.mxchange.org Git - flightgear.git/blob - src/FDM/LaRCsim/ls_trim.c
Add support for /controls/parking-brake -- it effective overrides the
[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.2  2001/07/30 20:53:54  curt
92 Various MSVC tweaks and warning fixes.
93
94 Revision 1.1.1.1  1999/06/17 18:07:33  curt
95 Start of 0.7.x branch
96
97 Revision 1.1.1.1  1999/04/05 21:32:45  curt
98 Start of 0.6.x branch.
99
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
103  *
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.
109  *
110  * Revision 1.7  1995/03/15  12:17:12  bjax
111  * Added flag marker line to ls_trim_put_set() routine output.
112  *
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.
116  *
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.
124  *  EBJ
125  *
126  * Revision 1.4  1995/03/07  13:04:16  bjax
127  * Configured to use ls_get_sym_val() and ls_set_sym_val().
128  *
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
132  *
133  * Revision 1.2  1995/02/27  19:53:41  bjax
134  * Moved symbol routines to ls_sym.c to declutter this file. EBJ
135  *
136  * Revision 1.1  1995/02/27  18:14:10  bjax
137  * Initial revision
138  *
139
140 ----------------------------------------------------------------------------
141
142         REFERENCES:
143
144 ----------------------------------------------------------------------------
145
146         CALLED BY:
147
148 ----------------------------------------------------------------------------
149
150         CALLS TO:
151
152 ----------------------------------------------------------------------------
153
154         INPUTS:
155
156 ----------------------------------------------------------------------------
157
158         OUTPUTS:
159
160 --------------------------------------------------------------------------*/
161
162 static char rcsid[] = "$Id$";
163
164 #ifdef __SUNPRO_CC
165 #  define _REENTRANT
166 #endif
167
168 #include <string.h>
169 #include "ls_constants.h"
170 #include "ls_types.h"
171 #include "ls_sym.h"
172 #include "ls_matrix.h"
173 #include "ls_interface.h"
174
175
176 #ifndef TRUE
177 #define FALSE  0
178 #define TRUE  !FALSE
179 #endif
180
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
185
186 #define FACILITY_NAME_STRING "trim"
187 #define CURRENT_VERSION 10
188
189
190 typedef struct
191 {
192     symbol_rec  Symbol;
193     double      Min_Val, Max_Val, Curr_Val, Authority;
194     double      Percent, Requested_Percent, Pert_Size;
195     int         Ineffective, At_Limit;
196 } control_rec;
197
198 typedef struct
199 {
200     symbol_rec  Symbol;
201     double      Curr_Val, Weighting, Trim_Criteria;
202     int         Uncontrollable;
203 } output_rec;
204
205
206 static  int             Symbols_loaded = 0;
207 static  int             Index;
208 static  int             Trim_Cycles;
209 static  int             First;
210 static  int             Trimmed;
211 static  double          Gain;
212
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 ];
217
218 static  double          **H_Partials;
219
220 static  double          Baseline_Output[ MAX_NUMBER_OF_OUTPUTS ];
221 static  double          Saved_Control, Saved_Control_Percent;
222
223 static  double          Cost, Previous_Cost;
224
225
226
227
228 int ls_trim_init()
229 /*  Initialize partials matrix */
230 {
231     int i, error;
232     // int result;
233
234     Index = -1;
235     Trim_Cycles = 0;
236     Gain = 1;
237     First = 1;
238     Previous_Cost = 0.0;
239     Trimmed = 0;
240
241     for (i=0;i<Number_of_Controls;i++)
242         {
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;
248         }
249
250     H_Partials = nr_matrix( 1, Number_of_Controls, 1, Number_of_Controls );
251     if (H_Partials == 0) return -1;
252
253     return 0;
254 }
255
256 void ls_trim_get_vals()
257 /* Load the Output vector, and calculate control percent used */
258 {
259     int i, error;
260
261     for (i=0;i<Number_of_Outputs;i++)
262         {
263             Outputs[i].Curr_Val = ls_get_sym_val( &Outputs[i].Symbol, &error );
264             if (error) Outputs[i].Symbol.Addr = NIL_POINTER;
265         }
266
267     Cost = 0.0;
268     for (i=0;i<Number_of_Controls;i++)
269         {
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;
275         }
276
277
278 }
279
280 void ls_trim_move_controls()
281 /* This routine moves the current control to specified percent of authority */
282 {
283     int i;
284
285     for(i=0;i<Number_of_Controls;i++)
286         {
287             Controls[i].At_Limit = 0;
288             if (Controls[i].Requested_Percent <= 0.0)
289                 {
290                     Controls[i].Requested_Percent = 0.0;
291                     Controls[i].At_Limit = 1;
292                 }
293             if (Controls[i].Requested_Percent >= 1.0)
294                 {
295                     Controls[i].Requested_Percent = 1.0;
296                     Controls[i].At_Limit = 1;
297                 }
298             Controls[i].Curr_Val = Controls[i].Min_Val +
299                 (Controls[i].Max_Val - Controls[i].Min_Val) *
300                 Controls[i].Requested_Percent;
301         }
302 }
303
304 void ls_trim_put_controls()
305 /* Put current control requests out to controls themselves */
306 {
307     int i;
308
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 );
312 }
313
314 void ls_trim_calc_cost()
315 /* This routine calculates the current distance, or cost, from trim */
316 {
317     int i;
318
319     Cost = 0.0;
320     for(i=0;i<Number_of_Outputs;i++)
321         Cost += pow((Outputs[i].Curr_Val/Outputs[i].Trim_Criteria),2.0);
322 }
323
324 void ls_trim_save_baseline_outputs()
325 {
326     int i, error;
327
328     for (i=0;i<Number_of_Outputs;i++)
329             Baseline_Output[i] = ls_get_sym_val( &Outputs[i].Symbol, &error );
330 }
331
332 int  ls_trim_eval_outputs()
333 {
334     int i, trimmed;
335
336     trimmed = 1;
337     for(i=0;i<Number_of_Outputs;i++)
338         if( fabs(Outputs[i].Curr_Val) > Outputs[i].Trim_Criteria) trimmed = 0;
339     return trimmed;
340 }
341
342 void ls_trim_calc_h_column()
343 {
344     int i;
345     double delta_control, delta_output;
346
347     delta_control = (Controls[Index].Curr_Val - Saved_Control)/Controls[Index].Authority;
348
349     for(i=0;i<Number_of_Outputs;i++)
350         {
351             delta_output = Outputs[i].Curr_Val - Baseline_Output[i];
352             H_Partials[i+1][Index+1] = delta_output/delta_control;
353         }
354 }
355
356 void ls_trim_do_step()
357 {
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 ];
363
364     /* Identify ineffective controls (whose partials are all near zero) */
365
366     for (j=0;j<Number_of_Controls;j++)
367         {
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;
371         }
372
373     /* Identify uncontrollable outputs */
374
375     for (j=0;j<Number_of_Outputs;j++)
376         {
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;
380         }
381
382     /* Calculate well-conditioned partials matrix [ H' W H ] */
383
384     h_trans_w_h = nr_matrix(1, Number_of_Controls, 1, Number_of_Controls);
385     if (h_trans_w_h == 0)
386         {
387             fprintf(stderr, "Memory error in ls_trim().\n");
388             exit(1);
389         }
390     for (l=1;l<=Number_of_Controls;l++)
391         for (j=1;j<=Number_of_Controls;j++)
392             {
393                 h_trans_w_h[l][j] = 0.0;
394                 for (i=1;i<=Number_of_Outputs;i++)
395                     h_trans_w_h[l][j] +=
396                         H_Partials[i][l]*H_Partials[i][j]*Outputs[i-1].Weighting;
397             }
398
399     /* Invert the partials array  [ inv( H' W H ) ]; note: h_trans_w_h is replaced
400        with its inverse during this function call */
401
402     singular = nr_gaussj( h_trans_w_h, Number_of_Controls, 0, 0 );
403
404     if (singular) /* Can't invert successfully */
405         {
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");
409             return;
410         }
411
412     /* Form right hand side of equality: temp = [ H' W (-Y) ] */
413
414     for(i=0;i<Number_of_Controls;i++)
415         {
416             temp[i] = 0.0;
417             for(j=0;j<Number_of_Outputs;j++)
418                 temp[i] -= H_Partials[j+1][i+1]*Baseline_Output[j]*Outputs[j].Weighting;
419         }
420
421     /* Solve for dU = [inv( H' W H )][ H' W (-Y)] */
422     for(i=0;i<Number_of_Controls;i++)
423         {
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];
427         }
428
429     /* limit step magnitude to certain size, but not direction */
430
431     delta_req_mag = 0.0;
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;
436     if (scaling < 1.0)
437         for(i=0;i<Number_of_Controls;i++)
438             delta_U_requested[i] *= scaling;
439
440     /* Convert deltas to percent of authority */
441
442     for(i=0;i<Number_of_Controls;i++)
443         Controls[i].Requested_Percent = Controls[i].Percent + delta_U_requested[i];
444
445     /* free up temporary matrix */
446
447     nr_free_matrix( h_trans_w_h, 1, Number_of_Controls,
448                                  1, Number_of_Controls );
449
450 }
451
452
453
454 int ls_trim()
455 {
456     const int Max_Cycles = 100;
457     int Baseline;
458
459     Trimmed = 0;
460     if (Symbols_loaded) {
461
462         ls_trim_init();                 /* Initialize Outputs & controls */
463         ls_trim_get_vals();  /* Limit the current control settings */
464         Baseline = TRUE;
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
468                                            controls */
469
470         /* Main trim cycle loop follows */
471
472         while((!Trimmed) && (Trim_Cycles < Max_Cycles))
473             {
474                 ls_trim_get_vals();
475                 if (Index == -1)
476                     {
477                         ls_trim_calc_cost();
478                         /*Adjust_Gain();        */
479                         ls_trim_save_baseline_outputs();
480                         Trimmed = ls_trim_eval_outputs();
481                     }
482                 else
483                     {
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;
488                     }
489                 Index++;
490                 if (!Trimmed)
491                     {
492                         if (Index >= Number_of_Controls)
493                             {
494                                 Baseline = TRUE;
495                                 Index = -1;
496                                 ls_trim_do_step();
497                             }
498                         else
499                             { /* Save the current value & pert next control */
500                                 Baseline = FALSE;
501                                 Saved_Control = Controls[Index].Curr_Val;
502                                 Saved_Control_Percent = Controls[Index].Percent;
503
504                                 if (Controls[Index].Percent < 
505                                     (1.0 - Controls[Index].Pert_Size) )
506                                     {
507                                         Controls[Index].Requested_Percent =
508                                             Controls[Index].Percent +
509                                             Controls[Index].Pert_Size ;
510                                     }
511                                 else
512                                     {
513                                         Controls[Index].Requested_Percent =
514                                             Controls[Index].Percent -
515                                             Controls[Index].Pert_Size;
516                                     }
517                             }
518                         ls_trim_move_controls();
519                         ls_trim_put_controls();
520                         ls_loop( 0.0, -1 );
521                         Trim_Cycles++;
522                     }
523             }
524
525         nr_free_matrix( H_Partials, 1, Number_of_Controls, 1, Number_of_Controls );
526     }
527
528     if (!Trimmed)  fprintf(stderr, "Trim unsuccessful.\n");
529     return Trimmed;
530
531 }
532
533
534 char *ls_trim_get_set(char *buffer, char *eob)
535 /* This routine parses the settings file for "trim" entries. */
536 {
537
538     static char *fac_name = FACILITY_NAME_STRING;
539     char *bufptr, **lasts, *nullptr, null = '\0';
540     char line[256];
541     int n, ver, i, error, abrt;
542     enum {controls_header, controls, outputs_header, outputs, done} looking_for;
543
544     nullptr = &null;
545     lasts = &nullptr;
546     abrt = 0;
547     looking_for = controls_header;
548
549
550     n = sscanf(buffer, "%s", line);
551     if (n == 0) return 0L;
552     if (strncasecmp( fac_name, line, strlen(fac_name) )) return 0L;
553
554     bufptr = strtok_r( buffer+strlen(fac_name)+1, "\n", lasts);
555     if (bufptr == 0) return 0L;
556
557     sscanf( bufptr, "%d", &ver );
558     if (ver != CURRENT_VERSION) return 0L;
559
560     while( !abrt && (eob > bufptr))
561       {
562         bufptr = strtok_r( 0L, "\n", lasts );
563         if (bufptr == 0) return 0L;
564         if (strncasecmp( bufptr, "end", 3) == 0) break;
565
566         sscanf( bufptr, "%s", line );
567         if (line[0] != '#') /* ignore comments */
568             {
569                 switch (looking_for)
570                     {
571                     case controls_header:
572                         {
573                             if (strncasecmp( line, "controls", 8) == 0) 
574                                 {
575                                     n = sscanf( bufptr, "%s%d", line, &Number_of_Controls );
576                                     if (n != 2) abrt = 1;
577                                     looking_for = controls;
578                                     i = 0;
579                                 }
580                             break;
581                         }
582                     case controls:
583                         {
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;
592                             i++;
593                             if (i >= Number_of_Controls) looking_for = outputs_header;
594                             break;
595                         }
596                     case outputs_header:
597                         {
598                             if (strncasecmp( line, "outputs", 7) == 0) 
599                                 {
600                                     n = sscanf( bufptr, "%s%d", line, &Number_of_Outputs );
601                                     if (n != 2) abrt = 1;
602                                     looking_for = outputs;
603                                     i = 0;
604                                 }
605                             break;
606                         }
607                     case outputs:
608                         {
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;
615                             i++;
616                             if (i >= Number_of_Outputs) looking_for = done;
617                         }  
618                     case done:
619                         {
620                             break;
621                         }
622                     }
623
624             }
625       }
626
627     if ((!abrt) && 
628         (Number_of_Controls > 0) && 
629         (Number_of_Outputs == Number_of_Controls))
630         {
631             Symbols_loaded = 1;
632
633             for(i=0;i<Number_of_Controls;i++) /* Initialize fields in Controls data */
634                 {
635                     Controls[i].Curr_Val = ls_get_sym_val( &Controls[i].Symbol, &error );
636                     if (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;
645                 }
646
647             for (i=0;i<Number_of_Outputs;i++) /* Initialize fields in Outputs data */
648                 {
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;
653                 }
654         }
655
656     bufptr = *lasts;
657     return bufptr;
658 }
659
660
661
662 void ls_trim_put_set( FILE *fp )
663 {
664     int i;
665
666     if (fp==0) return;
667     fprintf(fp, "\n");
668     fprintf(fp, "#==============================  %s\n", FACILITY_NAME_STRING);
669     fprintf(fp, "\n");
670     fprintf(fp, FACILITY_NAME_STRING);
671     fprintf(fp, "\n");
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,
679                 Controls[i].Min_Val,
680                 Controls[i].Max_Val,
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");
690     return;
691 }