narx_export.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/code_generation/integrators/narx_export.hpp>
00035 
00036 using namespace std;
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 //
00041 // PUBLIC MEMBER FUNCTIONS:
00042 //
00043 
00044 NARXExport::NARXExport( UserInteraction* _userInteraction,
00045                                                                         const std::string& _commonHeaderName
00046                                                                         ) : DiscreteTimeExport( _userInteraction,_commonHeaderName )
00047 {
00048         delay = 1;
00049 }
00050 
00051 
00052 NARXExport::NARXExport( const NARXExport& arg
00053                                                                         ) : DiscreteTimeExport( arg )
00054 {
00055         delay = arg.delay;
00056         parms = arg.parms;
00057 
00058         copy( arg );
00059 }
00060 
00061 
00062 NARXExport::~NARXExport( )
00063 {
00064         clear( );
00065 }
00066 
00067 
00068 returnValue NARXExport::setup( )
00069 {
00070 //      NX = delay*(NX1+NX2)+NX3;               // IMPORTANT for NARX models where the state space is increased because of the delay
00071 
00072         int useOMP;
00073         get(CG_USE_OPENMP, useOMP);
00074         ExportStruct structWspace;
00075         structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
00076 
00077         LOG( LVL_DEBUG ) << "Preparing to export NARXExport... " << endl;
00078 
00079         ExportIndex run( "run" );
00080         ExportIndex i( "i" );
00081         ExportIndex j( "j" );
00082         ExportIndex k( "k" );
00083         ExportIndex tmp_index("tmp_index");
00084         diffsDim = NX*(NX+NU);
00085         inputDim = NX*(NX+NU+1) + NU + NOD;
00086         // setup INTEGRATE function
00087         rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
00088         rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
00089         if( equidistantControlGrid() ) {
00090                 integrate = ExportFunction( "integrate", rk_eta, reset_int );
00091         }
00092         else {
00093                 integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
00094         }
00095         integrate.setReturnValue( error_code );
00096 
00097         rk_eta.setDoc( "Working array to pass the input values and return the results." );
00098         reset_int.setDoc( "The internal memory of the integrator can be reset." );
00099         rk_index.setDoc( "Number of the shooting interval." );
00100         error_code.setDoc( "Status code of the integrator." );
00101         integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
00102 
00103         integrate.addIndex( run );
00104         integrate.addIndex( i );
00105         integrate.addIndex( j );
00106         integrate.addIndex( k );
00107         integrate.addIndex( tmp_index );
00108         rhs_in = ExportVariable( "x", inputDim-diffsDim, 1, REAL, ACADO_LOCAL );
00109         rhs_out = ExportVariable( "f", NX, 1, REAL, ACADO_LOCAL );
00110         fullRhs = ExportFunction( "full_rhs", rhs_in, rhs_out );
00111         rhs_in.setDoc( "The state and parameter values." );
00112         rhs_out.setDoc( "Right-hand side evaluation." );
00113         fullRhs.doc( "Evaluates the right-hand side of the full model." );
00114         rk_xxx = ExportVariable( "rk_xxx", 1, inputDim-diffsDim, REAL, structWspace );
00115         rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", delay*NX1, (delay-1)*NX1+NU, REAL, structWspace );
00116         rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", delay*NX2, delay*(NX1+NX2)+NU, REAL, structWspace );
00117         rk_diffsPrev3 = ExportVariable( "rk_diffsPrev3", NX3, NX+NU, REAL, structWspace );
00118         rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX1, NX1+NU, REAL, structWspace );
00119         rk_diffsNew2 = ExportVariable( "rk_diffsNew2", NX2, delay*(NX1+NX2), REAL, structWspace );
00120         rk_diffsNew3 = ExportVariable( "rk_diffsNew3", NX3, NX+NU, REAL, structWspace );
00121         rk_diffsTemp3 = ExportVariable( "rk_diffsTemp3", NX3, delay*(NX1+NX2)+NU, REAL, structWspace );
00122 
00123         ExportVariable numInt( "numInts", 1, 1, INT );
00124         if( !equidistantControlGrid() ) {
00125                 ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
00126                 integrate.addStatement( std::string( "int " ) + numInt.getName() + " = " + numStepsV.getName() + "[" + rk_index.getName() + "];\n" );
00127         }
00128 
00129         integrate.addStatement( rk_xxx.getCols( NX,inputDim-diffsDim ) == rk_eta.getCols( NX+diffsDim,inputDim ) );
00130         integrate.addLinebreak( );
00131 
00132         // Linear input:
00133         DMatrix eyeM = eye<double>((delay-1)*NX1);
00134         eyeM.appendRows( zeros<double>(NX1,(delay-1)*NX1) );
00135         eyeM.appendCols( zeros<double>(delay*NX1,NU) );
00136         if( NX1 > 0 ) {
00137                 integrate.addStatement( rk_diffsPrev1 == eyeM );
00138         }
00139 
00140         // Nonlinear part:
00141         for( uint i1 = 0; i1 < delay; i1++ ) {
00142                 eyeM = zeros<double>(NX2,i1*(NX1+NX2)+NX1);
00143                 eyeM.appendCols( eye<double>(NX2) );
00144                 eyeM.appendCols( zeros<double>(NX2,(delay-i1-1)*(NX1+NX2)+NU) );
00145                 integrate.addStatement( rk_diffsPrev2.getRows(i1*NX2,i1*NX2+NX2) == eyeM );
00146         }
00147         // evaluate sensitivities linear input:
00148         if( NX1 > 0 ) {
00149                 for( uint i1 = 0; i1 < NX1; i1++ ) {
00150                         for( uint i2 = 0; i2 < NX1; i2++ ) {
00151                                 integrate.addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,i2,i2+1) == A11(i1,i2) );
00152                         }
00153                         for( uint i2 = 0; i2 < NU; i2++ ) {
00154                                 integrate.addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,NX1+i2,NX1+i2+1) == B11(i1,i2) );
00155                         }
00156                 }
00157         }
00158         // evaluate sensitivities linear output:
00159         if( NX1 > 0 ) {
00160                 for( uint i1 = 0; i1 < NX3; i1++ ) {
00161                         for( uint i2 = 0; i2 < NX3; i2++ ) {
00162                                 integrate.addStatement( rk_diffsNew3.getSubMatrix(i1,i1+1,NX-NX3+i2,NX-NX3+i2+1) == A33(i1,i2) );
00163                         }
00164                 }
00165         }
00166         integrate.addLinebreak( );
00167 
00168         // integrator loop:
00169         ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
00170         ExportStatementBlock *loop;
00171         if( equidistantControlGrid() ) {
00172                 loop = &tmpLoop;
00173         }
00174         else {
00175                 loop = &integrate;
00176                 loop->addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
00177         }
00178 
00179         loop->addStatement( rk_xxx.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) );
00180 
00181         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00182                 loop->addStatement( std::string("if( run > 0 ) {\n") );
00183                 // SHIFT rk_diffsPrev:
00184                 // TODO: write using exportforloop
00185                 if( NX1 > 0 ) {
00186                         for( uint s = 1; s < delay; s++ ) {
00187                                 loop->addStatement( rk_diffsPrev1.getRows(s*NX1,s*NX1+NX1) == rk_diffsPrev1.getRows((s-1)*NX1,s*NX1) );
00188                         }
00189 
00190                         // Add to rk_diffsPrev:
00191                         ExportForLoop loopTemp1( i,0,NX1 );
00192                         loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+NX,i*NX+NX+NX1 ) );
00193                         if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,(delay-1)*NX1,(delay-1)*NX1+NU ) == rk_eta.getCols( i*NU+NX*(NX+1),i*NU+NX*(NX+1)+NU ) );
00194                         loop->addStatement( loopTemp1 );
00195                 }
00196 
00197                 if( NX2 > 0 ) {
00198                         for( uint s = 1; s < delay; s++ ) {
00199                                 loop->addStatement( rk_diffsPrev2.getRows(s*NX2,s*NX2+NX2) == rk_diffsPrev2.getRows((s-1)*NX2,s*NX2) );
00200                         }
00201 
00202                         // Add to rk_diffsPrev:
00203                         ExportForLoop loopTemp2( i,0,NX2 );
00204                         loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,0,delay*(NX1+NX2) ) == rk_eta.getCols( i*NX+NX+NX1*NX,i*NX+NX+NX1*NX+delay*(NX1+NX2) ) );
00205                         if( NU > 0 ) loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,delay*(NX1+NX2),delay*(NX1+NX2)+NU ) == rk_eta.getCols( i*NU+NX*(NX+1)+NX1*NU,i*NU+NX*(NX+1)+NX1*NU+NU ) );
00206                         loop->addStatement( loopTemp2 );
00207                 }
00208 
00209                 if( NX3 > 0 ) {
00210                         ExportForLoop loopTemp3( i,0,NX3 );
00211                         loopTemp3.addStatement( rk_diffsPrev3.getSubMatrix( i,i+1,0,NX ) == rk_eta.getCols( i*NX+NX+delay*(NX1+NX2)*NX,i*NX+NX+delay*(NX1+NX2)*NX+NX ) );
00212                         if( NU > 0 ) loopTemp3.addStatement( rk_diffsPrev3.getSubMatrix( i,i+1,NX,NX+NU ) == rk_eta.getCols( i*NU+NX*(NX+1)+delay*(NX1+NX2)*NU,i*NU+NX*(NX+1)+delay*(NX1+NX2)*NU+NU ) );
00213                         loop->addStatement( loopTemp3 );
00214                 }
00215                 loop->addStatement( std::string("}\n") );
00216         }
00217 
00218         // evaluate states:
00219         if( NX1 > 0 ) {
00220                 loop->addFunctionCall( lin_input.getName(), rk_xxx, rk_eta.getAddress(0,0) );
00221 
00222         }
00223         if( NX2 > 0 ) {
00224                 loop->addFunctionCall( rhs.getName(), rk_xxx, rk_eta.getAddress(0,NX1) );
00225         }
00226         // shifting memory of NARX model:
00227         for( uint s = 1; s < delay; s++ ) {
00228                 loop->addStatement( rk_eta.getCols(s*(NX1+NX2),s*(NX1+NX2)+(NX1+NX2)) == rk_xxx.getCols((s-1)*(NX1+NX2),s*(NX1+NX2)) );
00229         }
00230         if( NX3 > 0 ) {
00231                 loop->addFunctionCall( getNameOutputRHS(), rk_xxx, rk_eta.getAddress(0,delay*(NX1+NX2)) );
00232         }
00233 
00234         // evaluate sensitivities:
00235         if( NX2 > 0 ) {
00236                 loop->addFunctionCall( diffs_rhs.getName(), rk_xxx, rk_diffsNew2.getAddress(0,0) );
00237         }
00238         if( NX3 > 0 ) {
00239                 loop->addFunctionCall( getNameOutputDiffs(), rk_xxx, rk_diffsTemp3.getAddress(0,0) );
00240                 ExportForLoop loop1( i,0,NX3 );
00241                 ExportForLoop loop2( j,0,delay*(NX1+NX2) );
00242                 loop2.addStatement( rk_diffsNew3.getSubMatrix(i,i+1,j,j+1) == rk_diffsTemp3.getSubMatrix(i,i+1,j,j+1) );
00243                 loop1.addStatement( loop2 );
00244                 loop2 = ExportForLoop( j,0,NU );
00245                 loop2.addStatement( rk_diffsNew3.getSubMatrix(i,i+1,NX+j,NX+j+1) == rk_diffsTemp3.getSubMatrix(i,i+1,delay*(NX1+NX2)+j,delay*(NX1+NX2)+j+1) );
00246                 loop1.addStatement( loop2 );
00247                 loop->addStatement( loop1 );
00248         }
00249 
00250         // computation of the sensitivities using chain rule:
00251         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00252                 loop->addStatement( std::string( "if( run == 0 ) {\n" ) );
00253         }
00254         // PART 1
00255         updateInputSystem(loop, i, j, tmp_index);
00256         // PART 2
00257         updateImplicitSystem(loop, i, j, tmp_index);
00258         // PART 3
00259         updateOutputSystem(loop, i, j, tmp_index);
00260 
00261         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00262                 loop->addStatement( std::string( "}\n" ) );
00263                 loop->addStatement( std::string( "else {\n" ) );
00264                 // PART 1
00265                 propagateInputSystem(loop, i, j, k, tmp_index);
00266                 // PART 2
00267                 propagateImplicitSystem(loop, i, j, k, tmp_index);
00268                 // PART 3
00269                 propagateOutputSystem(loop, i, j, k, tmp_index);
00270                 loop->addStatement( std::string( "}\n" ) );
00271         }
00272 
00273         // end of the integrator loop.
00274         if( !equidistantControlGrid() ) {
00275                 loop->addStatement( "}\n" );
00276         }
00277         else {
00278                 integrate.addStatement( *loop );
00279         }
00280 
00281         // FILL IN ALL THE SENSITIVITIES OF THE DELAYED STATES BASED ON RK_DIFFSPREV + SPARSITY
00282         if( NX1 > 0 ) {
00283                 DMatrix zeroR = zeros<double>(1, NX2);
00284                 ExportForLoop loop1( i,0,NX1 );
00285                 loop1.addStatement( rk_eta.getCols( i*NX+NX+NX1,i*NX+NX+NX1+NX2 ) == zeroR );
00286                 zeroR = zeros<double>(1, (delay-1)*(NX1+NX2)+NX3);
00287                 loop1.addStatement( rk_eta.getCols( i*NX+NX+NX1+NX2,i*NX+NX+NX ) == zeroR );
00288                 integrate.addStatement( loop1 );
00289                 for( uint s1 = 1; s1 < delay; s1++ ) {
00290                         ExportForLoop loop2( i,0,NX1 );
00291                         // STATES
00292                         zeroR = zeros<double>(1, NX2);
00293                         for( uint s2 = 0; s2 < s1; s2++ ) {
00294                                 loop2.addStatement( rk_eta.getCols( i*NX+NX+s1*(NX1+NX2)*NX+s2*(NX1+NX2),i*NX+NX+s1*(NX1+NX2)*NX+s2*(NX1+NX2)+NX1 ) == rk_diffsPrev1.getSubMatrix( i+(s1-1)*NX1,i+(s1-1)*NX1+1,s2*NX1,s2*NX1+NX1 ) );
00295                                 loop2.addStatement( rk_eta.getCols( i*NX+NX+s1*(NX1+NX2)*NX+s2*(NX1+NX2)+NX1,i*NX+NX+s1*(NX1+NX2)*NX+s2*(NX1+NX2)+(NX1+NX2) ) == zeroR );
00296                         }
00297                         zeroR = zeros<double>(1, (delay-s1)*(NX1+NX2)+NX3);
00298                         loop2.addStatement( rk_eta.getCols( i*NX+NX+s1*(NX1+NX2)*NX+s1*(NX1+NX2),i*NX+NX+s1*(NX1+NX2)*NX+NX ) == zeroR );
00299                         // CONTROLS
00300                         if( NU > 0 ) loop2.addStatement( rk_eta.getCols( i*NU+NX*(1+NX)+s1*(NX1+NX2)*NU,i*NU+NX*(1+NX)+s1*(NX1+NX2)*NU+NU ) == rk_diffsPrev1.getSubMatrix( i+(s1-1)*NX1,i+(s1-1)*NX1+1,(delay-1)*NX1,(delay-1)*NX1+NU ) );
00301                         integrate.addStatement( loop2 );
00302                 }
00303         }
00304         if( NX2 > 0 ) {
00305                 for( uint s = 0; s < delay; s++ ) {
00306                         ExportForLoop loop3( i,0,NX2 );
00307                         // STATES
00308                         if( s > 0 ) loop3.addStatement( rk_eta.getCols( i*NX+NX+s*(NX1+NX2)*NX+NX1*NX,i*NX+NX+s*(NX1+NX2)*NX+NX1*NX+delay*(NX1+NX2) ) == rk_diffsPrev2.getSubMatrix( i+(s-1)*NX2,i+(s-1)*NX2+1,0,delay*(NX1+NX2) ) );
00309                         DMatrix zeroR;
00310                         if( NX3 > 0 ) {
00311                                 zeroR = zeros<double>(1, NX3);
00312                                 loop3.addStatement( rk_eta.getCols( i*NX+NX+s*(NX1+NX2)*NX+NX1*NX+delay*(NX1+NX2),i*NX+NX+s*(NX1+NX2)*NX+NX1*NX+NX ) == zeroR );
00313                         }
00314                         // CONTROLS
00315                         if( NU > 0 && s > 0 )   loop3.addStatement( rk_eta.getCols( i*NU+NX*(1+NX)+s*(NX1+NX2)*NU+NX1*NU,i*NU+NX*(1+NX)+s*(NX1+NX2)*NU+NX1*NU+NU ) == rk_diffsPrev2.getSubMatrix( i+(s-1)*NX2,i+(s-1)*NX2+1,delay*(NX1+NX2),delay*(NX1+NX2)+NU ) );
00316                         integrate.addStatement( loop3 );
00317                 }
00318         }
00319 
00320         LOG( LVL_DEBUG ) << "done" << endl;
00321 
00322         return SUCCESSFUL_RETURN;
00323 }
00324 
00325 
00326 returnValue NARXExport::prepareFullRhs( ) {
00327 
00328         uint i, j;
00329 
00330         for( i = 1; i < delay; i++ ) {
00331                 fullRhs.addStatement( rhs_out.getRows(i*(NX1+NX2),i*(NX1+NX2)+(NX1+NX2)) == rhs_in.getRows(i*(NX1+NX2)-(NX1+NX2),i*(NX1+NX2)) );
00332         }
00333 
00334         // PART 1:
00335         for( i = 0; i < NX1; i++ ) {
00336                 fullRhs.addStatement( rhs_out.getRow(i) == A11(i,0)*rhs_in.getRow(0) );
00337                 for( j = 1; j < NX1; j++ ) {
00338                         if( acadoRoundAway(A11(i,j)) != 0 ) {
00339                                 fullRhs.addStatement( rhs_out.getRow(i) += A11(i,j)*rhs_in.getRow(j) );
00340                         }
00341                 }
00342                 for( j = 0; j < NU; j++ ) {
00343                         if( acadoRoundAway(B11(i,j)) != 0 ) {
00344                                 fullRhs.addStatement( rhs_out.getRow(i) += B11(i,j)*rhs_in.getRow(NX+j) );
00345                         }
00346                 }
00347         }
00348 
00349         // PART 2:
00350         if( NX2 > 0 ) {
00351                 fullRhs.addFunctionCall( getNameRHS(), rhs_in, rhs_out.getAddress(NX1,0) );
00352         }
00353 
00354         // PART 3:
00355         if( NX3 > 0 ) {
00356                 fullRhs.addFunctionCall( getNameOutputRHS(), rhs_in, rhs_out.getAddress((NX1+NX2)*delay,0) );
00357         }
00358 
00359         return SUCCESSFUL_RETURN;
00360 }
00361 
00362 
00363 returnValue NARXExport::updateInputSystem(      ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index )
00364 {
00365         if( NX1 > 0 ) {
00366                 ExportForLoop loop01( index1,0,NX1 );
00367                 ExportForLoop loop02( index2,0,NX1 );
00368                 loop02.addStatement( tmp_index == index2+index1*NX );
00369                 loop02.addStatement( rk_eta.getCol( tmp_index+NX ) == rk_diffsNew1.getSubMatrix( index1,index1+1,index2,index2+1 ) );
00370                 loop01.addStatement( loop02 );
00371 
00372                 if( NU > 0 ) {
00373                         ExportForLoop loop03( index2,0,NU );
00374                         loop03.addStatement( tmp_index == index2+index1*NU );
00375                         loop03.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX) ) == rk_diffsNew1.getSubMatrix( index1,index1+1,NX1+index2,NX1+index2+1 ) );
00376                         loop01.addStatement( loop03 );
00377                 }
00378                 block->addStatement( loop01 );
00379         }
00380 
00381         return SUCCESSFUL_RETURN;
00382 }
00383 
00384 
00385 returnValue NARXExport::updateImplicitSystem(   ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index )
00386 {
00387         if( NX2 > 0 ) {
00388                 ExportForLoop loop01( index1,0,NX2 );
00389                 ExportForLoop loop02( index2,0,delay*(NX1+NX2) );
00390                 loop02.addStatement( tmp_index == index2+index1*NX );
00391                 loop02.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX ) == rk_diffsNew2.getSubMatrix( index1,index1+1,index2,index2+1 ) );
00392                 loop01.addStatement( loop02 );
00393 
00394                 if( NU > 0 ) {
00395                         ExportForLoop loop03( index2,0,NU );
00396                         loop03.addStatement( tmp_index == index2+index1*NU );
00397                         loop03.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+NX1*NU ) == 0.0 ); // the control inputs do not appear directly in the NARX model !!
00398                         loop01.addStatement( loop03 );
00399                 }
00400                 block->addStatement( loop01 );
00401         }
00402         // ALGEBRAIC STATES: NONE
00403 
00404         return SUCCESSFUL_RETURN;
00405 }
00406 
00407 
00408 returnValue NARXExport::updateOutputSystem(     ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index )
00409 {
00410         if( NX3 > 0 ) {
00411                 ExportForLoop loop01( index1,0,NX3 );
00412                 ExportForLoop loop02( index2,0,NX );
00413                 loop02.addStatement( tmp_index == index2+index1*NX );
00414                 loop02.addStatement( rk_eta.getCol( tmp_index+NX*(1+delay*(NX1+NX2)) ) == rk_diffsNew3.getSubMatrix( index1,index1+1,index2,index2+1 ) );
00415                 loop01.addStatement( loop02 );
00416 
00417                 if( NU > 0 ) {
00418                         ExportForLoop loop03( index2,0,NU );
00419                         loop03.addStatement( tmp_index == index2+index1*NU );
00420                         loop03.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+NU*delay*(NX1+NX2) ) == rk_diffsNew3.getSubMatrix( index1,index1+1,NX+index2,NX+index2+1 ) );
00421                         loop01.addStatement( loop03 );
00422                 }
00423                 block->addStatement( loop01 );
00424         }
00425 
00426         return SUCCESSFUL_RETURN;
00427 }
00428 
00429 
00430 returnValue NARXExport::propagateInputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2,
00431                                                                                                                          const ExportIndex& index3, const ExportIndex& tmp_index )
00432 {
00433         if( NX1 > 0 ) {
00434                 ExportForLoop loop01( index1,0,NX1 );
00435                 ExportForLoop loop02( index2,0,NX1 );
00436                 loop02.addStatement( tmp_index == index2+index1*NX );
00437                 loop02.addStatement( rk_eta.getCol( tmp_index+NX ) == rk_diffsNew1.getSubMatrix( index1,index1+1,0,1 )*rk_diffsPrev1.getSubMatrix( 0,1,index2,index2+1 ) );
00438                 ExportForLoop loop03( index3,1,NX1 );
00439                 loop03.addStatement( rk_eta.getCol( tmp_index+NX ) += rk_diffsNew1.getSubMatrix( index1,index1+1,index3,index3+1 )*rk_diffsPrev1.getSubMatrix( index3,index3+1,index2,index2+1 ) );
00440                 loop02.addStatement( loop03 );
00441                 loop01.addStatement( loop02 );
00442 
00443                 if( NU > 0 ) {
00444                         ExportForLoop loop04( index2,0,NU );
00445                         loop04.addStatement( tmp_index == index2+index1*NU );
00446                         loop04.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX) ) == rk_diffsNew1.getSubMatrix( index1,index1+1,NX1+index2,NX1+index2+1 ) );
00447                         ExportForLoop loop05( index3,0,NX1 );
00448                         loop05.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX) ) += rk_diffsNew1.getSubMatrix( index1,index1+1,index3,index3+1 )*rk_diffsPrev1.getSubMatrix( index3,index3+1,(delay-1)*NX1+index2,(delay-1)*NX1+index2+1 ) );
00449                         loop04.addStatement( loop05 );
00450                         loop01.addStatement( loop04 );
00451                 }
00452                 block->addStatement( loop01 );
00453         }
00454         return SUCCESSFUL_RETURN;
00455 }
00456 
00457 
00458 returnValue NARXExport::propagateImplicitSystem(        ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2,
00459                                                                                                                                 const ExportIndex& index3, const ExportIndex& tmp_index )
00460 {
00461         uint i;
00462         if( NX2 > 0 ) {
00463                 ExportForLoop loop01( index1,0,NX2 );
00464                 for( uint s = 0; s < delay; s++ ) {
00465                         ExportForLoop loop02( index2,0,NX1 );
00466                         loop02.addStatement( tmp_index == index2+index1*NX );
00467                         loop02.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX+s*(NX1+NX2) ) == 0.0 );
00468                         for( i = 0; i < delay; i++ ) {
00469                                 if( s < (delay-1) ) {
00470                                         ExportForLoop loop03( index3,0,NX1 );
00471                                         loop03.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX+s*(NX1+NX2) ) += rk_diffsNew2.getSubMatrix( index1,index1+1,i*(NX1+NX2)+index3,i*(NX1+NX2)+index3+1 )*rk_diffsPrev1.getSubMatrix( i*NX1+index3,i*NX1+index3+1,index2+s*NX1,index2+s*NX1+1 ) );
00472                                         loop02.addStatement( loop03 );
00473                                 }
00474                                 ExportForLoop loop04( index3,0,NX2 );
00475                                 loop04.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX+s*(NX1+NX2) ) += rk_diffsNew2.getSubMatrix( index1,index1+1,i*(NX1+NX2)+NX1+index3,i*(NX1+NX2)+NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( i*NX2+index3,i*NX2+index3+1,index2+s*(NX1+NX2),index2+s*(NX1+NX2)+1 ) );
00476                                 loop02.addStatement( loop04 );
00477                         }
00478                         loop01.addStatement( loop02 );
00479 
00480                         ExportForLoop loop05( index2,0,NX2 );
00481                         loop05.addStatement( tmp_index == index2+index1*NX );
00482                         loop05.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX+s*(NX1+NX2)+NX1 ) == 0.0 );
00483                         for( i = 0; i < delay; i++ ) {
00484                                 ExportForLoop loop06( index3,0,NX2 );
00485                                 loop06.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX+s*(NX1+NX2)+NX1 ) += rk_diffsNew2.getSubMatrix( index1,index1+1,i*(NX1+NX2)+NX1+index3,i*(NX1+NX2)+NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( i*NX2+index3,i*NX2+index3+1,index2+s*(NX1+NX2)+NX1,index2+s*(NX1+NX2)+NX1+1 ) );
00486                                 loop05.addStatement( loop06 );
00487                         }
00488                         loop01.addStatement( loop05 );
00489                 }
00490 
00491                 if( NU > 0 ) {
00492                         ExportForLoop loop07( index2,0,NU );
00493                         loop07.addStatement( tmp_index == index2+index1*NU );
00494                         loop07.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+NX1*NU ) == 0.0 );
00495                         for( i = 0; i < delay; i++ ) {
00496                                 ExportForLoop loop08( index3,0,NX1 );
00497                                 loop08.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+NX1*NU ) += rk_diffsNew2.getSubMatrix( index1,index1+1,i*(NX1+NX2)+index3,i*(NX1+NX2)+index3+1 )*rk_diffsPrev1.getSubMatrix( i*NX1+index3,i*NX1+index3+1,(delay-1)*NX1+index2,(delay-1)*NX1+index2+1 ) );
00498                                 loop07.addStatement( loop08 );
00499                                 ExportForLoop loop09( index3,0,NX2 );
00500                                 loop09.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+NX1*NU ) += rk_diffsNew2.getSubMatrix( index1,index1+1,i*(NX1+NX2)+NX1+index3,i*(NX1+NX2)+NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( i*NX2+index3,i*NX2+index3+1,delay*(NX1+NX2)+index2,delay*(NX1+NX2)+index2+1 ) );
00501                                 loop07.addStatement( loop09 );
00502                         }
00503                         loop01.addStatement( loop07 );
00504                 }
00505                 block->addStatement( loop01 );
00506         }
00507         // ALGEBRAIC STATES: NO PROPAGATION OF SENSITIVITIES NEEDED
00508 
00509         return SUCCESSFUL_RETURN;
00510 }
00511 
00512 
00513 returnValue NARXExport::propagateOutputSystem(  ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2,
00514                                                                                                                                 const ExportIndex& index3, const ExportIndex& tmp_index )
00515 {
00516         uint i;
00517         if( NX3 > 0 ) {
00518                 ExportForLoop loop01( index1,0,NX3 );
00519                 for( uint s = 0; s < delay; s++ ) {
00520                         ExportForLoop loop02( index2,0,NX1 );
00521                         loop02.addStatement( tmp_index == index2+index1*NX );
00522                         loop02.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2) ) == 0.0 );
00523                         for( i = 0; i < delay; i++ ) {
00524                                 if( s < (delay-1) ) {
00525                                         ExportForLoop loop03( index3,0,NX1 );
00526                                         loop03.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2) ) += rk_diffsNew3.getSubMatrix( index1,index1+1,i*(NX1+NX2)+index3,i*(NX1+NX2)+index3+1 )*rk_diffsPrev1.getSubMatrix( i*NX1+index3,i*NX1+index3+1,index2+s*NX1,index2+s*NX1+1 ) );
00527                                         loop02.addStatement( loop03 );
00528                                 }
00529                                 ExportForLoop loop04( index3,0,NX2 );
00530                                 loop04.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2) ) += rk_diffsNew3.getSubMatrix( index1,index1+1,i*(NX1+NX2)+NX1+index3,i*(NX1+NX2)+NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( i*NX2+index3,i*NX2+index3+1,index2+s*(NX1+NX2),index2+s*(NX1+NX2)+1 ) );
00531                                 loop02.addStatement( loop04 );
00532                         }
00533                         ExportForLoop loop022( index3,0,NX3 );
00534                         loop022.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2) ) += rk_diffsNew3.getSubMatrix( index1,index1+1,delay*(NX1+NX2)+index3,delay*(NX1+NX2)+index3+1 )*rk_diffsPrev3.getSubMatrix( index3,index3+1,index2+s*(NX1+NX2),index2+s*(NX1+NX2)+1 ) );
00535                         loop02.addStatement( loop022 );
00536                         loop01.addStatement( loop02 );
00537 
00538                         ExportForLoop loop05( index2,0,NX2 );
00539                         loop05.addStatement( tmp_index == index2+index1*NX );
00540                         loop05.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2)+NX1 ) == 0.0 );
00541                         for( i = 0; i < delay; i++ ) {
00542                                 ExportForLoop loop06( index3,0,NX2 );
00543                                 loop06.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2)+NX1 ) += rk_diffsNew3.getSubMatrix( index1,index1+1,i*(NX1+NX2)+NX1+index3,i*(NX1+NX2)+NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( i*NX2+index3,i*NX2+index3+1,index2+s*(NX1+NX2)+NX1,index2+s*(NX1+NX2)+NX1+1 ) );
00544                                 loop05.addStatement( loop06 );
00545                         }
00546                         ExportForLoop loop055( index3,0,NX3 );
00547                         loop055.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2)+NX1 ) += rk_diffsNew3.getSubMatrix( index1,index1+1,delay*(NX1+NX2)+index3,delay*(NX1+NX2)+index3+1 )*rk_diffsPrev3.getSubMatrix( index3,index3+1,index2+s*(NX1+NX2)+NX1,index2+s*(NX1+NX2)+NX1+1 ) );
00548                         loop05.addStatement( loop055 );
00549                         loop01.addStatement( loop05 );
00550                 }
00551                 ExportForLoop loop06( index2,0,NX3 );
00552                 loop06.addStatement( tmp_index == index2+index1*NX );
00553                 loop06.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+delay*(NX1+NX2) ) == 0.0 );
00554                 ExportForLoop loop061( index3,0,NX3 );
00555                 loop061.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+delay*(NX1+NX2) ) += rk_diffsNew3.getSubMatrix( index1,index1+1,delay*(NX1+NX2)+index3,delay*(NX1+NX2)+index3+1 )*rk_diffsPrev3.getSubMatrix( index3,index3+1,delay*(NX1+NX2)+index2,delay*(NX1+NX2)+index2+1 ) );
00556                 loop06.addStatement( loop061 );
00557                 loop01.addStatement( loop06 );
00558 
00559                 if( NU > 0 ) {
00560                         ExportForLoop loop07( index2,0,NU );
00561                         loop07.addStatement( tmp_index == index2+index1*NU );
00562                         loop07.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+delay*(NX1+NX2)*NU ) == 0.0 );
00563                         for( i = 0; i < delay; i++ ) {
00564                                 ExportForLoop loop08( index3,0,NX1 );
00565                                 loop08.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+delay*(NX1+NX2)*NU ) += rk_diffsNew3.getSubMatrix( index1,index1+1,i*(NX1+NX2)+index3,i*(NX1+NX2)+index3+1 )*rk_diffsPrev1.getSubMatrix( i*NX1+index3,i*NX1+index3+1,(delay-1)*NX1+index2,(delay-1)*NX1+index2+1 ) );
00566                                 loop07.addStatement( loop08 );
00567                                 ExportForLoop loop09( index3,0,NX2 );
00568                                 loop09.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+delay*(NX1+NX2)*NU ) += rk_diffsNew3.getSubMatrix( index1,index1+1,i*(NX1+NX2)+NX1+index3,i*(NX1+NX2)+NX1+index3+1 )*rk_diffsPrev2.getSubMatrix( i*NX2+index3,i*NX2+index3+1,delay*(NX1+NX2)+index2,delay*(NX1+NX2)+index2+1 ) );
00569                                 loop07.addStatement( loop09 );
00570                         }
00571                         ExportForLoop loop010( index3,0,NX3 );
00572                         loop010.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+delay*(NX1+NX2)*NU ) += rk_diffsNew3.getSubMatrix( index1,index1+1,delay*(NX1+NX2)+index3,delay*(NX1+NX2)+index3+1 )*rk_diffsPrev3.getSubMatrix( index3,index3+1,NX+index2,NX+index2+1 ) );
00573                         loop07.addStatement( loop010 );
00574                         loop01.addStatement( loop07 );
00575                 }
00576                 block->addStatement( loop01 );
00577         }
00578 
00579         return SUCCESSFUL_RETURN;
00580 }
00581 
00582 
00583 returnValue NARXExport::setDifferentialEquation(        const Expression& rhs_ )
00584 {
00585 
00586         return ACADOERROR( RET_INVALID_OPTION );
00587 }
00588 
00589 
00590 returnValue NARXExport::setModel(       const std::string& _rhs, const std::string& _diffs_rhs ) {
00591 
00592         // You can't use this feature yet with NARX integrators !
00593         return ACADOERROR( RET_INVALID_OPTION );
00594 }
00595 
00596 
00597 returnValue NARXExport::getDataDeclarations(    ExportStatementBlock& declarations,
00598                                                                                                 ExportStruct dataStruct
00599                                                                                                         ) const
00600 {
00601 
00602         return DiscreteTimeExport::getDataDeclarations( declarations, dataStruct );
00603 }
00604 
00605 
00606 returnValue NARXExport::setNARXmodel( const uint _delay, const DMatrix& _parms ) {
00607 
00608         NX2 = _parms.getNumRows();
00609         delay = _delay;
00610         parms = _parms;
00611 
00612         DifferentialState dummy;
00613         dummy.clearStaticCounters();
00614         uint n = _delay*(NX1+NX2);                              // IMPORTANT for NARX models where the state space is increased because of the delay
00615         x = DifferentialState("", n, 1);
00616 
00617         OutputFcn narxFun;
00618         OutputFcn narxDiff;
00619         for( uint i = 0; i < NX2; i++ ) {
00620                 Expression expr;
00621                 expr = _parms(i,0);
00622                 if( _delay >= 1 ) {
00623                         IntermediateState sum;
00624                         for( uint j = 0; j < n; j++ ) {
00625                                 sum = sum + _parms(i,j+1)*x(j);
00626                         }
00627                         expr = expr + sum;
00628                         uint indexParms = n+1;
00629                         for( uint j = 1; j < _delay; j++ ) {
00630                                 IntermediateState tmp;
00631                                 formNARXpolynomial(i, j, indexParms, 0, tmp);
00632                                 expr = expr + tmp;
00633                         }
00634                 }
00635 
00636                 narxFun << expr;
00637                 narxDiff << forwardDerivative( expr, x );
00638         }
00639         rhs.init( narxFun,"acado_NARX_fun",NX,NXA,NU );
00640         diffs_rhs.init( narxDiff,"acado_NARX_diff",NX,NXA,NU );
00641 
00642         dummy.clearStaticCounters();
00643         x = DifferentialState("", NX, 1);
00644 
00645         return SUCCESSFUL_RETURN;
00646 }
00647 
00648 
00649 returnValue NARXExport::setLinearOutput( const DMatrix& M3, const DMatrix& A3, const Expression& _rhs )
00650 {
00651         if( !A3.isEmpty() ) {
00652                 if( A3.getNumRows() != M3.getNumRows() || M3.getNumRows() != M3.getNumCols() || A3.getNumRows() != A3.getNumCols() || A3.getNumRows() != _rhs.getDim() ) {
00653                         return RET_UNABLE_TO_EXPORT_CODE;
00654                 }
00655                 NX3 = A3.getNumRows();
00656                 M33 = M3;
00657                 A33 = A3;
00658 
00659                 OutputFcn f;
00660                 f << _rhs;
00661                 OnlineData        dummy0;
00662                 Control           dummy1;
00663                 DifferentialState dummy2;
00664                 AlgebraicState    dummy3;
00665                 DifferentialStateDerivative dummy4;
00666                 dummy0.clearStaticCounters();
00667                 dummy1.clearStaticCounters();
00668                 dummy2.clearStaticCounters();
00669                 uint n = delay*(NX1+NX2);
00670                 x = DifferentialState("", n, 1);
00671                 u = Control("", NU, 1);
00672                 od = OnlineData("", NOD, 1);
00673 
00674                 if( (uint)f.getNDX() > 0 ) {
00675                         return ACADOERROR( RET_INVALID_OPTION );
00676                 }
00677                 NDX3 = 0;
00678                 dummy4.clearStaticCounters();
00679                 dx = DifferentialStateDerivative("", NDX3, 1);
00680 
00681                 if( f.getNXA() > 0 ) {
00682                         return ACADOERROR( RET_INVALID_OPTION );
00683                 }
00684                 NXA3 = 0;
00685                 dummy3.clearStaticCounters();
00686                 z = AlgebraicState("", NXA3, 1);
00687 
00688                 uint i;
00689                 OutputFcn g;
00690                 for( i = 0; i < _rhs.getDim(); i++ ) {
00691                         g << forwardDerivative( _rhs(i), x );
00692 //                      g << forwardDerivative( _rhs(i), z );
00693                         g << forwardDerivative( _rhs(i), u );
00694 //                      g << forwardDerivative( _rhs(i), dx );
00695                 }
00696 
00697                 dummy2.clearStaticCounters();
00698                 x = DifferentialState("", NX, 1);
00699 
00700                 DMatrix dependencyMat = _rhs.getDependencyPattern( x );
00701                 DVector dependency = dependencyMat.sumRow();
00702                 for( i = n; i < NX; i++ ) {
00703                         if( acadoRoundAway(dependency(i)) != 0 ) { // This expression should not depend on these differential states
00704                                 return RET_UNABLE_TO_EXPORT_CODE;
00705                         }
00706                 }
00707 
00708                 OutputFcn f_large;
00709                 DMatrix A3_large = expandOutputMatrix(A3);
00710                 f_large << _rhs + A3_large*x;
00711 
00712                 return (rhs3.init(f_large, "acado_rhs3", NX, NXA, NU, NP, NDX, NOD) &
00713                                 diffs_rhs3.init(g, "acado_diffs3", NX, NXA, NU, NP, NDX, NOD));
00714         }
00715 
00716         return SUCCESSFUL_RETURN;
00717 }
00718 
00719 
00720 returnValue NARXExport::setLinearOutput( const DMatrix& M3, const DMatrix& A3, const std::string& _rhs3, const std::string& _diffs_rhs3 )
00721 {
00722         // You can't use this feature yet with NARX integrators !
00723         return ACADOERROR( RET_INVALID_OPTION );
00724 }
00725 
00726 
00727 returnValue NARXExport::formNARXpolynomial( const uint num, const uint order, uint& base, const uint index, IntermediateState& result ) {
00728 
00729         uint n = delay*(NX1+NX2);
00730         for( uint i = index; i < n; i++ ) {
00731                 if( order == 0 ) {      // compute sum
00732                         result = result + parms(num,base)*x(i);
00733                         base = base+1;
00734                 }
00735                 else {                          // recursive call
00736                         IntermediateState tmp;
00737                         formNARXpolynomial( num, order-1, base, i, tmp );
00738                         result = result + tmp*x(i);
00739                 }
00740         }
00741 
00742         return SUCCESSFUL_RETURN;
00743 }
00744 
00745 
00746 // PROTECTED:
00747 
00748 //
00749 // Register the integrator
00750 //
00751 
00752 IntegratorExport* createNARXExport(     UserInteraction* _userInteraction,
00753                                                                                                         const std::string &_commonHeaderName)
00754 {
00755         return new NARXExport(_userInteraction, _commonHeaderName);
00756 }
00757 
00758 
00759 
00760 CLOSE_NAMESPACE_ACADO
00761 
00762 // end of file.


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