]> git.mxchange.org Git - flightgear.git/blob - src/FDM/LaRCsim/ls_trim.c
Adds a basic FDM model for LaRCsim debugging purposes.
[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  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
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
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
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
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
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
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
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
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
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
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
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
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 }