ellipsoidal_integrator.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/validated_integrator/ellipsoidal_integrator.hpp>
00035 
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 
00041 // IMPLEMENTATION OF PUBLIC MEMBER FUNCTIONS:
00042 // ------------------------------------------
00043 
00044 EllipsoidalIntegrator::EllipsoidalIntegrator():AlgorithmicBase(){ setupOptions(); }
00045 
00046 EllipsoidalIntegrator::EllipsoidalIntegrator( const DifferentialEquation &rhs_, const int &N_ ):AlgorithmicBase(){
00047 
00048     ASSERT( rhs_.isODE( ) == BT_TRUE );
00049         setupOptions();
00050     init( rhs_, N_ );
00051 }
00052 
00053 EllipsoidalIntegrator::EllipsoidalIntegrator( const EllipsoidalIntegrator& arg ):AlgorithmicBase( arg ){ copy(arg); }
00054 
00055 EllipsoidalIntegrator::~EllipsoidalIntegrator( ){ }
00056 
00057 EllipsoidalIntegrator& EllipsoidalIntegrator::operator=( const EllipsoidalIntegrator& arg ){
00058   
00059         if( this != &arg ){
00060                 AlgorithmicBase::operator=(arg);
00061                 copy(arg);
00062         }
00063         return *this;
00064 }
00065 
00066 
00067 
00068 returnValue EllipsoidalIntegrator::init( const DifferentialEquation &rhs_, const int &N_ ){
00069  
00070   if( rhs_.isODE( ) == BT_FALSE ) return ACADOERROR(RET_CANNOT_TREAT_DAE);
00071   
00072   nx = rhs_.getDim();
00073   
00074   Q.resize(nx,nx);
00075   for( int i=0; i<nx*nx; i++ ) Q(i) = 0.0;
00076   N = N_;
00077   
00078   IntermediateState derivatives = rhs_.getODEexpansion( N );
00079   
00080   IntermediateState gg = derivatives.getCol(0);
00081   for( int i=0; i<N; i++ ) gg << derivatives.getCol(i+1)/acadoFactorial(i+1);
00082   
00083   g  << gg;
00084   
00085 //   FILE *file = fopen("g.c","w");
00086 //   file << g;
00087 //   fclose(file);
00088   
00089   gr << derivatives.getCol(N+1)/acadoFactorial(N+1);
00090   dg << gg.ADforward( VT_DIFFERENTIAL_STATE, rhs_.getComponents(), nx );
00091   
00092   DifferentialState r("", nx, 1);
00093   
00094   IntermediateState dgg = gg.ADforward( VT_DIFFERENTIAL_STATE, rhs_.getComponents(), r );
00095   ddg << 0.5*dgg.ADforward( VT_DIFFERENTIAL_STATE, rhs_.getComponents(), r );
00096   
00097   return SUCCESSFUL_RETURN;
00098 }
00099 
00100 
00101 
00102 Tmatrix<Interval> EllipsoidalIntegrator::integrate( double t0, double tf, int M,
00103                                                                                                         const Tmatrix<Interval> &x ){
00104 
00105         Tmatrix<Interval> p(0);
00106         Tmatrix<Interval> w(0);
00107         return integrate( t0, tf, M, x, p, w );
00108 }
00109 
00110 Tmatrix<Interval> EllipsoidalIntegrator::integrate( double t0, double tf, int M,
00111                                                                                                         const Tmatrix<Interval> &x,
00112                                                                                                         const Tmatrix<Interval> &p ){
00113 
00114         Tmatrix<Interval> w(0);
00115         return integrate( t0, tf, M, x, p, w );
00116 }
00117 
00118 Tmatrix<Interval> EllipsoidalIntegrator::integrate( double t0, double tf, int M,
00119                                                                                                         const Tmatrix<Interval> &x,
00120                                                                                                         const Tmatrix<Interval> &p,
00121                                                                                                         const Tmatrix<Interval> &w ){
00122 
00123   
00124         typedef TaylorVariable<Interval> T;
00125         
00126         Tmatrix< TaylorVariable<Interval> > xx(x.getDim());
00127         
00128         Tmatrix<T> *pp = 0;
00129         Tmatrix<T> *ww = 0;
00130         
00131         if( p.getDim() > 0 ) pp = new Tmatrix<T>(p.getDim());
00132         if( w.getDim() > 0 ) ww = new Tmatrix<T>(w.getDim());
00133         
00134         int nn = 0;
00135         for( int i=0; i<(int) x.getDim(); i++ ) if( diam(x(i)) > EQUALITY_EPS ) nn++;
00136         for( int i=0; i<(int) p.getDim(); i++ ) if( diam(p(i)) > EQUALITY_EPS ) nn++;
00137         for( int i=0; i<(int) w.getDim(); i++ ) if( diam(w(i)) > EQUALITY_EPS ) nn++;
00138         
00139         TaylorModel<Interval> Mod( nn, M );
00140         
00141         nn = 0;
00142         for( int i=0; i<(int) x.getDim(); i++ ){
00143                 if( diam(x(i)) > EQUALITY_EPS ){ xx(i) = T( &Mod, nn, x(i) ); nn++; }
00144                 else xx(i) = x(i);
00145         }
00146         for( int i=0; i<(int) p.getDim(); i++ ){
00147                 if( diam(p(i)) > EQUALITY_EPS ){ pp->operator()(i) = T( &Mod, nn, p(i) ); nn++; }
00148                 else pp->operator()(i) = p(i);
00149         }
00150         for( int i=0; i<(int) w.getDim(); i++ ){
00151                 if( diam(w(i)) > EQUALITY_EPS ){ ww->operator()(i) = T( &Mod, nn, w(i) ); nn++; }
00152                 else ww->operator()(i) = w(i);
00153         }
00154         
00155         integrate( t0, tf, &xx, pp, ww );
00156         
00157         return getStateBound( xx );
00158 }
00159 
00160 
00161 
00162 // IMPLEMENTATION OF PRIVATE MEMBER FUNCTIONS:
00163 // ======================================================================================
00164 
00165 
00166 void EllipsoidalIntegrator::copy( const EllipsoidalIntegrator& arg ){
00167   
00168         nx  = arg.nx ;
00169         N   = arg.N  ;
00170         g   = arg.g  ;
00171         gr  = arg.gr ;
00172         dg  = arg.dg ;
00173         ddg = arg.ddg;
00174         Q   = arg.Q  ;
00175 }
00176 
00177 
00178 Tmatrix<Interval> EllipsoidalIntegrator::evalC( const Tmatrix<double> &C, double h ) const{
00179 
00180         Tmatrix<Interval> r(nx,nx);
00181         
00182         for( int i=0; i<nx*nx; i++ ){
00183           
00184                 double r_i;
00185                 r_i = C(i);
00186                 for( int j=0; j<N; j++ ) r_i += ::pow(h,j+1)*C(i+(j+1)*nx*nx);
00187                 r(i) = r_i;
00188         }
00189         return r;
00190 }
00191 
00192 
00193 Tmatrix<double> EllipsoidalIntegrator::evalC2( const Tmatrix<double> &C, double h ) const{
00194 
00195         Tmatrix<double> r(nx,nx);
00196         
00197         for( int i=0; i<nx*nx; i++ ){
00198           
00199                 double r_i;
00200                 r_i = C(i);
00201                 for( int j=0; j<N; j++ ) r_i += ::pow(h,j+1)*C(i+(j+1)*nx*nx);
00202                 r(i) = r_i;
00203         }
00204         return r;
00205 }
00206 
00207 double EllipsoidalIntegrator::scale( const Interval &E, const Interval &X ) const{
00208 
00209     double TOL,ATOL;
00210     get( INTEGRATOR_TOLERANCE, TOL  );
00211     get( ABSOLUTE_TOLERANCE  , ATOL );
00212 
00213     return 0.5*acadoMax( ::fabs(E.l()), ::fabs(E.u()) )/
00214            ( TOL*acadoMax( ::fabs(X.l()), ::fabs(X.u()) ) + ATOL );
00215 }
00216 
00217 
00218 double EllipsoidalIntegrator::norm ( const Tmatrix<Interval> &E, Tmatrix<Interval> &X ) const{
00219   
00220     ASSERT( E.getDim() == X.getDim() );
00221 
00222     double sum = 0.0;
00223         
00224         for( int i=0; i < (int) E.getDim(); i++ ) sum += scale(E(i),X(i));
00225         
00226         return sum/((double) E.getDim());
00227 }
00228 
00229 
00230 void EllipsoidalIntegrator::updateQ( Tmatrix<double> C, Tmatrix<Interval> R ){
00231 
00232         Tmatrix<double> CT(nx,nx);
00233         
00234         for( int i=0; i<nx; i++ )
00235                 for( int j=0;j<nx;j++)
00236                         CT(i,j) = C(j,i);
00237         
00238         Q = C*Q*CT;
00239         
00240         double trQ = 0.0;
00241         for( int i=0; i<nx; i++ ) trQ += Q(i,i)/(Q(i,i)+1e-8);
00242         trQ = ::sqrt(trQ);
00243         
00244         DVector sqrR(nx);
00245         for( int i=0; i<nx; i++ ) sqrR(i) = acadoMax(::fabs(R(i).l()),::fabs(R(i).u()))/::sqrt(Q(i,i)+1e-8);
00246         
00247         double kappa = trQ;
00248         for( int i=0; i<nx; i++ ) kappa += sqrR(i);
00249         
00250         Q *= kappa/(trQ+EPS);
00251         for( int i=0; i<nx; i++ ){
00252                 double tmp = acadoMax(::fabs(R(i).l()),::fabs(R(i).u()));
00253                 tmp *= ::sqrt(kappa/(sqrR(i)+EPS));
00254                 Q(i,i) += tmp*tmp+EPS;
00255         }
00256 }
00257 
00258 Tmatrix<Interval> EllipsoidalIntegrator::boundQ() const{
00259 
00260         Tmatrix<Interval> R(nx);
00261         for( int i=0; i<nx; i++ ) R(i) = Interval(-1.0,1.0)*::sqrt(Q(i,i)+EPS*EPS);
00262         return R;
00263 }
00264 
00265 BooleanType EllipsoidalIntegrator::isIncluded( const Tmatrix<Interval> &A, const Tmatrix<Interval> &B ) const{
00266 
00267         ASSERT( A.getDim() == B.getDim() );
00268 
00269         BooleanType result = BT_TRUE;
00270 
00271         for( int i=0; i < (int) A.getDim(); i++ ){
00272                 if( A(i).l() < B(i).l() || A(i).u() > B(i).u() ){ result = BT_FALSE; }
00273         }
00274 
00275         return result;
00276 }
00277 
00278 
00279 void EllipsoidalIntegrator::setInfinity(){ Q *= INFINITY; }
00280 
00281 
00282 returnValue EllipsoidalIntegrator::setupOptions( ){
00283  
00284         addOption( MAX_NUM_INTEGRATOR_STEPS    , defaultMaxNumSteps             );
00285         addOption( INTEGRATOR_TOLERANCE        , defaultIntegratorTolerance     );
00286         addOption( ABSOLUTE_TOLERANCE          , defaultAbsoluteTolerance       );
00287         addOption( MIN_INTEGRATOR_STEPSIZE     , defaultMinStepsize             );
00288         addOption( MAX_INTEGRATOR_STEPSIZE     , defaultMaxStepsize             );
00289         addOption( INTEGRATOR_PRINTLEVEL       , defaultIntegratorPrintlevel    );
00290         addOption( PRINT_INTEGRATOR_PROFILE    , defaultprintIntegratorProfile  );
00291         addOption( STEPSIZE_TUNING             , defaultStepsizeTuning          );
00292 
00293         return SUCCESSFUL_RETURN;
00294 }
00295 
00296 
00297 
00298 
00299 CLOSE_NAMESPACE_ACADO
00300 
00301 // end of file.


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