integrator_lyapunov.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00027 
00034 #include <acado/utils/acado_utils.hpp>
00035 #include <acado/matrix_vector/matrix_vector.hpp>
00036 #include <acado/symbolic_expression/symbolic_expression.hpp>
00037 #include <acado/function/function_.hpp>
00038 #include <acado/function/differential_equation.hpp>
00039 #include <acado/integrator/integrator.hpp>
00040 #include <acado/integrator/integrator_runge_kutta.hpp>
00041 #include <acado/integrator/integrator_lyapunov.hpp>
00042 
00043 using namespace std;
00044 
00045 BEGIN_NAMESPACE_ACADO
00046 
00047 
00048 //
00049 // PUBLIC MEMBER FUNCTIONS:
00050 //
00051 
00052 IntegratorLYAPUNOV::IntegratorLYAPUNOV( )
00053              :Integrator( ){
00054 
00055     initializeVariables();
00056 }
00057 
00058 
00059 IntegratorLYAPUNOV::IntegratorLYAPUNOV( int dim_ , double power_)
00060              :Integrator( ){
00061 
00062     int run1;
00063 
00064     initializeVariables();
00065     dim       = dim_  ;
00066     err_power = power_;
00067 
00068     // BUTCHER TABLEAU:
00069     // ----------------
00070     A  = new double*[dim];
00071     b4 = new double [dim];
00072     b5 = new double [dim];
00073     c  = new double [dim];
00074 
00075     for( run1 = 0; run1 < dim; run1++ ){
00076         A[run1] = new double[dim];
00077     }
00078 }
00079 
00080 
00081 IntegratorLYAPUNOV::IntegratorLYAPUNOV( const DifferentialEquation& rhs_, int dim_, double power_ )
00082              :Integrator( ){
00083 
00084     int run1;
00085 
00086     dim       = dim_  ;
00087     err_power = power_;
00088 
00089     // BUTCHER TABLEAU:
00090     // ----------------
00091     A  = new double*[dim];
00092     b4 = new double [dim];
00093     b5 = new double [dim];
00094     c  = new double [dim];
00095 
00096     for( run1 = 0; run1 < dim; run1++ ){
00097         A[run1] = new double[dim];
00098     }
00099 
00100     init( rhs_ );
00101 }
00102 
00103 
00104 IntegratorLYAPUNOV::IntegratorLYAPUNOV( const IntegratorLYAPUNOV& arg )
00105              :Integrator( arg ){
00106 
00107     constructAll( arg );
00108 }
00109 
00110 
00111 IntegratorLYAPUNOV::~IntegratorLYAPUNOV( ){
00112 
00113     deleteAll();
00114 }
00115 
00116 
00117 returnValue IntegratorLYAPUNOV::init( const DifferentialEquation &rhs_ ){
00118 
00119     lyap = rhs_.getLyapunovObject();
00120     dimxmy=(lyap.x1).getDim()+(lyap.x2).getDim()+(lyap.x1).getDim()*(lyap.x1).getDim();
00121     dimu=(lyap.u).getDim();
00122     dimp=(lyap.p).getDim();
00123     dimw=(lyap.w).getDim();
00124 
00125     //printf("dimxmy=%d dimu=%d dimp=%d dimw=%d m=%d \n", dimxmy, dimu, dimp, dimw,m);
00126     rhs = new DifferentialEquation( rhs_ );
00127     m   = rhs->getDim ();
00128     //m   = dimxmy;
00129     ma  = 0;
00130     mn  = rhs->getN   ();
00131     mu  = rhs->getNU  ();
00132     mui = rhs->getNUI ();
00133     mp  = rhs->getNP  ();
00134     mpi = rhs->getNPI ();
00135     mw  = rhs->getNW  ();
00136 
00137     //printf("m=%d mn=%d mu=%d mui=%d mp=%d mpi=%d mw=%d \n",m, mn, mu, mui, mp, mpi, mw);
00138     allocateMemory();
00139 
00140     return SUCCESSFUL_RETURN;
00141 }
00142 
00143 
00144 void IntegratorLYAPUNOV::initializeVariables(){
00145 
00146     dim = 0; A = 0; b4 = 0; b5 = 0; c = 0;
00147     eta4 = 0; eta5 = 0; eta4_ = 0; eta5_ = 0;
00148     k = 0; k2 = 0; l = 0; l2 = 0; x = 0;
00149 
00150     G = 0; etaG = 0;
00151     G2 = 0; G3 = 0; etaG2 = 0; etaG3 = 0;
00152 
00153     H = 0; etaH = 0; H2 = 0; H3 = 0;
00154     etaH2 = 0; etaH3 = 0;
00155 
00156     maxAlloc  = 0;
00157     err_power = 1.0;
00158 
00159     dimxmy = 0;
00160     dimu = 0;
00161     dimp = 0; 
00162     dimw = 0; 
00163     Y = 0;    
00164     seedmy = 0; 
00165 }
00166 
00167 
00168 void IntegratorLYAPUNOV::allocateMemory( ){
00169 
00170     int run1, run2;
00171 
00172     if( 0 != rhs->getNXA() ){
00173         ACADOERROR(RET_RK45_CAN_NOT_TREAT_DAE);
00174         ASSERT(1 == 0);
00175     }
00176 
00177     if( 0 != rhs->getNDX() ){
00178         ACADOERROR(RET_RK45_CAN_NOT_TREAT_DAE);
00179         ASSERT(1 == 0);
00180     }
00181 
00182 
00183     if( m < 1 ){
00184         ACADOERROR(RET_TRIVIAL_RHS);
00185         ASSERT(1 == 0);
00186     }
00187 
00188     // RK-ALGORITHM:
00189     // -------------
00190     eta4  = new double [m];
00191     eta5  = new double [m];
00192     eta4_ = new double [m];
00193     eta5_ = new double [m];
00194 
00195     for( run1 = 0; run1 < m; run1++ ){
00196 
00197         eta4 [run1] = 0.0;
00198         eta5 [run1] = 0.0;
00199         eta4_[run1] = 0.0;
00200         eta5_[run1] = 0.0;
00201     }
00202 
00203     k     = new double*[dim];
00204     k2    = new double*[dim];
00205     l     = new double*[dim];
00206     l2    = new double*[dim];
00207     x     = new double [rhs->getNumberOfVariables() + 1 + m];
00208 
00209     for( run1 = 0; run1 < rhs->getNumberOfVariables() + 1 + m; run1++ ){
00210         x[run1] = 0.0;
00211     }
00212 
00213     t     = 0.0;
00214 
00215 
00216     for( run1 = 0; run1 < dim; run1++ ){
00217         k    [run1] = new double[m];
00218         k2   [run1] = new double[m];
00219 
00220         for( run2 = 0; run2 < m; run2++ ){
00221             k [run1][run2] = 0.0;
00222             k2[run1][run2] = 0.0;
00223         }
00224 
00225         l    [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
00226         l2   [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
00227 
00228         for( run2 = 0; run2 < rhs->getNumberOfVariables() + 1 + m; run2++ ){
00229             l [run1][run2] = 0.0;
00230             l2[run1][run2] = 0.0;
00231         }
00232     }
00233 
00234     // INTERNAL INDEX LISTS:
00235     // ---------------------
00236     diff_index = new int[m];
00237 
00238     for( run1 = 0; run1 < m; run1++ ){
00239         diff_index[run1] = rhs->getStateEnumerationIndex( run1 );
00240         if( diff_index[run1] == rhs->getNumberOfVariables() ){
00241             diff_index[run1] = diff_index[run1] + 1 + run1;
00242         }
00243     }
00244 
00245     control_index       = new int[mu ];
00246 
00247     for( run1 = 0; run1 < mu; run1++ ){
00248         control_index[run1] = rhs->index( VT_CONTROL, run1 );
00249     }
00250 
00251     parameter_index     = new int[mp ];
00252 
00253     for( run1 = 0; run1 < mp; run1++ ){
00254         parameter_index[run1] = rhs->index( VT_PARAMETER, run1 );
00255     }
00256 
00257     int_control_index   = new int[mui];
00258 
00259     for( run1 = 0; run1 < mui; run1++ ){
00260         int_control_index[run1] = rhs->index( VT_INTEGER_CONTROL, run1 );
00261     }
00262 
00263     int_parameter_index = new int[mpi];
00264 
00265     for( run1 = 0; run1 < mpi; run1++ ){
00266         int_parameter_index[run1] = rhs->index( VT_INTEGER_PARAMETER, run1 );
00267     }
00268 
00269     disturbance_index   = new int[mw ];
00270 
00271     for( run1 = 0; run1 < mw; run1++ ){
00272         disturbance_index[run1] = rhs->index( VT_DISTURBANCE, run1 );
00273     }
00274 
00275     time_index = rhs->index( VT_TIME, 0 );
00276 
00277     diff_scale.init(m);
00278     for( run1 = 0; run1 < m; run1++ )
00279         diff_scale(run1) = rhs->scale( VT_DIFFERENTIAL_STATE, run1 );
00280 
00281 
00282     // SENSITIVITIES:
00283     // --------------
00284     G          = NULL;
00285     etaG       = NULL;
00286 
00287     G2         = NULL;
00288     G3         = NULL;
00289     etaG2      = NULL;
00290     etaG3      = NULL;
00291 
00292     H          = NULL;
00293     etaH       = NULL;
00294 
00295     H2         = NULL;
00296     H3         = NULL;
00297     etaH2      = NULL;
00298     etaH3      = NULL;
00299 
00300 
00301     // STORAGE:
00302     // --------
00303     maxAlloc = 1;
00304 
00305 
00306     // Derivative  matrix + seedvector
00307     Y     = new double*[dimxmy+dimu+dimp+dimw];
00308     seedmy     = new double[dimxmy+dimu+dimp+dimw];
00309 
00310     for( run1 = 0; run1 < rhs->getNumberOfVariables() + 1 + m; run1++ ){
00311         x[run1] = 0.0;
00312     }
00313 
00314     for( run1 = 0; run1 < dimxmy+dimu+dimp+dimw; run1++ ){
00315         Y[run1] = new double[dimxmy+dimu+dimp+dimw];
00316     }
00317 
00318 }
00319 
00320 
00321 
00322 void IntegratorLYAPUNOV::deleteAll(){
00323 
00324     int run1;
00325 
00326 
00327     // BUTCHER-
00328     // TABLEAU:
00329     // ----------
00330     for( run1 = 0; run1 < dim; run1++ ){
00331         delete[] A[run1];
00332     }
00333 
00334     delete[] A;
00335     delete[] b4;
00336     delete[] b5;
00337     delete[] c ;
00338 
00339 
00340     // RK-ALGORITHM:
00341     // -------------
00342     if( eta4 != NULL ){
00343         delete[] eta4;
00344     }
00345     if( eta4_ != NULL ){
00346         delete[] eta4_;
00347     }
00348     if( eta5 != NULL ){
00349         delete[] eta5;
00350     }
00351     if( eta5_ != NULL ){
00352         delete[] eta5_;
00353     }
00354 
00355     for( run1 = 0; run1 < dim; run1++ ){
00356       if( k[run1]  != NULL )
00357           delete[] k[run1] ;
00358       if( k2[run1] != NULL )
00359           delete[] k2[run1];
00360       if( l[run1]  != NULL )
00361           delete[] l[run1] ;
00362       if( l2[run1] != NULL )
00363           delete[] l2[run1];
00364     }
00365 
00366     if( k != NULL )
00367         delete[] k ;
00368 
00369     if( k2!= NULL )
00370         delete[] k2;
00371 
00372     if( l != NULL )
00373         delete[] l ;
00374 
00375     if( l2!= NULL )
00376         delete[] l2;
00377 
00378     if( x != NULL )
00379         delete[] x;
00380 
00381 
00382     // SENSITIVITIES:
00383     // ----------------------------------------
00384 
00385     if( G  != NULL )
00386         delete[] G;
00387 
00388     if( etaG  != NULL )
00389         delete[] etaG;
00390 
00391     if( G2  != NULL )
00392         delete[] G2;
00393 
00394     if( G3  != NULL )
00395         delete[] G3;
00396 
00397     if( etaG2  != NULL )
00398         delete[] etaG2;
00399 
00400     if( etaG3  != NULL )
00401         delete[] etaG3;
00402 
00403 
00404     // ----------------------------------------
00405 
00406     if( H  != NULL )
00407         delete[] H;
00408 
00409     if( etaH  != NULL )
00410         delete[] etaH;
00411 
00412     if( H2  != NULL )
00413         delete[] H2;
00414 
00415     if( H3  != NULL )
00416         delete[] H3;
00417 
00418     if( etaH2  != NULL )
00419         delete[] etaH2;
00420 
00421     if( etaH3  != NULL )
00422         delete[] etaH3;
00423 
00424 
00425     for( run1 = 0; run1 < dimxmy+dimu+dimp+dimw; run1++ ){
00426       if( Y[run1]  != NULL )
00427           delete[] Y[run1] ;
00428     }
00429 
00430     if( Y != NULL )
00431         delete[] Y ;
00432 
00433     if( seedmy != NULL )
00434         delete[] seedmy ;
00435 
00436 }
00437 
00438 
00439 IntegratorLYAPUNOV& IntegratorLYAPUNOV::operator=( const IntegratorLYAPUNOV& arg ){
00440 
00441     if ( this != &arg ){
00442         deleteAll();
00443         Integrator::operator=( arg );
00444         constructAll( arg );
00445     }
00446 
00447     return *this;
00448 }
00449 
00450 
00451 void IntegratorLYAPUNOV::constructAll( const IntegratorLYAPUNOV& arg ){
00452 
00453     int run1, run2;
00454 
00455     rhs = new DifferentialEquation( *arg.rhs );
00456 
00457     m   = arg.m              ;
00458     ma  = arg.ma             ;
00459     mn  = arg.mn             ;
00460     mu  = arg.mu             ;
00461     mui = arg.mui            ;
00462     mp  = arg.mp             ;
00463     mpi = arg.mpi            ;
00464     mw  = arg.mw             ;
00465 
00466 
00467     if( m < 1 ){
00468         ACADOERROR(RET_TRIVIAL_RHS);
00469         ASSERT(1 == 0);
00470     }
00471 
00472     // BUTCHER TABLEAU:
00473     // ----------------
00474     dim = arg.dim;
00475     A  = new double*[dim];
00476     b4 = new double [dim];
00477     b5 = new double [dim];
00478     c  = new double [dim];
00479 
00480     for( run1 = 0; run1 < dim; run1++ ){
00481 
00482         b4[run1] = arg.b4[run1];
00483         b5[run1] = arg.b5[run1];
00484         c [run1] = arg.c [run1];
00485 
00486         A[run1] = new double[dim];
00487         for( run2 = 0; run2 < dim; run2++ )
00488             A[run1][run2] = arg.A[run1][run2];
00489     }
00490 
00491     // RK-ALGORITHM:
00492     // -------------
00493     eta4  = new double [m];
00494     eta5  = new double [m];
00495     eta4_ = new double [m];
00496     eta5_ = new double [m];
00497 
00498     for( run1 = 0; run1 < m; run1++ ){
00499 
00500         eta4 [run1] = arg.eta4 [run1];
00501         eta5 [run1] = arg.eta5 [run1];
00502         eta4_[run1] = arg.eta4_[run1];
00503         eta5_[run1] = arg.eta5_[run1];
00504     }
00505 
00506     k     = new double*[dim];
00507     k2    = new double*[dim];
00508     l     = new double*[dim];
00509     l2    = new double*[dim];
00510     x     = new double [rhs->getNumberOfVariables() + 1 + m];
00511 
00512     for( run1 = 0; run1 < rhs->getNumberOfVariables() + 1 + m; run1++ ){
00513         x[run1] = arg.x[run1];
00514     }
00515 
00516     t     = arg.t;
00517 
00518 
00519     for( run1 = 0; run1 < dim; run1++ ){
00520         k    [run1] = new double[m];
00521         k2   [run1] = new double[m];
00522 
00523         for( run2 = 0; run2 < m; run2++ ){
00524             k [run1][run2] = arg.k [run1][run2];
00525             k2[run1][run2] = arg.k2[run1][run2];
00526         }
00527 
00528         l    [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
00529         l2   [run1] = new double[rhs->getNumberOfVariables() + 1 + m];
00530 
00531         for( run2 = 0; run2 < rhs->getNumberOfVariables() + 1 + m; run2++ ){
00532             l [run1][run2] = arg.l [run1][run2];
00533             l2[run1][run2] = arg.l2[run1][run2];
00534         }
00535     }
00536 
00537 
00538     // SETTINGS:
00539     // ---------
00540     h    = (double*)calloc(arg.maxAlloc,sizeof(double));
00541     for( run1 = 0; run1 < arg.maxAlloc; run1++ ){
00542        h[run1] = arg.h[run1];
00543     }
00544     hini = arg.hini;
00545     hmin = arg.hmin;
00546     hmax = arg.hmax;
00547 
00548     tune  = arg.tune;
00549     TOL   = arg.TOL;
00550 
00551     err_power = arg.err_power;
00552 
00553 
00554     // INTERNAL INDEX LISTS:
00555     // ---------------------
00556     diff_index = new int[m];
00557 
00558     for( run1 = 0; run1 < m; run1++ ){
00559         diff_index[run1] = arg.diff_index[run1];
00560     }
00561 
00562     ddiff_index = 0;
00563     alg_index   = 0;
00564 
00565     control_index       = new int[mu ];
00566 
00567     for( run1 = 0; run1 < mu; run1++ ){
00568         control_index[run1] = arg.control_index[run1];
00569     }
00570 
00571     parameter_index     = new int[mp ];
00572 
00573     for( run1 = 0; run1 < mp; run1++ ){
00574         parameter_index[run1] = arg.parameter_index[run1];
00575     }
00576 
00577     int_control_index   = new int[mui];
00578 
00579     for( run1 = 0; run1 < mui; run1++ ){
00580         int_control_index[run1] = arg.int_control_index[run1];
00581     }
00582 
00583     int_parameter_index = new int[mpi];
00584 
00585     for( run1 = 0; run1 < mpi; run1++ ){
00586         int_parameter_index[run1] = arg.int_parameter_index[run1];
00587     }
00588 
00589     disturbance_index   = new int[mw ];
00590 
00591     for( run1 = 0; run1 < mw; run1++ ){
00592         disturbance_index[run1] = arg.disturbance_index[run1];
00593     }
00594 
00595     time_index = arg.time_index;
00596 
00597 
00598     // OTHERS:
00599     // -------
00600     maxNumberOfSteps = arg.maxNumberOfSteps;
00601     count            = arg.count           ;
00602     count2           = arg.count2          ;
00603     count3           = arg.count3          ;
00604 
00605     diff_scale = arg.diff_scale;
00606 
00607 
00608     // PRINT-LEVEL:
00609     // ------------
00610     PrintLevel = arg.PrintLevel;
00611 
00612 
00613     // SENSITIVITIES:
00614     // ---------------
00615     nFDirs     = 0   ;
00616     nBDirs     = 0   ;
00617 
00618     nFDirs2    = 0   ;
00619     nBDirs2    = 0   ;
00620 
00621     G          = NULL;
00622     etaG       = NULL;
00623 
00624     G2         = NULL;
00625     G3         = NULL;
00626     etaG2      = NULL;
00627     etaG3      = NULL;
00628 
00629     H          = NULL;
00630     etaH       = NULL;
00631 
00632     H2         = NULL;
00633     H3         = NULL;
00634     etaH2      = NULL;
00635     etaH3      = NULL;
00636 
00637 
00638     // THE STATE OF AGGREGATION:
00639     // -------------------------
00640     soa        = arg.soa;
00641 
00642 
00643     // STORAGE:
00644     // --------
00645     maxAlloc = arg.maxAlloc;
00646 
00647 
00648     // DERIVATIVES  MATRIX + SEEDVECTOR
00649     lyap = arg.lyap          ;
00650     dimxmy = arg.dimxmy      ;
00651     dimu = arg.dimu          ;
00652     dimp = arg.dimp          ;
00653     dimw = arg.dimw          ;
00654  
00655     Y     = new double*[dimxmy+dimu+dimp+dimw];
00656     seedmy     = new double[dimxmy+dimu+dimp+dimw];
00657 
00658     for( run1 = 0; run1 < dimxmy+dimu+dimp+dimw; run1++ ){
00659         Y[run1] = new double[dimxmy+dimu+dimp+dimw];
00660     }
00661  
00662     for( run1 = 0; run1 < dimxmy+dimu+dimp+dimw; run1++ ){
00663       seedmy[run1] = arg.seedmy[run1];
00664       for( run2 = 0; run2 < dimxmy+dimu+dimp+dimw; run2++ ){
00665         Y[run1][run2] = arg.Y[run1][run2];
00666       }
00667     }
00668 
00669 }
00670 
00671 
00672 returnValue IntegratorLYAPUNOV::freezeMesh(){
00673 
00674 
00675     if( soa != SOA_UNFROZEN ){
00676        if( PrintLevel != NONE ){
00677            return ACADOWARNING(RET_ALREADY_FROZEN);
00678        }
00679        return RET_ALREADY_FROZEN;
00680     }
00681 
00682     soa = SOA_FREEZING_MESH;
00683     return SUCCESSFUL_RETURN;
00684 }
00685 
00686 
00687 
00688 returnValue IntegratorLYAPUNOV::freezeAll(){
00689 
00690     if( soa != SOA_UNFROZEN ){
00691        if( PrintLevel != NONE ){
00692            return ACADOWARNING(RET_ALREADY_FROZEN);
00693        }
00694        return RET_ALREADY_FROZEN;
00695     }
00696 
00697     soa = SOA_FREEZING_ALL;
00698     return SUCCESSFUL_RETURN;
00699 }
00700 
00701 
00702 returnValue IntegratorLYAPUNOV::unfreeze(){
00703 
00704     maxAlloc = 1;
00705     h = (double*)realloc(h,maxAlloc*sizeof(double));
00706     soa = SOA_UNFROZEN;
00707 
00708     return SUCCESSFUL_RETURN;
00709 }
00710 
00711 
00712 returnValue IntegratorLYAPUNOV::evaluate( const DVector &x0  ,
00713                                     const DVector &xa  ,
00714                                     const DVector &p   ,
00715                                     const DVector &u   ,
00716                                     const DVector &w   ,
00717                                     const Grid   &t_    ){
00718 
00719     int         run1;
00720     returnValue returnvalue;
00721 
00722     if( rhs == NULL ){
00723         return ACADOERROR(RET_TRIVIAL_RHS);
00724     }
00725 
00726     if( xa.getDim() != 0 )
00727         ACADOWARNING(RET_RK45_CAN_NOT_TREAT_DAE);
00728 
00729 
00730     Integrator::initializeOptions();
00731 
00732     timeInterval  = t_;
00733 
00734     xStore.init(  m, timeInterval );
00735     iStore.init( mn, timeInterval );
00736 
00737     t             = timeInterval.getFirstTime();
00738     x[time_index] = timeInterval.getFirstTime();
00739 //      printf("initial integrator t = %e\n", t );
00740 //      printf("initial integrator x[time_index] = %e\n", x[time_index] );
00741 //      printf(" with time_index = %d\n", time_index );
00742 
00743     if( soa != SOA_MESH_FROZEN && soa != SOA_MESH_FROZEN_FREEZING_ALL && soa != SOA_EVERYTHING_FROZEN  ){
00744        h[0] = hini;
00745 
00746        if( timeInterval.getLastTime() - timeInterval.getFirstTime() - h[0] < EPS ){
00747            h[0] = timeInterval.getLastTime() - timeInterval.getFirstTime();
00748 
00749            if( h[0] < 10.0*EPS )
00750                return ACADOERROR(RET_TO_SMALL_OR_NEGATIVE_TIME_INTERVAL);
00751        }
00752     }
00753 
00754     if( x0.isEmpty() == BT_TRUE ) return ACADOERROR(RET_MISSING_INPUTS);
00755 
00756 
00757     if( (int) x0.getDim() < m )
00758       {
00759         printf("x0dim = %d m=%d \n",(int) x0.getDim(), m);
00760         return ACADOERROR(RET_INPUT_HAS_WRONG_DIMENSION);
00761       }
00762 
00763     for( run1 = 0; run1 < m; run1++ ){
00764         eta4[run1]     = x0(run1);
00765         eta5[run1]     = x0(run1);
00766         xStore(0,run1) = x0(run1);
00767     }
00768 
00769     if( nFDirs != 0 ){
00770         for( run1 = 0; run1 < m; run1++ ){
00771             etaG[run1] = fseed(diff_index[run1]);
00772         }
00773     }
00774 
00775     if( mp > 0 ){
00776         if( (int) p.getDim() < mp )
00777             return ACADOERROR(RET_INPUT_HAS_WRONG_DIMENSION);
00778 
00779         for( run1 = 0; run1 < mp; run1++ ){
00780             x[parameter_index[run1]] = p(run1);
00781         }
00782     }
00783 
00784     if( mu > 0 ){
00785         if( (int) u.getDim() < mu )
00786             return ACADOERROR(RET_INPUT_HAS_WRONG_DIMENSION);
00787 
00788         for( run1 = 0; run1 < mu; run1++ ){
00789             x[control_index[run1]] = u(run1);
00790         }
00791     }
00792 
00793 
00794     if( mw > 0 ){
00795         if( (int) w.getDim() < mw )
00796             return ACADOERROR(RET_INPUT_HAS_WRONG_DIMENSION);
00797 
00798         for( run1 = 0; run1 < mw; run1++ ){
00799             x[disturbance_index[run1]] = w(run1);
00800         }
00801     }
00802 
00803 
00804     totalTime.start();
00805     nFcnEvaluations = 0;
00806 
00807 
00808      // Initialize the scaling based on the initial states:
00809      // ---------------------------------------------------
00810 
00811         double atol;
00812         get( ABSOLUTE_TOLERANCE, atol );
00813 
00814         for( run1 = 0; run1 < m; run1++ )
00815             diff_scale(run1) = fabs(eta4[run1]) + atol/TOL;
00816 
00817 
00818      // PRINTING:
00819      // ---------
00820         if( PrintLevel == HIGH || PrintLevel == MEDIUM ){
00821             acadoPrintCopyrightNotice( "IntegratorLYAPUNOV -- A Runge Kutta integrator." );
00822         }
00823         if( PrintLevel == HIGH ){
00824                 cout << "RK: t = " << t << "\t";
00825 
00826             for( run1 = 0; run1 < m; run1++ ){
00827                 cout << "x[" << run1 << "] = " << scientific << eta4[run1] << "  ";
00828             }
00829             cout << endl;
00830         }
00831 
00832 
00833     returnvalue = RET_FINAL_STEP_NOT_PERFORMED_YET;
00834 
00835     count3 = 0;
00836     count  = 1;
00837 
00838     while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET && count <= maxNumberOfSteps ){
00839 
00840         returnvalue = step(count);
00841         count++;
00842     }
00843 
00844     count2 = count-1;
00845 
00846     for( run1 = 0; run1 < mn; run1++ )
00847         iStore( 0, run1 ) = iStore( 1, run1 );
00848 
00849     totalTime.stop();
00850 
00851     if( count > maxNumberOfSteps ){
00852         if( PrintLevel != NONE )
00853             return ACADOERROR(RET_MAX_NUMBER_OF_STEPS_EXCEEDED);
00854         return RET_MAX_NUMBER_OF_STEPS_EXCEEDED;
00855     }
00856 
00857 
00858     // SET THE LOGGING INFORMATION:
00859     // ----------------------------------------------------------------------------------------
00860 
00861        setLast( LOG_TIME_INTEGRATOR                              , totalTime.getTime()           );
00862        setLast( LOG_NUMBER_OF_INTEGRATOR_STEPS                   , count-1                       );
00863        setLast( LOG_NUMBER_OF_INTEGRATOR_REJECTED_STEPS          , getNumberOfRejectedSteps()    );
00864        setLast( LOG_NUMBER_OF_INTEGRATOR_FUNCTION_EVALUATIONS    , nFcnEvaluations               );
00865        setLast( LOG_NUMBER_OF_BDF_INTEGRATOR_JACOBIAN_EVALUATIONS, 0                             );
00866        setLast( LOG_TIME_INTEGRATOR_FUNCTION_EVALUATIONS         , functionEvaluation.getTime()  );
00867        setLast( LOG_TIME_BDF_INTEGRATOR_JACOBIAN_EVALUATION      , 0.0                           );
00868        setLast( LOG_TIME_BDF_INTEGRATOR_JACOBIAN_DECOMPOSITION   , 0.0                           );
00869 
00870     // ----------------------------------------------------------------------------------------
00871 
00872 
00873      // PRINTING:
00874      // ---------
00875         if( PrintLevel == MEDIUM ){
00876 
00877             if( soa == SOA_EVERYTHING_FROZEN ){
00878                 cout << "\n Results at  t = " << t << "\t";
00879 
00880                 for( run1 = 0; run1 < m; run1++ ){
00881                         cout << "x[" << run1 << "] = " << scientific << eta4[ run1 ];
00882                 }
00883                 cout << endl;
00884             }
00885             printIntermediateResults();
00886         }
00887         
00888         int printIntegratorProfile = 0;
00889         get( PRINT_INTEGRATOR_PROFILE,printIntegratorProfile );
00890         
00891         if ( (BooleanType)printIntegratorProfile == BT_TRUE )
00892         {
00893                 printRunTimeProfile( );
00894         }
00895         else
00896         {
00897                 if( PrintLevel == MEDIUM  || PrintLevel == HIGH )
00898                         cout << "RK: number of steps:  " << count - 1 << endl;
00899         }
00900 
00901     return returnvalue;
00902 }
00903 
00904 
00905 
00906 returnValue IntegratorLYAPUNOV::setProtectedForwardSeed( const DVector &xSeed,
00907                                                    const DVector &pSeed,
00908                                                    const DVector &uSeed,
00909                                                    const DVector &wSeed,
00910                                                    const int    &order  ){
00911 
00912     //float aa;
00913     if( order == 2 ){
00914         return setForwardSeed2( xSeed, pSeed, uSeed, wSeed);
00915     }
00916     if( order < 1 || order > 2 ){
00917         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
00918     }
00919 
00920     if( nBDirs > 0 ){
00921         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
00922     }
00923 
00924     int run2;
00925 
00926     if( G  != NULL ){
00927         delete[] G;
00928         G = NULL;
00929     }
00930 
00931     if( etaG  != NULL ){
00932         delete[] etaG;
00933         etaG = NULL;
00934     }
00935 
00936     nFDirs = 1;
00937 
00938     fseed.init(rhs->getNumberOfVariables()+1+m);
00939     fseed.setZero();
00940 
00941     G = new double[rhs->getNumberOfVariables() + 1 + m];
00942 
00943     for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++ ){
00944         G[run2] = 0.0;
00945     }
00946 
00947     etaG = new double[m];
00948 
00949     if( xSeed.getDim() != 0 ){
00950         for( run2 = 0; run2 < m; run2++ ){
00951             fseed(diff_index[run2]) = xSeed(run2);
00952             //printf("state i=%d fseed=%f \n",diff_index[run2],fseed(diff_index[run2]));
00953         }
00954     }
00955 
00956     if( pSeed.getDim() != 0 ){
00957         for( run2 = 0; run2 < mp; run2++ ){
00958              fseed(parameter_index[run2]) = pSeed(run2);
00959              G    [parameter_index[run2]] = pSeed(run2);
00960              //printf("parameters i=%d fseed=%f  \n",parameter_index[run2],fseed(parameter_index[run2]));
00961          }
00962     }
00963 
00964     if( uSeed.getDim() != 0 ){
00965         for( run2 = 0; run2 < mu; run2++ ){
00966             fseed(control_index[run2]) = uSeed(run2);
00967             G    [control_index[run2]] = uSeed(run2);
00968             //printf("controls i=%d fseed=%f  \n",control_index[run2],fseed(control_index[run2]));
00969         }
00970     }
00971 
00972     if( wSeed.getDim() != 0 ){
00973 
00974         for( run2 = 0; run2 < mw; run2++ ){
00975             fseed(disturbance_index[run2]) = wSeed(run2);
00976                 G[disturbance_index[run2]] = wSeed(run2);
00977                 //printf("disturbance i=%d fseed=%f  \n",disturbance_index[run2],fseed(disturbance_index[run2]));
00978        }
00979     }
00980     //scanf("%f  ",&aa);
00981 
00982     return SUCCESSFUL_RETURN;
00983 }
00984 
00985 
00986 returnValue IntegratorLYAPUNOV::setForwardSeed2( const DVector &xSeed ,
00987                                            const DVector &pSeed ,
00988                                            const DVector &uSeed ,
00989                                            const DVector &wSeed   ){
00990 
00991     int run2;
00992 
00993     if( G2  != NULL ){
00994         delete[] G2;
00995         G2 = NULL;
00996     }
00997 
00998     if( G3  != NULL ){
00999         delete[] G3;
01000         G3 = NULL;
01001     }
01002 
01003     if( etaG2  != NULL ){
01004         delete[] etaG2;
01005         etaG2 = NULL;
01006     }
01007 
01008     if( etaG3  != NULL ){
01009         delete[] etaG3;
01010         etaG3 = NULL;
01011     }
01012 
01013     nFDirs2 = 1;
01014 
01015     fseed2.init(rhs->getNumberOfVariables() + 1 + m);
01016     fseed2.setZero();
01017 
01018     G2 = new double[rhs->getNumberOfVariables() + 1 + m];
01019     G3 = new double[rhs->getNumberOfVariables() + 1 + m];
01020 
01021     for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++ ){
01022          G2[run2] = 0.0;
01023          G3[run2] = 0.0;
01024     }
01025     etaG2 = new double[m];
01026     etaG3 = new double[m];
01027 
01028     if( xSeed.getDim() != 0 ){
01029         for( run2 = 0; run2 < m; run2++ ){
01030             fseed2(diff_index[run2]) = xSeed(run2);
01031         }
01032     }
01033 
01034     if( pSeed.getDim() != 0 ){
01035         for( run2 = 0; run2 < mp; run2++ ){
01036             fseed2(parameter_index[run2]) = pSeed(run2);
01037             G2    [parameter_index[run2]] = pSeed(run2);
01038         }
01039     }
01040 
01041     if( uSeed.getDim() != 0 ){
01042         for( run2 = 0; run2 < mu; run2++ ){
01043              fseed2(control_index[run2]) = uSeed(run2);
01044              G2    [control_index[run2]] = uSeed(run2);
01045         }
01046     }
01047 
01048     if( wSeed.getDim() != 0 ){
01049         for( run2 = 0; run2 < mw; run2++ ){
01050             fseed2(disturbance_index[run2]) = wSeed(run2);
01051             G2    [disturbance_index[run2]] = wSeed(run2);
01052         }
01053     }
01054     return SUCCESSFUL_RETURN;
01055 }
01056 
01057 
01058 returnValue IntegratorLYAPUNOV::setProtectedBackwardSeed( const DVector &seed, const int &order ){
01059 
01060     if( order == 2 ){
01061         return setBackwardSeed2(seed);
01062     }
01063     if( order < 1 || order > 2 ){
01064         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
01065     }
01066 
01067     if( nFDirs > 0 ){
01068         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
01069     }
01070 
01071     int run2;
01072 
01073     if( H  != NULL ){
01074         delete[] H;
01075         H = NULL;
01076     }
01077 
01078     if( etaH  != NULL ){
01079         delete[] etaH;
01080         etaH = NULL;
01081     }
01082 
01083     nBDirs = 1;
01084 
01085     bseed.init( m );
01086     bseed.setZero();
01087 
01088     H = new double[m];
01089 
01090     etaH = new double[rhs->getNumberOfVariables()+1+m];
01091     for( run2 = 0; run2 < rhs->getNumberOfVariables()+1+m; run2++ ){
01092         etaH[run2] = 0.0;
01093     }
01094 
01095     if( seed.getDim() != 0 ){
01096         for( run2 = 0; run2 < m; run2++ ){
01097             bseed(run2) = seed(run2);
01098         }
01099     }
01100 
01101     return SUCCESSFUL_RETURN;
01102 }
01103 
01104 
01105 
01106 returnValue IntegratorLYAPUNOV::setBackwardSeed2( const DVector &seed ){
01107 
01108     int run2;
01109 
01110     if( H2  != NULL ){
01111         delete[] H2;
01112         H2 = NULL;
01113     }
01114 
01115     if( H3  != NULL ){
01116         delete[] H3;
01117         H3 = NULL;
01118     }
01119 
01120     if( etaH2  != NULL ){
01121         delete[] etaH2;
01122         etaH2 = NULL;
01123     }
01124 
01125     if( etaH3  != NULL ){
01126         delete[] etaH3;
01127         etaH3 = NULL;
01128     }
01129 
01130 
01131     nBDirs2 = 1;
01132 
01133     bseed2.init(m);
01134     bseed2.setZero();
01135 
01136     H2 = new double[m];
01137     H3 = new double[m];
01138 
01139     etaH2 = new double[rhs->getNumberOfVariables()+1+m];
01140     etaH3 = new double[rhs->getNumberOfVariables()+1+m];
01141 
01142     if( seed.getDim() != 0 ){
01143          for( run2 = 0; run2 < m; run2++ ){
01144              bseed2(run2) = seed(run2);
01145          }
01146     }
01147 
01148     return SUCCESSFUL_RETURN;
01149 }
01150 
01151 
01152 returnValue IntegratorLYAPUNOV::evaluateSensitivities(){
01153 
01154     int         run1, run2 ;
01155     returnValue returnvalue;
01156 
01157     if( rhs == NULL ){
01158         return ACADOERROR(RET_TRIVIAL_RHS);
01159     }
01160 
01161     if( soa != SOA_EVERYTHING_FROZEN ){
01162         return ACADOERROR(RET_NOT_FROZEN);
01163     }
01164 
01165 
01166     if( nFDirs != 0 ){
01167         t = timeInterval.getFirstTime();
01168         dxStore.init( m, timeInterval );
01169         for( run1 = 0; run1 < m; run1++ ){
01170             etaG[run1] = fseed(diff_index[run1]);
01171         }
01172     }
01173 
01174     if( nBDirs != 0 ){
01175         for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++){
01176             etaH[run2] = 0.0;
01177         }
01178     }
01179 
01180     if( nBDirs != 0 ){
01181         for( run1 = 0; run1 < m; run1++ ){
01182             etaH[diff_index[run1]] = bseed(run1);
01183         }
01184     }
01185 
01186     if( nFDirs2 != 0 ){
01187         t = timeInterval.getFirstTime();
01188         ddxStore.init( m, timeInterval );
01189         for( run1 = 0; run1 < m; run1++ ){
01190             etaG2[run1] = fseed2(diff_index[run1]);
01191             etaG3[run1] = 0.0;
01192         }
01193     }
01194 
01195     if( nBDirs2 != 0 ){
01196         for( run2 = 0; run2 < (rhs->getNumberOfVariables()+1+m); run2++){
01197             etaH2[run2] = 0.0;
01198             etaH3[run2] = 0.0;
01199         }
01200     }
01201 
01202     if( nBDirs2 != 0 ){
01203         for( run1 = 0; run1 < m; run1++ ){
01204             etaH2[diff_index[run1]] = bseed2(run1);
01205         }
01206     }
01207 
01208     if( PrintLevel == HIGH ){
01209         printIntermediateResults();
01210     }
01211 
01212     returnvalue = RET_FINAL_STEP_NOT_PERFORMED_YET;
01213 
01214 
01215     if( nBDirs > 0 || nBDirs2 > 0 ){
01216 
01217         int oldCount = count;
01218 
01219         count--;
01220         while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET && count >= 1 ){
01221 
01222             returnvalue = step( count );
01223             count--;
01224         }
01225 
01226         if( count == 0 && (returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET ||
01227                            returnvalue == SUCCESSFUL_RETURN   )            ){
01228 
01229 
01230             if( PrintLevel == MEDIUM ){
01231                 printIntermediateResults();
01232             }
01233             count = oldCount;
01234 
01235             return SUCCESSFUL_RETURN;
01236         }
01237         count = oldCount;
01238     }
01239     else{
01240 
01241     double a_my=0.0, b_my=0.0, c_my=0.0;//, aa;
01242     int counter_my=0;
01243   
01244     for( run2 = 0; run2 < (int)(lyap.x1).getDim(); run2++ ){
01245       a_my += etaG[run2];
01246       if (etaG[run2] >= 1.0)
01247         counter_my=run2;
01248     }
01249     b_my=a_my;
01250     for( run2 = (lyap.x1).getDim(); run2 < (int)((lyap.x1).getDim()+(lyap.x2).getDim()); run2++ ){
01251       b_my += etaG[run2];
01252       if (etaG[run2] >= 1.0)
01253         counter_my=run2;
01254     }
01255     c_my=b_my;
01256     for( run2 = (lyap.x1).getDim()+(lyap.x2).getDim(); run2 < m; run2++ ){
01257       c_my += etaG[run2];
01258       if (etaG[run2] >= 1.0)
01259         counter_my=run2;
01260     }
01261     //printf("a_my=%f b_my=%f c_my=%f counter_my=%d \n",a_my,b_my,c_my,counter_my);
01262 
01263 
01264     if (b_my>=1.0 || c_my<=0.0){
01265         count = 1;
01266         while( returnvalue == RET_FINAL_STEP_NOT_PERFORMED_YET &&
01267                count <= maxNumberOfSteps ){
01268 
01269             returnvalue = step(count);
01270             count++;
01271         }
01272        if( a_my>= 1.0){
01273            for( run2 = 0; run2 < m; run2++ ){
01274              Y[run2][counter_my]=etaG[run2];
01275              //printf("Y[%d][%d]=%f ",run2,counter_my,Y[run2][counter_my]);
01276            }
01277            //scanf("%f",&aa);
01278        }
01279     }
01280     else{
01281       int dimxmy12=(lyap.x1).getDim()+(lyap.x2).getDim();
01282       int counter_my1=counter_my-dimxmy12;
01283       counter_my1 = counter_my1/(lyap.x1).getDim();
01284       int counter_my2 = counter_my-dimxmy12-counter_my1*(lyap.x1).getDim();
01285       for( run2 = 0; run2 < dimxmy12; run2++ )
01286         etaG[run2]=0.0;
01287           int run4, run5;
01288       for( run4 = 0; run4 < (int)(lyap.x1).getDim(); run4++ )
01289        for( run5 = 0; run5 < (int)(lyap.x1).getDim(); run5++ )
01290 
01291           etaG[dimxmy12+(run4)*(lyap.x1).getDim()+run5] = Y[run4][counter_my1]*Y[run5][counter_my2];
01292           //etaG[dimxmy12+(run4)*4+run1] = Y[run4][counter_my1]*Y[run1][counter_my2];
01293       //printf("counter_my=%d counter_my1=%d counter_my2=%d  \n",counter_my,counter_my1,counter_my2);
01294            //for( run2 = 0; run2 < m; run2++ ){
01295              //printf("etaG[%d]=%f ",run2,etaG[run2]);
01296            //}
01297            //scanf("%f",&aa);
01298 
01299            returnvalue = SUCCESSFUL_RETURN;
01300     }
01301 
01302         if( nBDirs2 == 0 && nFDirs != 0 )
01303             for( run1 = 0; run1 < m; run1++ )
01304                 dxStore( 0, run1 ) = dxStore( 1, run1 );
01305 
01306         if( nFDirs2 != 0 )
01307             for( run1 = 0; run1 < m; run1++ )
01308                 ddxStore( 0, run1 ) = ddxStore( 1, run1 );
01309 
01310         if( count > maxNumberOfSteps ){
01311             if( PrintLevel != NONE )
01312                 return ACADOERROR(RET_MAX_NUMBER_OF_STEPS_EXCEEDED);
01313             return RET_MAX_NUMBER_OF_STEPS_EXCEEDED;
01314         }
01315 
01316         if( PrintLevel == MEDIUM ){
01317             printIntermediateResults();
01318         }
01319     }
01320     return returnvalue;
01321 }
01322 
01323 
01324 returnValue IntegratorLYAPUNOV::step(int number_){
01325 
01326     int run1;
01327     double E = EPS;
01328 
01329     if( soa == SOA_EVERYTHING_FROZEN || soa == SOA_MESH_FROZEN || soa == SOA_MESH_FROZEN_FREEZING_ALL ){
01330         h[0] = h[number_];
01331     }
01332 
01333     if( soa == SOA_FREEZING_MESH ||
01334         soa == SOA_MESH_FROZEN   ||
01335         soa == SOA_MESH_FROZEN_FREEZING_ALL ||
01336         soa == SOA_UNFROZEN      ){
01337 
01338         if( nFDirs > 0 ){
01339             E = determineEta45(0);
01340         }
01341         else{
01342             E = determineEta45();
01343         }
01344     }
01345     if( soa == SOA_FREEZING_ALL ){
01346         E = determineEta45(dim*number_);
01347     }
01348 
01349 
01350     if( soa != SOA_EVERYTHING_FROZEN && soa != SOA_MESH_FROZEN && soa != SOA_MESH_FROZEN_FREEZING_ALL ){
01351 
01352         int number_of_rejected_steps = 0;
01353 
01354         if( E < 0.0 ){
01355             return ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01356         }
01357 
01358         // REJECT THE STEP IF GIVEN TOLERANCE IS NOT ACHIEVED:
01359         // ---------------------------------------------------
01360         while( E >= TOL*h[0] ){
01361 
01362             if( PrintLevel == HIGH ){
01363                 cout << "STEP REJECTED: error estimate           = " << scientific << E << endl
01364                          << "               required local tolerance = " << TOL * h[0] << endl;
01365             }
01366 
01367             number_of_rejected_steps++;
01368 
01369             for( run1 = 0; run1 < m; run1++ ){
01370                 eta4[run1] = eta4_[run1];
01371             }
01372             if( h[0] <= hmin + EPS ){
01373                 return ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01374             }
01375             h[0] = 0.5*h[0];
01376             if( h[0] < hmin ){
01377                 h[0] = hmin;
01378             }
01379 
01380             if( soa == SOA_FREEZING_MESH ||
01381                 soa == SOA_UNFROZEN      ){
01382 
01383                 if( nFDirs > 0 ){
01384                     E = determineEta45(0);
01385                 }
01386                 else{
01387                     E = determineEta45();
01388                 }
01389             }
01390             if( soa == SOA_FREEZING_ALL ){
01391                 E = determineEta45(dim*number_);
01392             }
01393 
01394             if( E < 0.0 ){
01395                 return ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01396             }
01397         }
01398 
01399         count3 += number_of_rejected_steps;
01400     }
01401 
01402     // PROCEED IF THE STEP IS ACCEPTED:
01403     // --------------------------------
01404 
01405        double *etaG_  = new double[m];
01406        double *etaG3_ = new double[m];
01407 
01408 
01409      // compute forward derivatives if requested:
01410      // ------------------------------------------
01411 
01412      if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ){
01413 
01414          for( run1 = 0; run1 < m; run1++ )
01415              etaG_[run1]  = etaG[run1];
01416 
01417          if( nBDirs != 0 ){
01418              return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
01419          }
01420 
01421          if( soa == SOA_FREEZING_ALL || soa == SOA_EVERYTHING_FROZEN ){
01422              determineEtaGForward(dim*number_);
01423          }
01424          else{
01425              determineEtaGForward(0);
01426          }
01427      }
01428      if( nBDirs > 0 ){
01429 
01430          if( soa != SOA_EVERYTHING_FROZEN ){
01431              return ACADOERROR(RET_NOT_FROZEN);
01432          }
01433          if( nFDirs != 0 || nBDirs2 != 0 || nFDirs2 != 0 ){
01434              return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
01435          }
01436          determineEtaHBackward(dim*number_);
01437      }
01438      if( nFDirs2 > 0 ){
01439 
01440          for( run1 = 0; run1 < m; run1++ )
01441              etaG3_[run1]  = etaG3[run1];
01442 
01443          if( soa != SOA_EVERYTHING_FROZEN ){
01444              return ACADOERROR(RET_NOT_FROZEN);
01445          }
01446          if( nBDirs != 0 || nBDirs2 != 0 || nFDirs != 1 ){
01447              return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
01448          }
01449          determineEtaGForward2(dim*number_);
01450      }
01451      if( nBDirs2 > 0 ){
01452 
01453          if( soa != SOA_EVERYTHING_FROZEN ){
01454              return ACADOERROR(RET_NOT_FROZEN);
01455          }
01456          if( nBDirs != 0 || nFDirs2 != 0 || nFDirs != 1 ){
01457              return ACADOERROR(RET_WRONG_DEFINITION_OF_SEEDS);
01458          }
01459 
01460          determineEtaHBackward2(dim*number_);
01461      }
01462 
01463 
01464      // increase the time:
01465      // ----------------------------------------------
01466 
01467      if( nBDirs > 0 || nBDirs2 > 0 ){
01468 
01469          t = t - h[0];
01470      }
01471      else{
01472 
01473          t = t + h[0];
01474      }
01475 //      printf("t = %e\n",t );
01476 
01477      // PRINTING:
01478      // ---------
01479      if( PrintLevel == HIGH ){
01480          cout << "RK: t = " << scientific << t << "  h = " << h[ 0 ] << "  ";
01481          printIntermediateResults();
01482      }
01483 
01484 
01485      // STORAGE:
01486      // --------
01487 
01488      if( soa == SOA_FREEZING_MESH || soa == SOA_FREEZING_ALL || soa == SOA_MESH_FROZEN_FREEZING_ALL ){
01489 
01490          if( number_ >= maxAlloc){
01491 
01492              maxAlloc = 2*maxAlloc;
01493              h = (double*)realloc(h,maxAlloc*sizeof(double));
01494          }
01495          h[number_] = h[0];
01496      }
01497 
01498      int i1 = timeInterval.getFloorIndex( t-h[0] );
01499      int i2 = timeInterval.getFloorIndex( t      );
01500      int jj;
01501 
01502      for( jj = i1+1; jj <= i2; jj++ ){
01503 
01504          if( nFDirs == 0 && nBDirs  == 0 && nFDirs2 == 0 && nBDirs == 0 ) interpolate( jj, eta4_ , k[0], eta4 ,   xStore );
01505          if( nFDirs  > 0 && nBDirs2 == 0 && nFDirs2 == 0                ) interpolate( jj, etaG_ , k[0], etaG ,  dxStore );
01506          if( nFDirs2 > 0                                                ) interpolate( jj, etaG3_, k[0], etaG3, ddxStore );
01507 
01508          for( run1 = 0; run1 < mn; run1++ )
01509              iStore( jj, run1 ) = x[rhs->index( VT_INTERMEDIATE_STATE, run1 )];
01510      }
01511 
01512      delete[] etaG_ ;
01513      delete[] etaG3_;
01514 
01515 
01516      if( nBDirs == 0 || nBDirs2 == 0 ){
01517 
01518      // Stop the algorithm if  t >= te:
01519      // ----------------------------------------------
01520         if( t >= timeInterval.getLastTime() - EPS ){
01521             x[time_index] = timeInterval.getLastTime();
01522             for( run1 = 0; run1 < m; run1++ ){
01523                 x[diff_index[run1]] = eta4[run1];
01524             }
01525 
01526             if( soa == SOA_FREEZING_MESH ){
01527                 soa = SOA_MESH_FROZEN;
01528             }
01529             if( soa == SOA_FREEZING_ALL || soa == SOA_MESH_FROZEN_FREEZING_ALL ){
01530                 soa = SOA_EVERYTHING_FROZEN;
01531             }
01532 
01533             return SUCCESSFUL_RETURN;
01534         }
01535      }
01536 
01537 
01538      if( soa != SOA_EVERYTHING_FROZEN && soa != SOA_MESH_FROZEN && soa != SOA_MESH_FROZEN_FREEZING_ALL ){
01539 
01540 
01541      // recompute the scaling based on the actual states:
01542      // -------------------------------------------------
01543 
01544         double atol;
01545         get( ABSOLUTE_TOLERANCE, atol );
01546 
01547         for( run1 = 0; run1 < m; run1++ )
01548             diff_scale(run1) = fabs(eta4[run1]) + atol/TOL;
01549 
01550 
01551 
01552      // apply a numeric stabilization of the step size control:
01553      // -------------------------------------------------------
01554         double Emin = 1e-3*sqrt(TOL)*pow(hini, ((1.0/err_power)+1.0)/2.0 );
01555 
01556         if( E < Emin     ) E = Emin    ;
01557         if( E < 10.0*EPS ) E = 10.0*EPS;
01558 
01559 
01560      // determine the new step size:
01561      // ----------------------------------------------
01562         h[0] = h[0]*pow( tune*(TOL*h[0]/E), err_power );
01563 
01564         if( h[0] > hmax ){
01565           h[0] = hmax;
01566         }
01567         if( h[0] < hmin ){
01568           h[0] = hmin;
01569         }
01570 
01571         if( t + h[0] >= timeInterval.getLastTime() ){
01572           h[0] = timeInterval.getLastTime()-t;
01573         }
01574     }
01575 
01576     return RET_FINAL_STEP_NOT_PERFORMED_YET;
01577 }
01578 
01579 
01580 
01581 returnValue IntegratorLYAPUNOV::stop(){
01582 
01583     return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
01584 }
01585 
01586 
01587 returnValue IntegratorLYAPUNOV::getProtectedX( DVector *xEnd ) const{
01588 
01589     int run1;
01590 
01591     if( (int) xEnd[0].getDim() != m )
01592         return RET_INPUT_HAS_WRONG_DIMENSION;
01593 
01594     for( run1 = 0; run1 < m; run1++ )
01595         xEnd[0](run1) = eta4[run1];
01596 
01597     return SUCCESSFUL_RETURN;
01598 }
01599 
01600 
01601 returnValue IntegratorLYAPUNOV::getProtectedForwardSensitivities( DMatrix *Dx, int order ) const{
01602 
01603     int run1;
01604     //double aa;
01605 
01606     if( Dx == NULL ){
01607         return SUCCESSFUL_RETURN;
01608     }
01609 
01610     if( order == 1 && nFDirs2 == 0 ){
01611         for( run1 = 0; run1 < m; run1++ ){
01612             Dx[0](run1,0) = etaG[run1];
01613             //printf("Dx[0](%d,0)=%f \n",run1,Dx[0](run1,0));
01614         }
01615         //scanf("%f",&aa);
01616     }
01617 
01618     if( order == 2 ){
01619         for( run1 = 0; run1 < m; run1++ ){
01620             Dx[0](run1,0) = etaG3[run1];
01621         }
01622     }
01623 
01624     if( order == 1 && nFDirs2 > 0 ){
01625         for( run1 = 0; run1 < m; run1++ ){
01626             Dx[0](run1,0) = etaG2[run1];
01627         }
01628     }
01629 
01630     if( order < 1 || order > 2 ){
01631         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
01632     }
01633 
01634     return SUCCESSFUL_RETURN;
01635 }
01636 
01637 
01638 returnValue IntegratorLYAPUNOV::getProtectedBackwardSensitivities( DVector &Dx_x0,
01639                                                              DVector &Dx_p ,
01640                                                              DVector &Dx_u ,
01641                                                              DVector &Dx_w ,
01642                                                              int order      ) const{
01643 
01644     int run2;
01645 
01646     if( order == 1 && nBDirs2 == 0 ){
01647 
01648         if( Dx_x0.getDim() != 0 ){
01649             for( run2 = 0; run2 < m; run2++ )
01650                 Dx_x0(run2) = etaH[diff_index[run2]];
01651         }
01652         if( Dx_p.getDim() != 0 ){
01653             for( run2 = 0; run2 < mp; run2++ ){
01654                 Dx_p(run2) = etaH[parameter_index[run2]];
01655             }
01656         }
01657         if( Dx_u.getDim() != 0 ){
01658             for( run2 = 0; run2 < mu; run2++ ){
01659                 Dx_u(run2) = etaH[control_index[run2]];
01660             }
01661         }
01662         if( Dx_w.getDim() != 0 ){
01663             for( run2 = 0; run2 < mw; run2++ ){
01664                 Dx_w(run2) = etaH[disturbance_index[run2]];
01665             }
01666         }
01667     }
01668 
01669     if( order == 1 && nBDirs2 > 0 ){
01670 
01671         if( Dx_x0.getDim() != 0 ){
01672             for( run2 = 0; run2 < m; run2++ ){
01673                 Dx_x0(run2) = etaH2[diff_index[run2]];
01674             }
01675         }
01676         if( Dx_u.getDim() != 0 ){
01677             for( run2 = 0; run2 < mu; run2++ ){
01678                 Dx_u(run2) = etaH2[control_index[run2]];
01679             }
01680         }
01681         if( Dx_p.getDim() != 0 ){
01682             for( run2 = 0; run2 < mp; run2++ ){
01683                 Dx_p(run2) = etaH2[parameter_index[run2]];
01684             }
01685         }
01686         if( Dx_w.getDim() != 0 ){
01687             for( run2 = 0; run2 < mw; run2++ ){
01688                  Dx_w(run2) = etaH2[disturbance_index[run2]];
01689             }
01690         }
01691     }
01692 
01693 
01694     if( order == 2 ){
01695 
01696         if( Dx_x0.getDim() != 0 ){
01697             for( run2 = 0; run2 < m; run2++ ){
01698                 Dx_x0(run2) = etaH3[diff_index[run2]   ];
01699             }
01700         }
01701         if( Dx_u.getDim() != 0 ){
01702             for( run2 = 0; run2 < mu; run2++ ){
01703                Dx_u(run2) = etaH3[control_index[run2]];
01704             }
01705         }
01706         if( Dx_p.getDim() != 0 ){
01707             for( run2 = 0; run2 < mp; run2++ ){
01708                  Dx_p(run2) = etaH3[parameter_index[run2]];
01709             }
01710         }
01711         if( Dx_w.getDim() != 0 ){
01712             for( run2 = 0; run2 < mw; run2++ ){
01713                  Dx_w(run2) = etaH3[disturbance_index[run2]];
01714             }
01715         }
01716     }
01717 
01718     if( order < 1 || order > 2 ){
01719 
01720         return ACADOERROR(RET_INPUT_OUT_OF_RANGE);
01721     }
01722 
01723     return SUCCESSFUL_RETURN;
01724 }
01725 
01726 
01727 int IntegratorLYAPUNOV::getNumberOfSteps() const{
01728 
01729     return count2;
01730 }
01731 
01732 int IntegratorLYAPUNOV::getNumberOfRejectedSteps() const{
01733 
01734     return count3;
01735 }
01736 
01737 
01738 double IntegratorLYAPUNOV::getStepSize() const{
01739 
01740     return h[0];
01741 }
01742 
01743 
01744 returnValue IntegratorLYAPUNOV::setDxInitialization( double *dx0 ){
01745 
01746     return SUCCESSFUL_RETURN;
01747 }
01748 
01749 //
01750 // PROTECTED MEMBER FUNCTIONS:
01751 //
01752 
01753 
01754 double IntegratorLYAPUNOV::determineEta45(){
01755 
01756     int run1, run2, run3;
01757     double E;
01758 
01759     // determine k:
01760     // -----------------------------------------------
01761        for( run1 = 0; run1 < dim; run1++ ){
01762            x[time_index] = t + c[run1]*h[0];
01763            for( run2 = 0; run2 < m; run2++ ){
01764                x[diff_index[run2]] = eta4[run2];
01765                for( run3 = 0; run3 < run1; run3++ ){
01766                    x[diff_index[run2]] = x[diff_index[run2]] +
01767                                          A[run1][run3]*h[0]*k[run3][run2];
01768                }
01769            }
01770            functionEvaluation.start();
01771 
01772            if( rhs[0].evaluate( 0, x, k[run1] ) != SUCCESSFUL_RETURN ){
01773                ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01774                return -1.0;
01775            }
01776 
01777            functionEvaluation.stop();
01778            nFcnEvaluations++;
01779        }
01780 
01781     // save previous eta4:
01782     // ----------------------------------------------
01783 
01784        for( run1 = 0; run1 < m; run1++ ){
01785            eta4_[run1]  = eta4[run1];
01786            eta5 [run1]  = eta4[run1]; // TODO: check if eta4 is correct here!?
01787                    //printf( "%e\n",eta4[run1] );
01788        }
01789 
01790     // determine eta4 and eta5:
01791     // ----------------------------------------------
01792 
01793        for( run1 = 0; run1 < dim; run1++ ){
01794            for( run2 = 0; run2 < m; run2++ ){
01795                eta4[run2] = eta4[run2] + b4[run1]*h[0]*k[run1][run2];
01796                eta5[run2] = eta5[run2] + b5[run1]*h[0]*k[run1][run2];
01797            }
01798        }
01799 
01800     // determine local error estimate E:
01801     // ----------------------------------------------
01802 
01803        E = EPS;
01804        for( run2 = 0; run2 < m; run2++ ){
01805            if( (eta4[run2]-eta5[run2])/diff_scale(run2) >= E  ){
01806                E = (eta4[run2]-eta5[run2])/diff_scale(run2);
01807            }
01808            if( (eta4[run2]-eta5[run2])/diff_scale(run2) <= -E ){
01809                E = (-eta4[run2]+eta5[run2])/diff_scale(run2);
01810            }
01811        }
01812 
01813     return E;
01814 }
01815 
01816 
01817 
01818 double IntegratorLYAPUNOV::determineEta45( int number_ ){
01819 
01820     int run1, run2, run3;
01821     double E;
01822 
01823     // determine k:
01824     // -----------------------------------------------
01825        for( run1 = 0; run1 < dim; run1++ ){
01826            x[time_index] = t + c[run1]*h[0];
01827            for( run2 = 0; run2 < m; run2++ ){
01828                x[diff_index[run2]] = eta4[run2];
01829                for( run3 = 0; run3 < run1; run3++ ){
01830                    x[diff_index[run2]] = x[diff_index[run2]] +
01831                                          A[run1][run3]*h[0]*k[run3][run2];
01832                }
01833            }
01834            functionEvaluation.start();
01835 
01836            if( rhs[0].evaluate( number_+run1, x, k[run1] ) != SUCCESSFUL_RETURN ){
01837                ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01838                return -1.0;
01839            }
01840 
01841            functionEvaluation.stop();
01842            nFcnEvaluations++;
01843        }
01844 
01845     // save previous eta4:
01846     // ----------------------------------------------
01847 
01848        for( run1 = 0; run1 < m; run1++ ){
01849            eta4_[run1]  = eta4[run1];
01850            eta5 [run1]  = eta4[run1];
01851        }
01852 
01853     // determine eta4 and eta5:
01854     // ----------------------------------------------
01855        for( run1 = 0; run1 < dim; run1++ ){
01856            for( run2 = 0; run2 < m; run2++ ){
01857                eta4[run2] = eta4[run2] + b4[run1]*h[0]*k[run1][run2];
01858                eta5[run2] = eta5[run2] + b5[run1]*h[0]*k[run1][run2];
01859            }
01860        }
01861 
01862     // determine local error estimate E:
01863     // ----------------------------------------------
01864 
01865        E = EPS;
01866        for( run2 = 0; run2 < m; run2++ ){
01867            if( (eta4[run2]-eta5[run2])/diff_scale(run2) >= E  )
01868                E = (eta4[run2]-eta5[run2])/diff_scale(run2);
01869            if( (eta4[run2]-eta5[run2])/diff_scale(run2) <= -E )
01870                E = (-eta4[run2]+eta5[run2])/diff_scale(run2);
01871        }
01872 
01873     return E;
01874 }
01875 
01876 
01877 void IntegratorLYAPUNOV::determineEtaGForward( int number_ ){
01878 
01879     int run1, run2, run3;
01880 
01881     // determine k:
01882     // -----------------------------------------------
01883        for( run1 = 0; run1 < dim; run1++ ){
01884            for( run2 = 0; run2 < m; run2++ ){
01885                G[diff_index[run2]] = etaG[run2];
01886                for( run3 = 0; run3 < run1; run3++ ){
01887                    G[diff_index[run2]] = G[diff_index[run2]] +
01888                                                A[run1][run3]*h[0]*k[run3][run2];
01889                }
01890            }
01891            if( rhs[0].AD_forward( number_+run1, G, k[run1] ) != SUCCESSFUL_RETURN ){
01892                ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01893                return;
01894            }
01895        }
01896 
01897     // determine etaG:
01898     // ----------------------------------------------
01899        for( run1 = 0; run1 < dim; run1++ ){
01900            for( run2 = 0; run2 < m; run2++ ){
01901                etaG[run2] = etaG[run2] + b4[run1]*h[0]*k[run1][run2];
01902            }
01903        }
01904 }
01905 
01906 
01907 
01908 void IntegratorLYAPUNOV::determineEtaGForward2( int number_ ){
01909 
01910     int run1, run2, run3;
01911 
01912     // determine k:
01913     // -----------------------------------------------
01914        for( run1 = 0; run1 < dim; run1++ ){
01915            for( run2 = 0; run2 < m; run2++ ){
01916                G2[diff_index[run2]] = etaG2[run2];
01917                G3[diff_index[run2]] = etaG3[run2];
01918                for( run3 = 0; run3 < run1; run3++ ){
01919                    G2[diff_index[run2]] = G2[diff_index[run2]] +
01920                                                 A[run1][run3]*h[0]*k[run3][run2];
01921                    G3[diff_index[run2]] = G3[diff_index[run2]] +
01922                                                 A[run1][run3]*h[0]*k2[run3][run2];
01923                }
01924            }
01925 
01926            if( rhs[0].AD_forward2( number_+run1, G2, G3, k[run1], k2[run1] )
01927                != SUCCESSFUL_RETURN ){
01928                ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01929                return;
01930            }
01931        }
01932 
01933     // determine etaG2:
01934     // ----------------------------------------------
01935 
01936        for( run1 = 0; run1 < dim; run1++ ){
01937            for( run2 = 0; run2 < m; run2++ ){
01938                etaG2[run2] = etaG2[run2] + b4[run1]*h[0]*k[run1][run2];
01939                etaG3[run2] = etaG3[run2] + b4[run1]*h[0]*k2[run1][run2];
01940            }
01941        }
01942 }
01943 
01944 
01945 
01946 void IntegratorLYAPUNOV::determineEtaHBackward( int number_ ){
01947 
01948     int run1, run2, run3;
01949     const int ndir = rhs->getNumberOfVariables() + 1 + m;
01950 
01951         for( run1 = 0; run1 < dim; run1++ ){
01952             for( run2 = 0; run2 < ndir; run2++ ){
01953                 l[run1][run2] = 0.0;
01954             }
01955         }
01956         for( run1 = dim-1; run1 >= 0; run1--){
01957              for( run2 = 0; run2 < m; run2++ ){
01958                  H[run2] = b4[run1]*h[0]*etaH[diff_index[run2]];
01959                  for( run3 = run1+1; run3 < dim; run3++ ){
01960                       H[run2] = H[run2] + A[run3][run1]*h[0]*l[run3][diff_index[run2]];
01961                  }
01962              }
01963 
01964              if( rhs[0].AD_backward( number_+run1, H, l[run1] )!= SUCCESSFUL_RETURN ){
01965                  ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
01966                  return;
01967              }
01968         }
01969 
01970     // determine etaH:
01971     // ----------------------------------------------
01972        for( run1 = 0; run1 < dim; run1++ ){
01973            for( run2 = 0; run2 < ndir; run2++ ){
01974                etaH[run2] = etaH[run2] + l[run1][run2];
01975            }
01976        }
01977 }
01978 
01979 
01980 void IntegratorLYAPUNOV::determineEtaHBackward2( int number_ ){
01981 
01982     int run1, run2, run3;
01983     const int ndir = rhs->getNumberOfVariables() + 1 + m;
01984 
01985     for( run1 = 0; run1 < dim; run1++ ){
01986         for( run2 = 0; run2 < ndir; run2++ ){
01987             l [run1][run2] = 0.0;
01988             l2[run1][run2] = 0.0;
01989         }
01990     }
01991 
01992     for( run1 = dim-1; run1 >= 0; run1--){
01993          for( run2 = 0; run2 < m; run2++ ){
01994              H2[run2] = b4[run1]*h[0]*etaH2[diff_index[run2]];
01995              H3[run2] = b4[run1]*h[0]*etaH3[diff_index[run2]];
01996              for( run3 = run1+1; run3 < dim; run3++ ){
01997                   H2[run2] = H2[run2] + A[run3][run1]*h[0]*l[run3][diff_index[run2]];
01998                   H3[run2] = H3[run2] + A[run3][run1]*h[0]*l2[run3][diff_index[run2]];
01999              }
02000          }
02001          if( rhs[0].AD_backward2( number_+run1, H2, H3, l[run1], l2[run1] )
02002              != SUCCESSFUL_RETURN ){
02003              ACADOERROR(RET_UNSUCCESSFUL_RETURN_FROM_INTEGRATOR_RK45);
02004               return;
02005          }
02006     }
02007 
02008     // determine etaH:
02009     // ----------------------------------------------
02010 
02011     for( run1 = 0; run1 < dim; run1++ ){
02012         for( run2 = 0; run2 < ndir; run2++ ){
02013             etaH2[run2] = etaH2[run2] + l [run1][run2];
02014             etaH3[run2] = etaH3[run2] + l2[run1][run2];
02015         }
02016     }
02017 }
02018 
02019 
02020 void IntegratorLYAPUNOV::printIntermediateResults(){
02021 
02022     int run1, run2;
02023 
02024         if( soa != SOA_EVERYTHING_FROZEN ){
02025             for( run1 = 0; run1 < m; run1++ ){
02026                 cout << "x[" << run1 << "] = " << eta4[run1] << "  ";
02027             }
02028             cout << endl;
02029         }
02030         else{
02031 
02032             cout << endl;
02033         }
02034 
02035         // Forward Sensitivities:
02036         // ----------------------
02037 
02038         if( nFDirs > 0 && nBDirs2 == 0 && nFDirs2 == 0 ){
02039             cout << "RK: Forward Sensitivities:\n";
02040             for( run1 = 0; run1 < m; run1++ ){
02041                 cout << scientific << etaG[run1] << "  ";
02042             }
02043             cout << endl;
02044         }
02045 
02046 
02047         // 2nd Order Forward Sensitivities:
02048         // ---------------------------------
02049 
02050         if( nFDirs2 > 0 ){
02051 
02052             cout << "RK: First Order Forward Sensitivities:\n";
02053             for( run1 = 0; run1 < m; run1++ ){
02054                 cout << scientific << etaG2[run1] << "  ";
02055             }
02056             cout << endl;
02057 
02058             cout << "RK: Second Order Forward Sensitivities:\n";
02059             for( run1 = 0; run1 < m; run1++ ){
02060                 cout << scientific << etaG3[run1] << "  ";
02061             }
02062             cout << "\n";
02063         }
02064 
02065         // Backward Sensitivities:
02066         // -----------------------
02067 
02068         if( nBDirs > 0 ){
02069 
02070             cout << "RK: Backward Sensitivities:\n" << "w.r.t. the states:\n";
02071             for( run2 = 0; run2 < m; run2++ ){
02072                 cout << scientific << etaH[diff_index[run2]] << "  ";
02073             }
02074             cout << endl;
02075 
02076             if( mu > 0 ){
02077                 cout << "w.r.t. the controls:\n";
02078                 for( run2 = 0; run2 < mu; run2++ ){
02079                         cout << scientific << etaH[control_index[run2]] << "  ";
02080                 }
02081                 cout << endl;
02082             }
02083             if( mp > 0 ){
02084                 cout << "w.r.t. the parameters:\n";
02085                 for( run2 = 0; run2 < mp; run2++ ){
02086                         cout << scientific << etaH[parameter_index[run2]] << "  ";
02087                 }
02088                 cout << endl;
02089             }
02090             if( mw > 0 ){
02091                 cout << "w.r.t. the disturbances:\n";
02092                 for( run2 = 0; run2 < mw; run2++ ){
02093                         cout << scientific << etaH[disturbance_index[run2]] << "  ";
02094                 }
02095                 cout << endl;
02096             }
02097         }
02098 
02099 
02100         // 2nd order Backward Sensitivities:
02101         // ---------------------------------
02102 
02103         if( nBDirs2 > 0 ){
02104 
02105             cout << "RK: First order Backward Sensitivities:\n";
02106 
02107             cout << "w.r.t. the states:\n";
02108             for( run2 = 0; run2 < m; run2++ ){
02109                 cout << scientific << etaH2[diff_index[run2]] << "  ";
02110             }
02111             cout << endl;
02112 
02113             if( mu > 0 ){
02114                 cout  << "w.r.t. the controls:\n";
02115                 for( run2 = 0; run2 < mu; run2++ ){
02116                         cout << scientific << etaH2[control_index[run2]] << "  ";
02117                 }
02118                 cout << endl;
02119             }
02120             if( mp > 0 ){
02121                 cout  << "w.r.t. the parameters:\n";
02122                 for( run2 = 0; run2 < mp; run2++ ){
02123                         cout << scientific << etaH2[parameter_index[run2]] << "  ";
02124                 }
02125                 cout << "\n";
02126             }
02127             if( mw > 0 ){
02128                 cout << "w.r.t. the disturbances:\n" << scientific;
02129                 for( run2 = 0; run2 < mw; run2++ ){
02130                         cout << etaH2[disturbance_index[run2]] << "  ";
02131                 }
02132                 cout << endl;
02133             }
02134 
02135             cout << "RK: Second order Backward Sensitivities:\n";
02136 
02137             cout << "w.r.t. the states:\n" << scientific;;
02138             for( run2 = 0; run2 < m; run2++ ){
02139                 cout << etaH3[diff_index[run2]] << "  ";
02140             }
02141             cout << endl;
02142 
02143             if( mu > 0 ){
02144                 cout << "w.r.t. the controls:\n" << scientific;
02145                 for( run2 = 0; run2 < mu; run2++ ){
02146                     cout << etaH3[control_index[run2]] << "  ";
02147                 }
02148                 cout << endl;
02149             }
02150 
02151             if( mp > 0 ){
02152                 cout << "w.r.t. the parameters:\n";
02153                 for( run2 = 0; run2 < mp; run2++ ){
02154                     cout << etaH3[parameter_index[run2]] << endl;
02155                 }
02156                 cout << endl;
02157             }
02158 
02159             if( mw > 0 ){
02160                 cout << "w.r.t. the disturbances:\n";
02161                 for( run2 = 0; run2 < mw; run2++ ){
02162                     cout << etaH3[disturbance_index[run2]] << "  ";
02163                 }
02164                 cout << endl;
02165             }
02166      }
02167 }
02168 
02169 void IntegratorLYAPUNOV::interpolate( int jj, double *e1, double *d1, double *e2, VariablesGrid &poly ){
02170 
02171     int run1;
02172 
02173     for( run1 = 0; run1 < m; run1++ ){
02174 
02175         double cc = e1[run1];
02176         double bb = d1[run1];
02177         double aa = (e2[run1] - bb*h[0] - cc)/(h[0]*h[0]);
02178 
02179         double tt = timeInterval.getTime(jj) - t + h[0];
02180 
02181         poly( jj, run1 ) = aa*tt*tt + bb*tt + cc;
02182     }
02183 }
02184 
02185 
02186 int IntegratorLYAPUNOV::getDim() const{
02187 
02188     return m;
02189 }
02190 
02191 
02192 CLOSE_NAMESPACE_ACADO
02193 
02194 
02195 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:37:27