integrator_lyapunov.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
27 
42 
43 using namespace std;
44 
46 
47 
48 //
49 // PUBLIC MEMBER FUNCTIONS:
50 //
51 
53  :Integrator( ){
54 
56 }
57 
58 
59 IntegratorLYAPUNOV::IntegratorLYAPUNOV( int dim_ , double power_)
60  :Integrator( ){
61 
62  int run1;
63 
65  dim = dim_ ;
66  err_power = power_;
67 
68  // BUTCHER TABLEAU:
69  // ----------------
70  A = new double*[dim];
71  b4 = new double [dim];
72  b5 = new double [dim];
73  c = new double [dim];
74 
75  for( run1 = 0; run1 < dim; run1++ ){
76  A[run1] = new double[dim];
77  }
78 }
79 
80 
81 IntegratorLYAPUNOV::IntegratorLYAPUNOV( const DifferentialEquation& rhs_, int dim_, double power_ )
82  :Integrator( ){
83 
84  int run1;
85 
86  dim = dim_ ;
87  err_power = power_;
88 
89  // BUTCHER TABLEAU:
90  // ----------------
91  A = new double*[dim];
92  b4 = new double [dim];
93  b5 = new double [dim];
94  c = new double [dim];
95 
96  for( run1 = 0; run1 < dim; run1++ ){
97  A[run1] = new double[dim];
98  }
99 
100  init( rhs_ );
101 }
102 
103 
105  :Integrator( arg ){
106 
107  constructAll( arg );
108 }
109 
110 
112 
113  deleteAll();
114 }
115 
116 
118 
119  lyap = rhs_.getLyapunovObject();
120  dimxmy=(lyap.x1).getDim()+(lyap.x2).getDim()+(lyap.x1).getDim()*(lyap.x1).getDim();
121  dimu=(lyap.u).getDim();
122  dimp=(lyap.p).getDim();
123  dimw=(lyap.w).getDim();
124 
125  //printf("dimxmy=%d dimu=%d dimp=%d dimw=%d m=%d \n", dimxmy, dimu, dimp, dimw,m);
126  rhs = new DifferentialEquation( rhs_ );
127  m = rhs->getDim ();
128  //m = dimxmy;
129  ma = 0;
130  mn = rhs->getN ();
131  mu = rhs->getNU ();
132  mui = rhs->getNUI ();
133  mp = rhs->getNP ();
134  mpi = rhs->getNPI ();
135  mw = rhs->getNW ();
136 
137  //printf("m=%d mn=%d mu=%d mui=%d mp=%d mpi=%d mw=%d \n",m, mn, mu, mui, mp, mpi, mw);
138  allocateMemory();
139 
140  return SUCCESSFUL_RETURN;
141 }
142 
143 
145 
146  dim = 0; A = 0; b4 = 0; b5 = 0; c = 0;
147  eta4 = 0; eta5 = 0; eta4_ = 0; eta5_ = 0;
148  k = 0; k2 = 0; l = 0; l2 = 0; x = 0;
149 
150  G = 0; etaG = 0;
151  G2 = 0; G3 = 0; etaG2 = 0; etaG3 = 0;
152 
153  H = 0; etaH = 0; H2 = 0; H3 = 0;
154  etaH2 = 0; etaH3 = 0;
155 
156  maxAlloc = 0;
157  err_power = 1.0;
158 
159  dimxmy = 0;
160  dimu = 0;
161  dimp = 0;
162  dimw = 0;
163  Y = 0;
164  seedmy = 0;
165 }
166 
167 
169 
170  int run1, run2;
171 
172  if( 0 != rhs->getNXA() ){
174  ASSERT(1 == 0);
175  }
176 
177  if( 0 != rhs->getNDX() ){
179  ASSERT(1 == 0);
180  }
181 
182 
183  if( m < 1 ){
185  ASSERT(1 == 0);
186  }
187 
188  // RK-ALGORITHM:
189  // -------------
190  eta4 = new double [m];
191  eta5 = new double [m];
192  eta4_ = new double [m];
193  eta5_ = new double [m];
194 
195  for( run1 = 0; run1 < m; run1++ ){
196 
197  eta4 [run1] = 0.0;
198  eta5 [run1] = 0.0;
199  eta4_[run1] = 0.0;
200  eta5_[run1] = 0.0;
201  }
202 
203  k = new double*[dim];
204  k2 = new double*[dim];
205  l = new double*[dim];
206  l2 = new double*[dim];
207  x = new double [rhs->getNumberOfVariables() + 1 + m];
208 
209  for( run1 = 0; run1 < rhs->getNumberOfVariables() + 1 + m; run1++ ){
210  x[run1] = 0.0;
211  }
212 
213  t = 0.0;
214 
215 
216  for( run1 = 0; run1 < dim; run1++ ){
217  k [run1] = new double[m];
218  k2 [run1] = new double[m];
219 
220  for( run2 = 0; run2 < m; run2++ ){
221  k [run1][run2] = 0.0;
222  k2[run1][run2] = 0.0;
223  }
224 
225  l [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
226  l2 [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
227 
228  for( run2 = 0; run2 < rhs->getNumberOfVariables() + 1 + m; run2++ ){
229  l [run1][run2] = 0.0;
230  l2[run1][run2] = 0.0;
231  }
232  }
233 
234  // INTERNAL INDEX LISTS:
235  // ---------------------
236  diff_index = new int[m];
237 
238  for( run1 = 0; run1 < m; run1++ ){
239  diff_index[run1] = rhs->getStateEnumerationIndex( run1 );
240  if( diff_index[run1] == rhs->getNumberOfVariables() ){
241  diff_index[run1] = diff_index[run1] + 1 + run1;
242  }
243  }
244 
245  control_index = new int[mu ];
246 
247  for( run1 = 0; run1 < mu; run1++ ){
248  control_index[run1] = rhs->index( VT_CONTROL, run1 );
249  }
250 
251  parameter_index = new int[mp ];
252 
253  for( run1 = 0; run1 < mp; run1++ ){
254  parameter_index[run1] = rhs->index( VT_PARAMETER, run1 );
255  }
256 
257  int_control_index = new int[mui];
258 
259  for( run1 = 0; run1 < mui; run1++ ){
260  int_control_index[run1] = rhs->index( VT_INTEGER_CONTROL, run1 );
261  }
262 
263  int_parameter_index = new int[mpi];
264 
265  for( run1 = 0; run1 < mpi; run1++ ){
267  }
268 
269  disturbance_index = new int[mw ];
270 
271  for( run1 = 0; run1 < mw; run1++ ){
272  disturbance_index[run1] = rhs->index( VT_DISTURBANCE, run1 );
273  }
274 
275  time_index = rhs->index( VT_TIME, 0 );
276 
277  diff_scale.init(m);
278  for( run1 = 0; run1 < m; run1++ )
279  diff_scale(run1) = rhs->scale( VT_DIFFERENTIAL_STATE, run1 );
280 
281 
282  // SENSITIVITIES:
283  // --------------
284  G = NULL;
285  etaG = NULL;
286 
287  G2 = NULL;
288  G3 = NULL;
289  etaG2 = NULL;
290  etaG3 = NULL;
291 
292  H = NULL;
293  etaH = NULL;
294 
295  H2 = NULL;
296  H3 = NULL;
297  etaH2 = NULL;
298  etaH3 = NULL;
299 
300 
301  // STORAGE:
302  // --------
303  maxAlloc = 1;
304 
305 
306  // Derivative matrix + seedvector
307  Y = new double*[dimxmy+dimu+dimp+dimw];
308  seedmy = new double[dimxmy+dimu+dimp+dimw];
309 
310  for( run1 = 0; run1 < rhs->getNumberOfVariables() + 1 + m; run1++ ){
311  x[run1] = 0.0;
312  }
313 
314  for( run1 = 0; run1 < dimxmy+dimu+dimp+dimw; run1++ ){
315  Y[run1] = new double[dimxmy+dimu+dimp+dimw];
316  }
317 
318 }
319 
320 
321 
323 
324  int run1;
325 
326 
327  // BUTCHER-
328  // TABLEAU:
329  // ----------
330  for( run1 = 0; run1 < dim; run1++ ){
331  delete[] A[run1];
332  }
333 
334  delete[] A;
335  delete[] b4;
336  delete[] b5;
337  delete[] c ;
338 
339 
340  // RK-ALGORITHM:
341  // -------------
342  if( eta4 != NULL ){
343  delete[] eta4;
344  }
345  if( eta4_ != NULL ){
346  delete[] eta4_;
347  }
348  if( eta5 != NULL ){
349  delete[] eta5;
350  }
351  if( eta5_ != NULL ){
352  delete[] eta5_;
353  }
354 
355  for( run1 = 0; run1 < dim; run1++ ){
356  if( k[run1] != NULL )
357  delete[] k[run1] ;
358  if( k2[run1] != NULL )
359  delete[] k2[run1];
360  if( l[run1] != NULL )
361  delete[] l[run1] ;
362  if( l2[run1] != NULL )
363  delete[] l2[run1];
364  }
365 
366  if( k != NULL )
367  delete[] k ;
368 
369  if( k2!= NULL )
370  delete[] k2;
371 
372  if( l != NULL )
373  delete[] l ;
374 
375  if( l2!= NULL )
376  delete[] l2;
377 
378  if( x != NULL )
379  delete[] x;
380 
381 
382  // SENSITIVITIES:
383  // ----------------------------------------
384 
385  if( G != NULL )
386  delete[] G;
387 
388  if( etaG != NULL )
389  delete[] etaG;
390 
391  if( G2 != NULL )
392  delete[] G2;
393 
394  if( G3 != NULL )
395  delete[] G3;
396 
397  if( etaG2 != NULL )
398  delete[] etaG2;
399 
400  if( etaG3 != NULL )
401  delete[] etaG3;
402 
403 
404  // ----------------------------------------
405 
406  if( H != NULL )
407  delete[] H;
408 
409  if( etaH != NULL )
410  delete[] etaH;
411 
412  if( H2 != NULL )
413  delete[] H2;
414 
415  if( H3 != NULL )
416  delete[] H3;
417 
418  if( etaH2 != NULL )
419  delete[] etaH2;
420 
421  if( etaH3 != NULL )
422  delete[] etaH3;
423 
424 
425  for( run1 = 0; run1 < dimxmy+dimu+dimp+dimw; run1++ ){
426  if( Y[run1] != NULL )
427  delete[] Y[run1] ;
428  }
429 
430  if( Y != NULL )
431  delete[] Y ;
432 
433  if( seedmy != NULL )
434  delete[] seedmy ;
435 
436 }
437 
438 
440 
441  if ( this != &arg ){
442  deleteAll();
443  Integrator::operator=( arg );
444  constructAll( arg );
445  }
446 
447  return *this;
448 }
449 
450 
452 
453  int run1, run2;
454 
455  rhs = new DifferentialEquation( *arg.rhs );
456 
457  m = arg.m ;
458  ma = arg.ma ;
459  mn = arg.mn ;
460  mu = arg.mu ;
461  mui = arg.mui ;
462  mp = arg.mp ;
463  mpi = arg.mpi ;
464  mw = arg.mw ;
465 
466 
467  if( m < 1 ){
469  ASSERT(1 == 0);
470  }
471 
472  // BUTCHER TABLEAU:
473  // ----------------
474  dim = arg.dim;
475  A = new double*[dim];
476  b4 = new double [dim];
477  b5 = new double [dim];
478  c = new double [dim];
479 
480  for( run1 = 0; run1 < dim; run1++ ){
481 
482  b4[run1] = arg.b4[run1];
483  b5[run1] = arg.b5[run1];
484  c [run1] = arg.c [run1];
485 
486  A[run1] = new double[dim];
487  for( run2 = 0; run2 < dim; run2++ )
488  A[run1][run2] = arg.A[run1][run2];
489  }
490 
491  // RK-ALGORITHM:
492  // -------------
493  eta4 = new double [m];
494  eta5 = new double [m];
495  eta4_ = new double [m];
496  eta5_ = new double [m];
497 
498  for( run1 = 0; run1 < m; run1++ ){
499 
500  eta4 [run1] = arg.eta4 [run1];
501  eta5 [run1] = arg.eta5 [run1];
502  eta4_[run1] = arg.eta4_[run1];
503  eta5_[run1] = arg.eta5_[run1];
504  }
505 
506  k = new double*[dim];
507  k2 = new double*[dim];
508  l = new double*[dim];
509  l2 = new double*[dim];
510  x = new double [rhs->getNumberOfVariables() + 1 + m];
511 
512  for( run1 = 0; run1 < rhs->getNumberOfVariables() + 1 + m; run1++ ){
513  x[run1] = arg.x[run1];
514  }
515 
516  t = arg.t;
517 
518 
519  for( run1 = 0; run1 < dim; run1++ ){
520  k [run1] = new double[m];
521  k2 [run1] = new double[m];
522 
523  for( run2 = 0; run2 < m; run2++ ){
524  k [run1][run2] = arg.k [run1][run2];
525  k2[run1][run2] = arg.k2[run1][run2];
526  }
527 
528  l [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
529  l2 [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
530 
531  for( run2 = 0; run2 < rhs->getNumberOfVariables() + 1 + m; run2++ ){
532  l [run1][run2] = arg.l [run1][run2];
533  l2[run1][run2] = arg.l2[run1][run2];
534  }
535  }
536 
537 
538  // SETTINGS:
539  // ---------
540  h = (double*)calloc(arg.maxAlloc,sizeof(double));
541  for( run1 = 0; run1 < arg.maxAlloc; run1++ ){
542  h[run1] = arg.h[run1];
543  }
544  hini = arg.hini;
545  hmin = arg.hmin;
546  hmax = arg.hmax;
547 
548  tune = arg.tune;
549  TOL = arg.TOL;
550 
551  err_power = arg.err_power;
552 
553 
554  // INTERNAL INDEX LISTS:
555  // ---------------------
556  diff_index = new int[m];
557 
558  for( run1 = 0; run1 < m; run1++ ){
559  diff_index[run1] = arg.diff_index[run1];
560  }
561 
562  ddiff_index = 0;
563  alg_index = 0;
564 
565  control_index = new int[mu ];
566 
567  for( run1 = 0; run1 < mu; run1++ ){
568  control_index[run1] = arg.control_index[run1];
569  }
570 
571  parameter_index = new int[mp ];
572 
573  for( run1 = 0; run1 < mp; run1++ ){
574  parameter_index[run1] = arg.parameter_index[run1];
575  }
576 
577  int_control_index = new int[mui];
578 
579  for( run1 = 0; run1 < mui; run1++ ){
580  int_control_index[run1] = arg.int_control_index[run1];
581  }
582 
583  int_parameter_index = new int[mpi];
584 
585  for( run1 = 0; run1 < mpi; run1++ ){
586  int_parameter_index[run1] = arg.int_parameter_index[run1];
587  }
588 
589  disturbance_index = new int[mw ];
590 
591  for( run1 = 0; run1 < mw; run1++ ){
592  disturbance_index[run1] = arg.disturbance_index[run1];
593  }
594 
595  time_index = arg.time_index;
596 
597 
598  // OTHERS:
599  // -------
601  count = arg.count ;
602  count2 = arg.count2 ;
603  count3 = arg.count3 ;
604 
605  diff_scale = arg.diff_scale;
606 
607 
608  // PRINT-LEVEL:
609  // ------------
610  PrintLevel = arg.PrintLevel;
611 
612 
613  // SENSITIVITIES:
614  // ---------------
615  nFDirs = 0 ;
616  nBDirs = 0 ;
617 
618  nFDirs2 = 0 ;
619  nBDirs2 = 0 ;
620 
621  G = NULL;
622  etaG = NULL;
623 
624  G2 = NULL;
625  G3 = NULL;
626  etaG2 = NULL;
627  etaG3 = NULL;
628 
629  H = NULL;
630  etaH = NULL;
631 
632  H2 = NULL;
633  H3 = NULL;
634  etaH2 = NULL;
635  etaH3 = NULL;
636 
637 
638  // THE STATE OF AGGREGATION:
639  // -------------------------
640  soa = arg.soa;
641 
642 
643  // STORAGE:
644  // --------
645  maxAlloc = arg.maxAlloc;
646 
647 
648  // DERIVATIVES MATRIX + SEEDVECTOR
649  lyap = arg.lyap ;
650  dimxmy = arg.dimxmy ;
651  dimu = arg.dimu ;
652  dimp = arg.dimp ;
653  dimw = arg.dimw ;
654 
655  Y = new double*[dimxmy+dimu+dimp+dimw];
656  seedmy = new double[dimxmy+dimu+dimp+dimw];
657 
658  for( run1 = 0; run1 < dimxmy+dimu+dimp+dimw; run1++ ){
659  Y[run1] = new double[dimxmy+dimu+dimp+dimw];
660  }
661 
662  for( run1 = 0; run1 < dimxmy+dimu+dimp+dimw; run1++ ){
663  seedmy[run1] = arg.seedmy[run1];
664  for( run2 = 0; run2 < dimxmy+dimu+dimp+dimw; run2++ ){
665  Y[run1][run2] = arg.Y[run1][run2];
666  }
667  }
668 
669 }
670 
671 
673 
674 
675  if( soa != SOA_UNFROZEN ){
676  if( PrintLevel != NONE ){
678  }
679  return RET_ALREADY_FROZEN;
680  }
681 
683  return SUCCESSFUL_RETURN;
684 }
685 
686 
687 
689 
690  if( soa != SOA_UNFROZEN ){
691  if( PrintLevel != NONE ){
693  }
694  return RET_ALREADY_FROZEN;
695  }
696 
698  return SUCCESSFUL_RETURN;
699 }
700 
701 
703 
704  maxAlloc = 1;
705  h = (double*)realloc(h,maxAlloc*sizeof(double));
706  soa = SOA_UNFROZEN;
707 
708  return SUCCESSFUL_RETURN;
709 }
710 
711 
713  const DVector &xa ,
714  const DVector &p ,
715  const DVector &u ,
716  const DVector &w ,
717  const Grid &t_ ){
718 
719  int run1;
720  returnValue returnvalue;
721 
722  if( rhs == NULL ){
723  return ACADOERROR(RET_TRIVIAL_RHS);
724  }
725 
726  if( xa.getDim() != 0 )
728 
729 
731 
732  timeInterval = t_;
733 
736 
739 // printf("initial integrator t = %e\n", t );
740 // printf("initial integrator x[time_index] = %e\n", x[time_index] );
741 // printf(" with time_index = %d\n", time_index );
742 
744  h[0] = hini;
745 
748 
749  if( h[0] < 10.0*EPS )
751  }
752  }
753 
754  if( x0.isEmpty() == BT_TRUE ) return ACADOERROR(RET_MISSING_INPUTS);
755 
756 
757  if( (int) x0.getDim() < m )
758  {
759  printf("x0dim = %d m=%d \n",(int) x0.getDim(), m);
761  }
762 
763  for( run1 = 0; run1 < m; run1++ ){
764  eta4[run1] = x0(run1);
765  eta5[run1] = x0(run1);
766  xStore(0,run1) = x0(run1);
767  }
768 
769  if( nFDirs != 0 ){
770  for( run1 = 0; run1 < m; run1++ ){
771  etaG[run1] = fseed(diff_index[run1]);
772  }
773  }
774 
775  if( mp > 0 ){
776  if( (int) p.getDim() < mp )
778 
779  for( run1 = 0; run1 < mp; run1++ ){
780  x[parameter_index[run1]] = p(run1);
781  }
782  }
783 
784  if( mu > 0 ){
785  if( (int) u.getDim() < mu )
787 
788  for( run1 = 0; run1 < mu; run1++ ){
789  x[control_index[run1]] = u(run1);
790  }
791  }
792 
793 
794  if( mw > 0 ){
795  if( (int) w.getDim() < mw )
797 
798  for( run1 = 0; run1 < mw; run1++ ){
799  x[disturbance_index[run1]] = w(run1);
800  }
801  }
802 
803 
804  totalTime.start();
805  nFcnEvaluations = 0;
806 
807 
808  // Initialize the scaling based on the initial states:
809  // ---------------------------------------------------
810 
811  double atol;
812  get( ABSOLUTE_TOLERANCE, atol );
813 
814  for( run1 = 0; run1 < m; run1++ )
815  diff_scale(run1) = fabs(eta4[run1]) + atol/TOL;
816 
817 
818  // PRINTING:
819  // ---------
820  if( PrintLevel == HIGH || PrintLevel == MEDIUM ){
821  acadoPrintCopyrightNotice( "IntegratorLYAPUNOV -- A Runge Kutta integrator." );
822  }
823  if( PrintLevel == HIGH ){
824  cout << "RK: t = " << t << "\t";
825 
826  for( run1 = 0; run1 < m; run1++ ){
827  cout << "x[" << run1 << "] = " << scientific << eta4[run1] << " ";
828  }
829  cout << endl;
830  }
831 
832 
833  returnvalue = RET_FINAL_STEP_NOT_PERFORMED_YET;
834 
835  count3 = 0;
836  count = 1;
837 
838  while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET && count <= maxNumberOfSteps ){
839 
840  returnvalue = step(count);
841  count++;
842  }
843 
844  count2 = count-1;
845 
846  for( run1 = 0; run1 < mn; run1++ )
847  iStore( 0, run1 ) = iStore( 1, run1 );
848 
849  totalTime.stop();
850 
851  if( count > maxNumberOfSteps ){
852  if( PrintLevel != NONE )
855  }
856 
857 
858  // SET THE LOGGING INFORMATION:
859  // ----------------------------------------------------------------------------------------
860 
869 
870  // ----------------------------------------------------------------------------------------
871 
872 
873  // PRINTING:
874  // ---------
875  if( PrintLevel == MEDIUM ){
876 
877  if( soa == SOA_EVERYTHING_FROZEN ){
878  cout << "\n Results at t = " << t << "\t";
879 
880  for( run1 = 0; run1 < m; run1++ ){
881  cout << "x[" << run1 << "] = " << scientific << eta4[ run1 ];
882  }
883  cout << endl;
884  }
886  }
887 
888  int printIntegratorProfile = 0;
889  get( PRINT_INTEGRATOR_PROFILE,printIntegratorProfile );
890 
891  if ( (BooleanType)printIntegratorProfile == BT_TRUE )
892  {
894  }
895  else
896  {
897  if( PrintLevel == MEDIUM || PrintLevel == HIGH )
898  cout << "RK: number of steps: " << count - 1 << endl;
899  }
900 
901  return returnvalue;
902 }
903 
904 
905 
907  const DVector &pSeed,
908  const DVector &uSeed,
909  const DVector &wSeed,
910  const int &order ){
911 
912  //float aa;
913  if( order == 2 ){
914  return setForwardSeed2( xSeed, pSeed, uSeed, wSeed);
915  }
916  if( order < 1 || order > 2 ){
918  }
919 
920  if( nBDirs > 0 ){
922  }
923 
924  int run2;
925 
926  if( G != NULL ){
927  delete[] G;
928  G = NULL;
929  }
930 
931  if( etaG != NULL ){
932  delete[] etaG;
933  etaG = NULL;
934  }
935 
936  nFDirs = 1;
937 
939  fseed.setZero();
940 
941  G = new double[rhs->getNumberOfVariables() + 1 + m];
942 
943  for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++ ){
944  G[run2] = 0.0;
945  }
946 
947  etaG = new double[m];
948 
949  if( xSeed.getDim() != 0 ){
950  for( run2 = 0; run2 < m; run2++ ){
951  fseed(diff_index[run2]) = xSeed(run2);
952  //printf("state i=%d fseed=%f \n",diff_index[run2],fseed(diff_index[run2]));
953  }
954  }
955 
956  if( pSeed.getDim() != 0 ){
957  for( run2 = 0; run2 < mp; run2++ ){
958  fseed(parameter_index[run2]) = pSeed(run2);
959  G [parameter_index[run2]] = pSeed(run2);
960  //printf("parameters i=%d fseed=%f \n",parameter_index[run2],fseed(parameter_index[run2]));
961  }
962  }
963 
964  if( uSeed.getDim() != 0 ){
965  for( run2 = 0; run2 < mu; run2++ ){
966  fseed(control_index[run2]) = uSeed(run2);
967  G [control_index[run2]] = uSeed(run2);
968  //printf("controls i=%d fseed=%f \n",control_index[run2],fseed(control_index[run2]));
969  }
970  }
971 
972  if( wSeed.getDim() != 0 ){
973 
974  for( run2 = 0; run2 < mw; run2++ ){
975  fseed(disturbance_index[run2]) = wSeed(run2);
976  G[disturbance_index[run2]] = wSeed(run2);
977  //printf("disturbance i=%d fseed=%f \n",disturbance_index[run2],fseed(disturbance_index[run2]));
978  }
979  }
980  //scanf("%f ",&aa);
981 
982  return SUCCESSFUL_RETURN;
983 }
984 
985 
987  const DVector &pSeed ,
988  const DVector &uSeed ,
989  const DVector &wSeed ){
990 
991  int run2;
992 
993  if( G2 != NULL ){
994  delete[] G2;
995  G2 = NULL;
996  }
997 
998  if( G3 != NULL ){
999  delete[] G3;
1000  G3 = NULL;
1001  }
1002 
1003  if( etaG2 != NULL ){
1004  delete[] etaG2;
1005  etaG2 = NULL;
1006  }
1007 
1008  if( etaG3 != NULL ){
1009  delete[] etaG3;
1010  etaG3 = NULL;
1011  }
1012 
1013  nFDirs2 = 1;
1014 
1015  fseed2.init(rhs->getNumberOfVariables() + 1 + m);
1016  fseed2.setZero();
1017 
1018  G2 = new double[rhs->getNumberOfVariables() + 1 + m];
1019  G3 = new double[rhs->getNumberOfVariables() + 1 + m];
1020 
1021  for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++ ){
1022  G2[run2] = 0.0;
1023  G3[run2] = 0.0;
1024  }
1025  etaG2 = new double[m];
1026  etaG3 = new double[m];
1027 
1028  if( xSeed.getDim() != 0 ){
1029  for( run2 = 0; run2 < m; run2++ ){
1030  fseed2(diff_index[run2]) = xSeed(run2);
1031  }
1032  }
1033 
1034  if( pSeed.getDim() != 0 ){
1035  for( run2 = 0; run2 < mp; run2++ ){
1036  fseed2(parameter_index[run2]) = pSeed(run2);
1037  G2 [parameter_index[run2]] = pSeed(run2);
1038  }
1039  }
1040 
1041  if( uSeed.getDim() != 0 ){
1042  for( run2 = 0; run2 < mu; run2++ ){
1043  fseed2(control_index[run2]) = uSeed(run2);
1044  G2 [control_index[run2]] = uSeed(run2);
1045  }
1046  }
1047 
1048  if( wSeed.getDim() != 0 ){
1049  for( run2 = 0; run2 < mw; run2++ ){
1050  fseed2(disturbance_index[run2]) = wSeed(run2);
1051  G2 [disturbance_index[run2]] = wSeed(run2);
1052  }
1053  }
1054  return SUCCESSFUL_RETURN;
1055 }
1056 
1057 
1059 
1060  if( order == 2 ){
1061  return setBackwardSeed2(seed);
1062  }
1063  if( order < 1 || order > 2 ){
1065  }
1066 
1067  if( nFDirs > 0 ){
1069  }
1070 
1071  int run2;
1072 
1073  if( H != NULL ){
1074  delete[] H;
1075  H = NULL;
1076  }
1077 
1078  if( etaH != NULL ){
1079  delete[] etaH;
1080  etaH = NULL;
1081  }
1082 
1083  nBDirs = 1;
1084 
1085  bseed.init( m );
1086  bseed.setZero();
1087 
1088  H = new double[m];
1089 
1090  etaH = new double[rhs->getNumberOfVariables()+1+m];
1091  for( run2 = 0; run2 < rhs->getNumberOfVariables()+1+m; run2++ ){
1092  etaH[run2] = 0.0;
1093  }
1094 
1095  if( seed.getDim() != 0 ){
1096  for( run2 = 0; run2 < m; run2++ ){
1097  bseed(run2) = seed(run2);
1098  }
1099  }
1100 
1101  return SUCCESSFUL_RETURN;
1102 }
1103 
1104 
1105 
1107 
1108  int run2;
1109 
1110  if( H2 != NULL ){
1111  delete[] H2;
1112  H2 = NULL;
1113  }
1114 
1115  if( H3 != NULL ){
1116  delete[] H3;
1117  H3 = NULL;
1118  }
1119 
1120  if( etaH2 != NULL ){
1121  delete[] etaH2;
1122  etaH2 = NULL;
1123  }
1124 
1125  if( etaH3 != NULL ){
1126  delete[] etaH3;
1127  etaH3 = NULL;
1128  }
1129 
1130 
1131  nBDirs2 = 1;
1132 
1133  bseed2.init(m);
1134  bseed2.setZero();
1135 
1136  H2 = new double[m];
1137  H3 = new double[m];
1138 
1139  etaH2 = new double[rhs->getNumberOfVariables()+1+m];
1140  etaH3 = new double[rhs->getNumberOfVariables()+1+m];
1141 
1142  if( seed.getDim() != 0 ){
1143  for( run2 = 0; run2 < m; run2++ ){
1144  bseed2(run2) = seed(run2);
1145  }
1146  }
1147 
1148  return SUCCESSFUL_RETURN;
1149 }
1150 
1151 
1153 
1154  int run1, run2 ;
1155  returnValue returnvalue;
1156 
1157  if( rhs == NULL ){
1158  return ACADOERROR(RET_TRIVIAL_RHS);
1159  }
1160 
1161  if( soa != SOA_EVERYTHING_FROZEN ){
1162  return ACADOERROR(RET_NOT_FROZEN);
1163  }
1164 
1165 
1166  if( nFDirs != 0 ){
1168  dxStore.init( m, timeInterval );
1169  for( run1 = 0; run1 < m; run1++ ){
1170  etaG[run1] = fseed(diff_index[run1]);
1171  }
1172  }
1173 
1174  if( nBDirs != 0 ){
1175  for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++){
1176  etaH[run2] = 0.0;
1177  }
1178  }
1179 
1180  if( nBDirs != 0 ){
1181  for( run1 = 0; run1 < m; run1++ ){
1182  etaH[diff_index[run1]] = bseed(run1);
1183  }
1184  }
1185 
1186  if( nFDirs2 != 0 ){
1189  for( run1 = 0; run1 < m; run1++ ){
1190  etaG2[run1] = fseed2(diff_index[run1]);
1191  etaG3[run1] = 0.0;
1192  }
1193  }
1194 
1195  if( nBDirs2 != 0 ){
1196  for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++){
1197  etaH2[run2] = 0.0;
1198  etaH3[run2] = 0.0;
1199  }
1200  }
1201 
1202  if( nBDirs2 != 0 ){
1203  for( run1 = 0; run1 < m; run1++ ){
1204  etaH2[diff_index[run1]] = bseed2(run1);
1205  }
1206  }
1207 
1208  if( PrintLevel == HIGH ){
1210  }
1211 
1212  returnvalue = RET_FINAL_STEP_NOT_PERFORMED_YET;
1213 
1214 
1215  if( nBDirs > 0 || nBDirs2 > 0 ){
1216 
1217  int oldCount = count;
1218 
1219  count--;
1220  while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET && count >= 1 ){
1221 
1222  returnvalue = step( count );
1223  count--;
1224  }
1225 
1226  if( count == 0 && (returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET ||
1227  returnvalue == SUCCESSFUL_RETURN ) ){
1228 
1229 
1230  if( PrintLevel == MEDIUM ){
1232  }
1233  count = oldCount;
1234 
1235  return SUCCESSFUL_RETURN;
1236  }
1237  count = oldCount;
1238  }
1239  else{
1240 
1241  double a_my=0.0, b_my=0.0, c_my=0.0;//, aa;
1242  int counter_my=0;
1243 
1244  for( run2 = 0; run2 < (int)(lyap.x1).getDim(); run2++ ){
1245  a_my += etaG[run2];
1246  if (etaG[run2] >= 1.0)
1247  counter_my=run2;
1248  }
1249  b_my=a_my;
1250  for( run2 = (lyap.x1).getDim(); run2 < (int)((lyap.x1).getDim()+(lyap.x2).getDim()); run2++ ){
1251  b_my += etaG[run2];
1252  if (etaG[run2] >= 1.0)
1253  counter_my=run2;
1254  }
1255  c_my=b_my;
1256  for( run2 = (lyap.x1).getDim()+(lyap.x2).getDim(); run2 < m; run2++ ){
1257  c_my += etaG[run2];
1258  if (etaG[run2] >= 1.0)
1259  counter_my=run2;
1260  }
1261  //printf("a_my=%f b_my=%f c_my=%f counter_my=%d \n",a_my,b_my,c_my,counter_my);
1262 
1263 
1264  if (b_my>=1.0 || c_my<=0.0){
1265  count = 1;
1266  while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET &&
1267  count <= maxNumberOfSteps ){
1268 
1269  returnvalue = step(count);
1270  count++;
1271  }
1272  if( a_my>= 1.0){
1273  for( run2 = 0; run2 < m; run2++ ){
1274  Y[run2][counter_my]=etaG[run2];
1275  //printf("Y[%d][%d]=%f ",run2,counter_my,Y[run2][counter_my]);
1276  }
1277  //scanf("%f",&aa);
1278  }
1279  }
1280  else{
1281  int dimxmy12=(lyap.x1).getDim()+(lyap.x2).getDim();
1282  int counter_my1=counter_my-dimxmy12;
1283  counter_my1 = counter_my1/(lyap.x1).getDim();
1284  int counter_my2 = counter_my-dimxmy12-counter_my1*(lyap.x1).getDim();
1285  for( run2 = 0; run2 < dimxmy12; run2++ )
1286  etaG[run2]=0.0;
1287  int run4, run5;
1288  for( run4 = 0; run4 < (int)(lyap.x1).getDim(); run4++ )
1289  for( run5 = 0; run5 < (int)(lyap.x1).getDim(); run5++ )
1290 
1291  etaG[dimxmy12+(run4)*(lyap.x1).getDim()+run5] = Y[run4][counter_my1]*Y[run5][counter_my2];
1292  //etaG[dimxmy12+(run4)*4+run1] = Y[run4][counter_my1]*Y[run1][counter_my2];
1293  //printf("counter_my=%d counter_my1=%d counter_my2=%d \n",counter_my,counter_my1,counter_my2);
1294  //for( run2 = 0; run2 < m; run2++ ){
1295  //printf("etaG[%d]=%f ",run2,etaG[run2]);
1296  //}
1297  //scanf("%f",&aa);
1298 
1299  returnvalue = SUCCESSFUL_RETURN;
1300  }
1301 
1302  if( nBDirs2 == 0 && nFDirs != 0 )
1303  for( run1 = 0; run1 < m; run1++ )
1304  dxStore( 0, run1 ) = dxStore( 1, run1 );
1305 
1306  if( nFDirs2 != 0 )
1307  for( run1 = 0; run1 < m; run1++ )
1308  ddxStore( 0, run1 ) = ddxStore( 1, run1 );
1309 
1310  if( count > maxNumberOfSteps ){
1311  if( PrintLevel != NONE )
1314  }
1315 
1316  if( PrintLevel == MEDIUM ){
1318  }
1319  }
1320  return returnvalue;
1321 }
1322 
1323 
1325 
1326  int run1;
1327  double E = EPS;
1328 
1330  h[0] = h[number_];
1331  }
1332 
1333  if( soa == SOA_FREEZING_MESH ||
1334  soa == SOA_MESH_FROZEN ||
1336  soa == SOA_UNFROZEN ){
1337 
1338  if( nFDirs > 0 ){
1339  E = determineEta45(0);
1340  }
1341  else{
1342  E = determineEta45();
1343  }
1344  }
1345  if( soa == SOA_FREEZING_ALL ){
1346  E = determineEta45(dim*number_);
1347  }
1348 
1349 
1351 
1352  int number_of_rejected_steps = 0;
1353 
1354  if( E < 0.0 ){
1356  }
1357 
1358  // REJECT THE STEP IF GIVEN TOLERANCE IS NOT ACHIEVED:
1359  // ---------------------------------------------------
1360  while( E >= TOL*h[0] ){
1361 
1362  if( PrintLevel == HIGH ){
1363  cout << "STEP REJECTED: error estimate = " << scientific << E << endl
1364  << " required local tolerance = " << TOL * h[0] << endl;
1365  }
1366 
1367  number_of_rejected_steps++;
1368 
1369  for( run1 = 0; run1 < m; run1++ ){
1370  eta4[run1] = eta4_[run1];
1371  }
1372  if( h[0] <= hmin + EPS ){
1374  }
1375  h[0] = 0.5*h[0];
1376  if( h[0] < hmin ){
1377  h[0] = hmin;
1378  }
1379 
1380  if( soa == SOA_FREEZING_MESH ||
1381  soa == SOA_UNFROZEN ){
1382 
1383  if( nFDirs > 0 ){
1384  E = determineEta45(0);
1385  }
1386  else{
1387  E = determineEta45();
1388  }
1389  }
1390  if( soa == SOA_FREEZING_ALL ){
1391  E = determineEta45(dim*number_);
1392  }
1393 
1394  if( E < 0.0 ){
1396  }
1397  }
1398 
1399  count3 += number_of_rejected_steps;
1400  }
1401 
1402  // PROCEED IF THE STEP IS ACCEPTED:
1403  // --------------------------------
1404 
1405  double *etaG_ = new double[m];
1406  double *etaG3_ = new double[m];
1407 
1408 
1409  // compute forward derivatives if requested:
1410  // ------------------------------------------
1411 
1412  if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ){
1413 
1414  for( run1 = 0; run1 < m; run1++ )
1415  etaG_[run1] = etaG[run1];
1416 
1417  if( nBDirs != 0 ){
1419  }
1420 
1422  determineEtaGForward(dim*number_);
1423  }
1424  else{
1426  }
1427  }
1428  if( nBDirs > 0 ){
1429 
1430  if( soa != SOA_EVERYTHING_FROZEN ){
1431  return ACADOERROR(RET_NOT_FROZEN);
1432  }
1433  if( nFDirs != 0 || nBDirs2 != 0 || nFDirs2 != 0 ){
1435  }
1436  determineEtaHBackward(dim*number_);
1437  }
1438  if( nFDirs2 > 0 ){
1439 
1440  for( run1 = 0; run1 < m; run1++ )
1441  etaG3_[run1] = etaG3[run1];
1442 
1443  if( soa != SOA_EVERYTHING_FROZEN ){
1444  return ACADOERROR(RET_NOT_FROZEN);
1445  }
1446  if( nBDirs != 0 || nBDirs2 != 0 || nFDirs != 1 ){
1448  }
1449  determineEtaGForward2(dim*number_);
1450  }
1451  if( nBDirs2 > 0 ){
1452 
1453  if( soa != SOA_EVERYTHING_FROZEN ){
1454  return ACADOERROR(RET_NOT_FROZEN);
1455  }
1456  if( nBDirs != 0 || nFDirs2 != 0 || nFDirs != 1 ){
1458  }
1459 
1460  determineEtaHBackward2(dim*number_);
1461  }
1462 
1463 
1464  // increase the time:
1465  // ----------------------------------------------
1466 
1467  if( nBDirs > 0 || nBDirs2 > 0 ){
1468 
1469  t = t - h[0];
1470  }
1471  else{
1472 
1473  t = t + h[0];
1474  }
1475 // printf("t = %e\n",t );
1476 
1477  // PRINTING:
1478  // ---------
1479  if( PrintLevel == HIGH ){
1480  cout << "RK: t = " << scientific << t << " h = " << h[ 0 ] << " ";
1482  }
1483 
1484 
1485  // STORAGE:
1486  // --------
1487 
1489 
1490  if( number_ >= maxAlloc){
1491 
1492  maxAlloc = 2*maxAlloc;
1493  h = (double*)realloc(h,maxAlloc*sizeof(double));
1494  }
1495  h[number_] = h[0];
1496  }
1497 
1498  int i1 = timeInterval.getFloorIndex( t-h[0] );
1499  int i2 = timeInterval.getFloorIndex( t );
1500  int jj;
1501 
1502  for( jj = i1+1; jj <= i2; jj++ ){
1503 
1504  if( nFDirs == 0 && nBDirs == 0 && nFDirs2 == 0 && nBDirs == 0 ) interpolate( jj, eta4_ , k[0], eta4 , xStore );
1505  if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ) interpolate( jj, etaG_ , k[0], etaG , dxStore );
1506  if( nFDirs2 > 0 ) interpolate( jj, etaG3_, k[0], etaG3, ddxStore );
1507 
1508  for( run1 = 0; run1 < mn; run1++ )
1509  iStore( jj, run1 ) = x[rhs->index( VT_INTERMEDIATE_STATE, run1 )];
1510  }
1511 
1512  delete[] etaG_ ;
1513  delete[] etaG3_;
1514 
1515 
1516  if( nBDirs == 0 || nBDirs2 == 0 ){
1517 
1518  // Stop the algorithm if t >= te:
1519  // ----------------------------------------------
1520  if( t >= timeInterval.getLastTime() - EPS ){
1522  for( run1 = 0; run1 < m; run1++ ){
1523  x[diff_index[run1]] = eta4[run1];
1524  }
1525 
1526  if( soa == SOA_FREEZING_MESH ){
1527  soa = SOA_MESH_FROZEN;
1528  }
1531  }
1532 
1533  return SUCCESSFUL_RETURN;
1534  }
1535  }
1536 
1537 
1539 
1540 
1541  // recompute the scaling based on the actual states:
1542  // -------------------------------------------------
1543 
1544  double atol;
1545  get( ABSOLUTE_TOLERANCE, atol );
1546 
1547  for( run1 = 0; run1 < m; run1++ )
1548  diff_scale(run1) = fabs(eta4[run1]) + atol/TOL;
1549 
1550 
1551 
1552  // apply a numeric stabilization of the step size control:
1553  // -------------------------------------------------------
1554  double Emin = 1e-3*sqrt(TOL)*pow(hini, ((1.0/err_power)+1.0)/2.0 );
1555 
1556  if( E < Emin ) E = Emin ;
1557  if( E < 10.0*EPS ) E = 10.0*EPS;
1558 
1559 
1560  // determine the new step size:
1561  // ----------------------------------------------
1562  h[0] = h[0]*pow( tune*(TOL*h[0]/E), err_power );
1563 
1564  if( h[0] > hmax ){
1565  h[0] = hmax;
1566  }
1567  if( h[0] < hmin ){
1568  h[0] = hmin;
1569  }
1570 
1571  if( t + h[0] >= timeInterval.getLastTime() ){
1572  h[0] = timeInterval.getLastTime()-t;
1573  }
1574  }
1575 
1577 }
1578 
1579 
1580 
1582 
1584 }
1585 
1586 
1588 
1589  int run1;
1590 
1591  if( (int) xEnd[0].getDim() != m )
1593 
1594  for( run1 = 0; run1 < m; run1++ )
1595  xEnd[0](run1) = eta4[run1];
1596 
1597  return SUCCESSFUL_RETURN;
1598 }
1599 
1600 
1602 
1603  int run1;
1604  //double aa;
1605 
1606  if( Dx == NULL ){
1607  return SUCCESSFUL_RETURN;
1608  }
1609 
1610  if( order == 1 && nFDirs2 == 0 ){
1611  for( run1 = 0; run1 < m; run1++ ){
1612  Dx[0](run1,0) = etaG[run1];
1613  //printf("Dx[0](%d,0)=%f \n",run1,Dx[0](run1,0));
1614  }
1615  //scanf("%f",&aa);
1616  }
1617 
1618  if( order == 2 ){
1619  for( run1 = 0; run1 < m; run1++ ){
1620  Dx[0](run1,0) = etaG3[run1];
1621  }
1622  }
1623 
1624  if( order == 1 && nFDirs2 > 0 ){
1625  for( run1 = 0; run1 < m; run1++ ){
1626  Dx[0](run1,0) = etaG2[run1];
1627  }
1628  }
1629 
1630  if( order < 1 || order > 2 ){
1632  }
1633 
1634  return SUCCESSFUL_RETURN;
1635 }
1636 
1637 
1639  DVector &Dx_p ,
1640  DVector &Dx_u ,
1641  DVector &Dx_w ,
1642  int order ) const{
1643 
1644  int run2;
1645 
1646  if( order == 1 && nBDirs2 == 0 ){
1647 
1648  if( Dx_x0.getDim() != 0 ){
1649  for( run2 = 0; run2 < m; run2++ )
1650  Dx_x0(run2) = etaH[diff_index[run2]];
1651  }
1652  if( Dx_p.getDim() != 0 ){
1653  for( run2 = 0; run2 < mp; run2++ ){
1654  Dx_p(run2) = etaH[parameter_index[run2]];
1655  }
1656  }
1657  if( Dx_u.getDim() != 0 ){
1658  for( run2 = 0; run2 < mu; run2++ ){
1659  Dx_u(run2) = etaH[control_index[run2]];
1660  }
1661  }
1662  if( Dx_w.getDim() != 0 ){
1663  for( run2 = 0; run2 < mw; run2++ ){
1664  Dx_w(run2) = etaH[disturbance_index[run2]];
1665  }
1666  }
1667  }
1668 
1669  if( order == 1 && nBDirs2 > 0 ){
1670 
1671  if( Dx_x0.getDim() != 0 ){
1672  for( run2 = 0; run2 < m; run2++ ){
1673  Dx_x0(run2) = etaH2[diff_index[run2]];
1674  }
1675  }
1676  if( Dx_u.getDim() != 0 ){
1677  for( run2 = 0; run2 < mu; run2++ ){
1678  Dx_u(run2) = etaH2[control_index[run2]];
1679  }
1680  }
1681  if( Dx_p.getDim() != 0 ){
1682  for( run2 = 0; run2 < mp; run2++ ){
1683  Dx_p(run2) = etaH2[parameter_index[run2]];
1684  }
1685  }
1686  if( Dx_w.getDim() != 0 ){
1687  for( run2 = 0; run2 < mw; run2++ ){
1688  Dx_w(run2) = etaH2[disturbance_index[run2]];
1689  }
1690  }
1691  }
1692 
1693 
1694  if( order == 2 ){
1695 
1696  if( Dx_x0.getDim() != 0 ){
1697  for( run2 = 0; run2 < m; run2++ ){
1698  Dx_x0(run2) = etaH3[diff_index[run2] ];
1699  }
1700  }
1701  if( Dx_u.getDim() != 0 ){
1702  for( run2 = 0; run2 < mu; run2++ ){
1703  Dx_u(run2) = etaH3[control_index[run2]];
1704  }
1705  }
1706  if( Dx_p.getDim() != 0 ){
1707  for( run2 = 0; run2 < mp; run2++ ){
1708  Dx_p(run2) = etaH3[parameter_index[run2]];
1709  }
1710  }
1711  if( Dx_w.getDim() != 0 ){
1712  for( run2 = 0; run2 < mw; run2++ ){
1713  Dx_w(run2) = etaH3[disturbance_index[run2]];
1714  }
1715  }
1716  }
1717 
1718  if( order < 1 || order > 2 ){
1719 
1721  }
1722 
1723  return SUCCESSFUL_RETURN;
1724 }
1725 
1726 
1728 
1729  return count2;
1730 }
1731 
1733 
1734  return count3;
1735 }
1736 
1737 
1739 
1740  return h[0];
1741 }
1742 
1743 
1745 
1746  return SUCCESSFUL_RETURN;
1747 }
1748 
1749 //
1750 // PROTECTED MEMBER FUNCTIONS:
1751 //
1752 
1753 
1755 
1756  int run1, run2, run3;
1757  double E;
1758 
1759  // determine k:
1760  // -----------------------------------------------
1761  for( run1 = 0; run1 < dim; run1++ ){
1762  x[time_index] = t + c[run1]*h[0];
1763  for( run2 = 0; run2 < m; run2++ ){
1764  x[diff_index[run2]] = eta4[run2];
1765  for( run3 = 0; run3 < run1; run3++ ){
1766  x[diff_index[run2]] = x[diff_index[run2]] +
1767  A[run1][run3]*h[0]*k[run3][run2];
1768  }
1769  }
1771 
1772  if( rhs[0].evaluate( 0, x, k[run1] ) != SUCCESSFUL_RETURN ){
1774  return -1.0;
1775  }
1776 
1778  nFcnEvaluations++;
1779  }
1780 
1781  // save previous eta4:
1782  // ----------------------------------------------
1783 
1784  for( run1 = 0; run1 < m; run1++ ){
1785  eta4_[run1] = eta4[run1];
1786  eta5 [run1] = eta4[run1]; // TODO: check if eta4 is correct here!?
1787  //printf( "%e\n",eta4[run1] );
1788  }
1789 
1790  // determine eta4 and eta5:
1791  // ----------------------------------------------
1792 
1793  for( run1 = 0; run1 < dim; run1++ ){
1794  for( run2 = 0; run2 < m; run2++ ){
1795  eta4[run2] = eta4[run2] + b4[run1]*h[0]*k[run1][run2];
1796  eta5[run2] = eta5[run2] + b5[run1]*h[0]*k[run1][run2];
1797  }
1798  }
1799 
1800  // determine local error estimate E:
1801  // ----------------------------------------------
1802 
1803  E = EPS;
1804  for( run2 = 0; run2 < m; run2++ ){
1805  if( (eta4[run2]-eta5[run2])/diff_scale(run2) >= E ){
1806  E = (eta4[run2]-eta5[run2])/diff_scale(run2);
1807  }
1808  if( (eta4[run2]-eta5[run2])/diff_scale(run2) <= -E ){
1809  E = (-eta4[run2]+eta5[run2])/diff_scale(run2);
1810  }
1811  }
1812 
1813  return E;
1814 }
1815 
1816 
1817 
1819 
1820  int run1, run2, run3;
1821  double E;
1822 
1823  // determine k:
1824  // -----------------------------------------------
1825  for( run1 = 0; run1 < dim; run1++ ){
1826  x[time_index] = t + c[run1]*h[0];
1827  for( run2 = 0; run2 < m; run2++ ){
1828  x[diff_index[run2]] = eta4[run2];
1829  for( run3 = 0; run3 < run1; run3++ ){
1830  x[diff_index[run2]] = x[diff_index[run2]] +
1831  A[run1][run3]*h[0]*k[run3][run2];
1832  }
1833  }
1835 
1836  if( rhs[0].evaluate( number_+run1, x, k[run1] ) != SUCCESSFUL_RETURN ){
1838  return -1.0;
1839  }
1840 
1842  nFcnEvaluations++;
1843  }
1844 
1845  // save previous eta4:
1846  // ----------------------------------------------
1847 
1848  for( run1 = 0; run1 < m; run1++ ){
1849  eta4_[run1] = eta4[run1];
1850  eta5 [run1] = eta4[run1];
1851  }
1852 
1853  // determine eta4 and eta5:
1854  // ----------------------------------------------
1855  for( run1 = 0; run1 < dim; run1++ ){
1856  for( run2 = 0; run2 < m; run2++ ){
1857  eta4[run2] = eta4[run2] + b4[run1]*h[0]*k[run1][run2];
1858  eta5[run2] = eta5[run2] + b5[run1]*h[0]*k[run1][run2];
1859  }
1860  }
1861 
1862  // determine local error estimate E:
1863  // ----------------------------------------------
1864 
1865  E = EPS;
1866  for( run2 = 0; run2 < m; run2++ ){
1867  if( (eta4[run2]-eta5[run2])/diff_scale(run2) >= E )
1868  E = (eta4[run2]-eta5[run2])/diff_scale(run2);
1869  if( (eta4[run2]-eta5[run2])/diff_scale(run2) <= -E )
1870  E = (-eta4[run2]+eta5[run2])/diff_scale(run2);
1871  }
1872 
1873  return E;
1874 }
1875 
1876 
1878 
1879  int run1, run2, run3;
1880 
1881  // determine k:
1882  // -----------------------------------------------
1883  for( run1 = 0; run1 < dim; run1++ ){
1884  for( run2 = 0; run2 < m; run2++ ){
1885  G[diff_index[run2]] = etaG[run2];
1886  for( run3 = 0; run3 < run1; run3++ ){
1887  G[diff_index[run2]] = G[diff_index[run2]] +
1888  A[run1][run3]*h[0]*k[run3][run2];
1889  }
1890  }
1891  if( rhs[0].AD_forward( number_+run1, G, k[run1] ) != SUCCESSFUL_RETURN ){
1893  return;
1894  }
1895  }
1896 
1897  // determine etaG:
1898  // ----------------------------------------------
1899  for( run1 = 0; run1 < dim; run1++ ){
1900  for( run2 = 0; run2 < m; run2++ ){
1901  etaG[run2] = etaG[run2] + b4[run1]*h[0]*k[run1][run2];
1902  }
1903  }
1904 }
1905 
1906 
1907 
1909 
1910  int run1, run2, run3;
1911 
1912  // determine k:
1913  // -----------------------------------------------
1914  for( run1 = 0; run1 < dim; run1++ ){
1915  for( run2 = 0; run2 < m; run2++ ){
1916  G2[diff_index[run2]] = etaG2[run2];
1917  G3[diff_index[run2]] = etaG3[run2];
1918  for( run3 = 0; run3 < run1; run3++ ){
1919  G2[diff_index[run2]] = G2[diff_index[run2]] +
1920  A[run1][run3]*h[0]*k[run3][run2];
1921  G3[diff_index[run2]] = G3[diff_index[run2]] +
1922  A[run1][run3]*h[0]*k2[run3][run2];
1923  }
1924  }
1925 
1926  if( rhs[0].AD_forward2( number_+run1, G2, G3, k[run1], k2[run1] )
1927  != SUCCESSFUL_RETURN ){
1929  return;
1930  }
1931  }
1932 
1933  // determine etaG2:
1934  // ----------------------------------------------
1935 
1936  for( run1 = 0; run1 < dim; run1++ ){
1937  for( run2 = 0; run2 < m; run2++ ){
1938  etaG2[run2] = etaG2[run2] + b4[run1]*h[0]*k[run1][run2];
1939  etaG3[run2] = etaG3[run2] + b4[run1]*h[0]*k2[run1][run2];
1940  }
1941  }
1942 }
1943 
1944 
1945 
1947 
1948  int run1, run2, run3;
1949  const int ndir = rhs->getNumberOfVariables() + 1 + m;
1950 
1951  for( run1 = 0; run1 < dim; run1++ ){
1952  for( run2 = 0; run2 < ndir; run2++ ){
1953  l[run1][run2] = 0.0;
1954  }
1955  }
1956  for( run1 = dim-1; run1 >= 0; run1--){
1957  for( run2 = 0; run2 < m; run2++ ){
1958  H[run2] = b4[run1]*h[0]*etaH[diff_index[run2]];
1959  for( run3 = run1+1; run3 < dim; run3++ ){
1960  H[run2] = H[run2] + A[run3][run1]*h[0]*l[run3][diff_index[run2]];
1961  }
1962  }
1963 
1964  if( rhs[0].AD_backward( number_+run1, H, l[run1] )!= SUCCESSFUL_RETURN ){
1966  return;
1967  }
1968  }
1969 
1970  // determine etaH:
1971  // ----------------------------------------------
1972  for( run1 = 0; run1 < dim; run1++ ){
1973  for( run2 = 0; run2 < ndir; run2++ ){
1974  etaH[run2] = etaH[run2] + l[run1][run2];
1975  }
1976  }
1977 }
1978 
1979 
1981 
1982  int run1, run2, run3;
1983  const int ndir = rhs->getNumberOfVariables() + 1 + m;
1984 
1985  for( run1 = 0; run1 < dim; run1++ ){
1986  for( run2 = 0; run2 < ndir; run2++ ){
1987  l [run1][run2] = 0.0;
1988  l2[run1][run2] = 0.0;
1989  }
1990  }
1991 
1992  for( run1 = dim-1; run1 >= 0; run1--){
1993  for( run2 = 0; run2 < m; run2++ ){
1994  H2[run2] = b4[run1]*h[0]*etaH2[diff_index[run2]];
1995  H3[run2] = b4[run1]*h[0]*etaH3[diff_index[run2]];
1996  for( run3 = run1+1; run3 < dim; run3++ ){
1997  H2[run2] = H2[run2] + A[run3][run1]*h[0]*l[run3][diff_index[run2]];
1998  H3[run2] = H3[run2] + A[run3][run1]*h[0]*l2[run3][diff_index[run2]];
1999  }
2000  }
2001  if( rhs[0].AD_backward2( number_+run1, H2, H3, l[run1], l2[run1] )
2002  != SUCCESSFUL_RETURN ){
2004  return;
2005  }
2006  }
2007 
2008  // determine etaH:
2009  // ----------------------------------------------
2010 
2011  for( run1 = 0; run1 < dim; run1++ ){
2012  for( run2 = 0; run2 < ndir; run2++ ){
2013  etaH2[run2] = etaH2[run2] + l [run1][run2];
2014  etaH3[run2] = etaH3[run2] + l2[run1][run2];
2015  }
2016  }
2017 }
2018 
2019 
2021 
2022  int run1, run2;
2023 
2024  if( soa != SOA_EVERYTHING_FROZEN ){
2025  for( run1 = 0; run1 < m; run1++ ){
2026  cout << "x[" << run1 << "] = " << eta4[run1] << " ";
2027  }
2028  cout << endl;
2029  }
2030  else{
2031 
2032  cout << endl;
2033  }
2034 
2035  // Forward Sensitivities:
2036  // ----------------------
2037 
2038  if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ){
2039  cout << "RK: Forward Sensitivities:\n";
2040  for( run1 = 0; run1 < m; run1++ ){
2041  cout << scientific << etaG[run1] << " ";
2042  }
2043  cout << endl;
2044  }
2045 
2046 
2047  // 2nd Order Forward Sensitivities:
2048  // ---------------------------------
2049 
2050  if( nFDirs2 > 0 ){
2051 
2052  cout << "RK: First Order Forward Sensitivities:\n";
2053  for( run1 = 0; run1 < m; run1++ ){
2054  cout << scientific << etaG2[run1] << " ";
2055  }
2056  cout << endl;
2057 
2058  cout << "RK: Second Order Forward Sensitivities:\n";
2059  for( run1 = 0; run1 < m; run1++ ){
2060  cout << scientific << etaG3[run1] << " ";
2061  }
2062  cout << "\n";
2063  }
2064 
2065  // Backward Sensitivities:
2066  // -----------------------
2067 
2068  if( nBDirs > 0 ){
2069 
2070  cout << "RK: Backward Sensitivities:\n" << "w.r.t. the states:\n";
2071  for( run2 = 0; run2 < m; run2++ ){
2072  cout << scientific << etaH[diff_index[run2]] << " ";
2073  }
2074  cout << endl;
2075 
2076  if( mu > 0 ){
2077  cout << "w.r.t. the controls:\n";
2078  for( run2 = 0; run2 < mu; run2++ ){
2079  cout << scientific << etaH[control_index[run2]] << " ";
2080  }
2081  cout << endl;
2082  }
2083  if( mp > 0 ){
2084  cout << "w.r.t. the parameters:\n";
2085  for( run2 = 0; run2 < mp; run2++ ){
2086  cout << scientific << etaH[parameter_index[run2]] << " ";
2087  }
2088  cout << endl;
2089  }
2090  if( mw > 0 ){
2091  cout << "w.r.t. the disturbances:\n";
2092  for( run2 = 0; run2 < mw; run2++ ){
2093  cout << scientific << etaH[disturbance_index[run2]] << " ";
2094  }
2095  cout << endl;
2096  }
2097  }
2098 
2099 
2100  // 2nd order Backward Sensitivities:
2101  // ---------------------------------
2102 
2103  if( nBDirs2 > 0 ){
2104 
2105  cout << "RK: First order Backward Sensitivities:\n";
2106 
2107  cout << "w.r.t. the states:\n";
2108  for( run2 = 0; run2 < m; run2++ ){
2109  cout << scientific << etaH2[diff_index[run2]] << " ";
2110  }
2111  cout << endl;
2112 
2113  if( mu > 0 ){
2114  cout << "w.r.t. the controls:\n";
2115  for( run2 = 0; run2 < mu; run2++ ){
2116  cout << scientific << etaH2[control_index[run2]] << " ";
2117  }
2118  cout << endl;
2119  }
2120  if( mp > 0 ){
2121  cout << "w.r.t. the parameters:\n";
2122  for( run2 = 0; run2 < mp; run2++ ){
2123  cout << scientific << etaH2[parameter_index[run2]] << " ";
2124  }
2125  cout << "\n";
2126  }
2127  if( mw > 0 ){
2128  cout << "w.r.t. the disturbances:\n" << scientific;
2129  for( run2 = 0; run2 < mw; run2++ ){
2130  cout << etaH2[disturbance_index[run2]] << " ";
2131  }
2132  cout << endl;
2133  }
2134 
2135  cout << "RK: Second order Backward Sensitivities:\n";
2136 
2137  cout << "w.r.t. the states:\n" << scientific;;
2138  for( run2 = 0; run2 < m; run2++ ){
2139  cout << etaH3[diff_index[run2]] << " ";
2140  }
2141  cout << endl;
2142 
2143  if( mu > 0 ){
2144  cout << "w.r.t. the controls:\n" << scientific;
2145  for( run2 = 0; run2 < mu; run2++ ){
2146  cout << etaH3[control_index[run2]] << " ";
2147  }
2148  cout << endl;
2149  }
2150 
2151  if( mp > 0 ){
2152  cout << "w.r.t. the parameters:\n";
2153  for( run2 = 0; run2 < mp; run2++ ){
2154  cout << etaH3[parameter_index[run2]] << endl;
2155  }
2156  cout << endl;
2157  }
2158 
2159  if( mw > 0 ){
2160  cout << "w.r.t. the disturbances:\n";
2161  for( run2 = 0; run2 < mw; run2++ ){
2162  cout << etaH3[disturbance_index[run2]] << " ";
2163  }
2164  cout << endl;
2165  }
2166  }
2167 }
2168 
2169 void IntegratorLYAPUNOV::interpolate( int jj, double *e1, double *d1, double *e2, VariablesGrid &poly ){
2170 
2171  int run1;
2172 
2173  for( run1 = 0; run1 < m; run1++ ){
2174 
2175  double cc = e1[run1];
2176  double bb = d1[run1];
2177  double aa = (e2[run1] - bb*h[0] - cc)/(h[0]*h[0]);
2178 
2179  double tt = timeInterval.getTime(jj) - t + h[0];
2180 
2181  poly( jj, run1 ) = aa*tt*tt + bb*tt + cc;
2182  }
2183 }
2184 
2185 
2187 
2188  return m;
2189 }
2190 
2191 
2193 
2194 
2195 // end of file.
returnValue setLast(LogName _name, int lastValue, double time=-INFTY)
int getStateEnumerationIndex(int index_)
virtual returnValue setProtectedBackwardSeed(const DVector &seed, const int &order)
double TOL
Definition: integrator.hpp:645
IntermediateState sqrt(const Expression &arg)
double tune
Definition: integrator.hpp:644
int getNPI() const
Definition: function.cpp:239
virtual returnValue unfreeze()
virtual returnValue setDxInitialization(double *dx0)
void constructAll(const IntegratorLYAPUNOV &arg)
void determineEtaHBackward2(int number)
void determineEtaGForward2(int number)
virtual returnValue freezeMesh()
double getTime(uint pointIdx) const
virtual returnValue step(int number)
int * int_parameter_index
Definition: integrator.hpp:659
virtual int getNumberOfRejectedSteps() const
double getFirstTime() const
int maxNumberOfSteps
Definition: integrator.hpp:666
VariablesGrid dxStore
Definition: integrator.hpp:712
virtual returnValue init(const DifferentialEquation &rhs_)
virtual returnValue freezeAll()
short int m
Definition: integrator.hpp:620
void initializeOptions()
Definition: integrator.cpp:587
BEGIN_NAMESPACE_ACADO const double EPS
int getNDX() const
Definition: function.cpp:217
int * diff_index
Definition: integrator.hpp:653
void init(unsigned _dim=0)
Definition: vector.hpp:155
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Allows to pass back messages to the calling function.
virtual returnValue getProtectedX(DVector *xEnd) const
double hmin
Definition: integrator.hpp:642
AlgorithmicBase & operator=(const AlgorithmicBase &rhs)
int getNUI() const
Definition: function.cpp:227
IntermediateState pow(const Expression &arg1, const Expression &arg2)
int getNU() const
Definition: function.cpp:222
virtual returnValue setProtectedForwardSeed(const DVector &xSeed, const DVector &pSeed, const DVector &uSeed, const DVector &wSeed, const int &order)
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
RealClock functionEvaluation
Definition: integrator.hpp:695
virtual IntegratorLYAPUNOV & operator=(const IntegratorLYAPUNOV &arg)
Grid timeInterval
Definition: integrator.hpp:648
#define CLOSE_NAMESPACE_ACADO
virtual returnValue getProtectedBackwardSensitivities(DVector &Dx_x0, DVector &Dx_p, DVector &Dx_u, DVector &Dx_w, int order) const
int * int_control_index
Definition: integrator.hpp:658
bool isEmpty() const
Definition: vector.hpp:176
int getNXA() const
Definition: function.cpp:212
virtual double getStepSize() const
VariablesGrid ddxStore
Definition: integrator.hpp:713
Abstract base class for all kinds of algorithms for integrating differential equations (ODEs or DAEs)...
Definition: integrator.hpp:61
VariablesGrid xStore
Definition: integrator.hpp:711
Lyapunov getLyapunovObject() const
uint getFloorIndex(double time) const
Definition: grid.cpp:593
int * alg_index
Definition: integrator.hpp:655
Expression p
Definition: lyapunov.hpp:122
int getNW() const
Definition: function.cpp:245
void interpolate(int jj, double *e1, double *d1, double *e2, VariablesGrid &poly)
StateOfAggregation soa
Definition: integrator.hpp:688
Expression x2
Definition: lyapunov.hpp:120
int nFcnEvaluations
Definition: integrator.hpp:696
int getNP() const
Definition: function.cpp:233
short int mu
Definition: integrator.hpp:624
void determineEtaHBackward(int number)
unsigned getDim() const
Definition: vector.hpp:172
int getDim() const
short int mw
Definition: integrator.hpp:628
double * h
Definition: integrator.hpp:640
double scale(VariableType variableType_, int index_) const
Definition: function.cpp:182
#define E
RealClock totalTime
Definition: integrator.hpp:694
returnValue init()
int getN() const
returnValue acadoPrintCopyrightNotice(const std::string &subpackage)
short int mui
Definition: integrator.hpp:625
int * control_index
Definition: integrator.hpp:656
Derived & setZero(Index size)
Expression x1
Definition: lyapunov.hpp:119
int * disturbance_index
Definition: integrator.hpp:660
int index(VariableType variableType_, int index_) const
Definition: function.cpp:176
short int mp
Definition: integrator.hpp:626
PrintLevel
virtual returnValue start()
Definition: real_clock.cpp:78
Expression u
Definition: lyapunov.hpp:121
VariablesGrid iStore
Definition: integrator.hpp:714
#define ASSERT(x)
#define BT_TRUE
Definition: acado_types.hpp:47
void determineEtaGForward(int number)
virtual returnValue setBackwardSeed2(const DVector &seed)
short int mn
Definition: integrator.hpp:623
int * parameter_index
Definition: integrator.hpp:657
virtual returnValue evaluate(const DVector &x0, const DVector &xa, const DVector &p, const DVector &u, const DVector &w, const Grid &t_)
int * ddiff_index
Definition: integrator.hpp:654
virtual returnValue stop()
Definition: real_clock.cpp:107
virtual int getNumberOfSteps() const
double getLastTime() const
#define ACADOWARNING(retval)
#define BEGIN_NAMESPACE_ACADO
virtual returnValue stop()
DVector diff_scale
Definition: integrator.hpp:670
virtual returnValue evaluateSensitivities()
Expression w
Definition: lyapunov.hpp:123
short int ma
Definition: integrator.hpp:621
short int mpi
Definition: integrator.hpp:627
double hmax
Definition: integrator.hpp:643
virtual returnValue printRunTimeProfile() const
Definition: integrator.cpp:624
virtual returnValue getProtectedForwardSensitivities(DMatrix *Dx, int order) const
virtual int getDim() const
double hini
Definition: integrator.hpp:641
returnValue setForwardSeed2(const DVector &xSeed, const DVector &pSeed, const DVector &uSeed, const DVector &wSeed)
Base class for all kinds of Runge-Kutta schemes for integrating ODEs.
int getNumberOfVariables() const
Definition: function.cpp:264
#define ACADOERROR(retval)
virtual returnValue getTime(double &_elapsedTime)
Definition: clock.cpp:92
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
DifferentialEquation * rhs
Definition: integrator.hpp:619


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:42