narx_export.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
27 
35 
36 using namespace std;
37 
39 
40 //
41 // PUBLIC MEMBER FUNCTIONS:
42 //
43 
45  const std::string& _commonHeaderName
46  ) : DiscreteTimeExport( _userInteraction,_commonHeaderName )
47 {
48  delay = 1;
49 }
50 
51 
53  ) : DiscreteTimeExport( arg )
54 {
55  delay = arg.delay;
56  parms = arg.parms;
57 
58  copy( arg );
59 }
60 
61 
63 {
64  clear( );
65 }
66 
67 
69 {
70 // NX = delay*(NX1+NX2)+NX3; // IMPORTANT for NARX models where the state space is increased because of the delay
71 
72  int useOMP;
73  get(CG_USE_OPENMP, useOMP);
74  ExportStruct structWspace;
75  structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
76 
77  LOG( LVL_DEBUG ) << "Preparing to export NARXExport... " << endl;
78 
79  ExportIndex run( "run" );
80  ExportIndex i( "i" );
81  ExportIndex j( "j" );
82  ExportIndex k( "k" );
83  ExportIndex tmp_index("tmp_index");
84  diffsDim = NX*(NX+NU);
85  inputDim = NX*(NX+NU+1) + NU + NOD;
86  // setup INTEGRATE function
87  rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
88  rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
89  if( equidistantControlGrid() ) {
90  integrate = ExportFunction( "integrate", rk_eta, reset_int );
91  }
92  else {
93  integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
94  }
96 
97  rk_eta.setDoc( "Working array to pass the input values and return the results." );
98  reset_int.setDoc( "The internal memory of the integrator can be reset." );
99  rk_index.setDoc( "Number of the shooting interval." );
100  error_code.setDoc( "Status code of the integrator." );
101  integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
102 
103  integrate.addIndex( run );
104  integrate.addIndex( i );
105  integrate.addIndex( j );
106  integrate.addIndex( k );
107  integrate.addIndex( tmp_index );
108  rhs_in = ExportVariable( "x", inputDim-diffsDim, 1, REAL, ACADO_LOCAL );
109  rhs_out = ExportVariable( "f", NX, 1, REAL, ACADO_LOCAL );
110  fullRhs = ExportFunction( "full_rhs", rhs_in, rhs_out );
111  rhs_in.setDoc( "The state and parameter values." );
112  rhs_out.setDoc( "Right-hand side evaluation." );
113  fullRhs.doc( "Evaluates the right-hand side of the full model." );
114  rk_xxx = ExportVariable( "rk_xxx", 1, inputDim-diffsDim, REAL, structWspace );
115  rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", delay*NX1, (delay-1)*NX1+NU, REAL, structWspace );
116  rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", delay*NX2, delay*(NX1+NX2)+NU, REAL, structWspace );
117  rk_diffsPrev3 = ExportVariable( "rk_diffsPrev3", NX3, NX+NU, REAL, structWspace );
118  rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX1, NX1+NU, REAL, structWspace );
119  rk_diffsNew2 = ExportVariable( "rk_diffsNew2", NX2, delay*(NX1+NX2), REAL, structWspace );
120  rk_diffsNew3 = ExportVariable( "rk_diffsNew3", NX3, NX+NU, REAL, structWspace );
121  rk_diffsTemp3 = ExportVariable( "rk_diffsTemp3", NX3, delay*(NX1+NX2)+NU, REAL, structWspace );
122 
123  ExportVariable numInt( "numInts", 1, 1, INT );
124  if( !equidistantControlGrid() ) {
125  ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
126  integrate.addStatement( std::string( "int " ) + numInt.getName() + " = " + numStepsV.getName() + "[" + rk_index.getName() + "];\n" );
127  }
128 
129  integrate.addStatement( rk_xxx.getCols( NX,inputDim-diffsDim ) == rk_eta.getCols( NX+diffsDim,inputDim ) );
131 
132  // Linear input:
133  DMatrix eyeM = eye<double>((delay-1)*NX1);
134  eyeM.appendRows( zeros<double>(NX1,(delay-1)*NX1) );
135  eyeM.appendCols( zeros<double>(delay*NX1,NU) );
136  if( NX1 > 0 ) {
138  }
139 
140  // Nonlinear part:
141  for( uint i1 = 0; i1 < delay; i1++ ) {
142  eyeM = zeros<double>(NX2,i1*(NX1+NX2)+NX1);
143  eyeM.appendCols( eye<double>(NX2) );
144  eyeM.appendCols( zeros<double>(NX2,(delay-i1-1)*(NX1+NX2)+NU) );
145  integrate.addStatement( rk_diffsPrev2.getRows(i1*NX2,i1*NX2+NX2) == eyeM );
146  }
147  // evaluate sensitivities linear input:
148  if( NX1 > 0 ) {
149  for( uint i1 = 0; i1 < NX1; i1++ ) {
150  for( uint i2 = 0; i2 < NX1; i2++ ) {
151  integrate.addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,i2,i2+1) == A11(i1,i2) );
152  }
153  for( uint i2 = 0; i2 < NU; i2++ ) {
154  integrate.addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,NX1+i2,NX1+i2+1) == B11(i1,i2) );
155  }
156  }
157  }
158  // evaluate sensitivities linear output:
159  if( NX1 > 0 ) {
160  for( uint i1 = 0; i1 < NX3; i1++ ) {
161  for( uint i2 = 0; i2 < NX3; i2++ ) {
162  integrate.addStatement( rk_diffsNew3.getSubMatrix(i1,i1+1,NX-NX3+i2,NX-NX3+i2+1) == A33(i1,i2) );
163  }
164  }
165  }
167 
168  // integrator loop:
169  ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
170  ExportStatementBlock *loop;
171  if( equidistantControlGrid() ) {
172  loop = &tmpLoop;
173  }
174  else {
175  loop = &integrate;
176  loop->addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
177  }
178 
179  loop->addStatement( rk_xxx.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) );
180 
181  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
182  loop->addStatement( std::string("if( run > 0 ) {\n") );
183  // SHIFT rk_diffsPrev:
184  // TODO: write using exportforloop
185  if( NX1 > 0 ) {
186  for( uint s = 1; s < delay; s++ ) {
187  loop->addStatement( rk_diffsPrev1.getRows(s*NX1,s*NX1+NX1) == rk_diffsPrev1.getRows((s-1)*NX1,s*NX1) );
188  }
189 
190  // Add to rk_diffsPrev:
191  ExportForLoop loopTemp1( i,0,NX1 );
192  loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+NX,i*NX+NX+NX1 ) );
193  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 ) );
194  loop->addStatement( loopTemp1 );
195  }
196 
197  if( NX2 > 0 ) {
198  for( uint s = 1; s < delay; s++ ) {
199  loop->addStatement( rk_diffsPrev2.getRows(s*NX2,s*NX2+NX2) == rk_diffsPrev2.getRows((s-1)*NX2,s*NX2) );
200  }
201 
202  // Add to rk_diffsPrev:
203  ExportForLoop loopTemp2( i,0,NX2 );
204  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) ) );
205  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 ) );
206  loop->addStatement( loopTemp2 );
207  }
208 
209  if( NX3 > 0 ) {
210  ExportForLoop loopTemp3( i,0,NX3 );
211  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 ) );
212  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 ) );
213  loop->addStatement( loopTemp3 );
214  }
215  loop->addStatement( std::string("}\n") );
216  }
217 
218  // evaluate states:
219  if( NX1 > 0 ) {
220  loop->addFunctionCall( lin_input.getName(), rk_xxx, rk_eta.getAddress(0,0) );
221 
222  }
223  if( NX2 > 0 ) {
224  loop->addFunctionCall( rhs.getName(), rk_xxx, rk_eta.getAddress(0,NX1) );
225  }
226  // shifting memory of NARX model:
227  for( uint s = 1; s < delay; s++ ) {
228  loop->addStatement( rk_eta.getCols(s*(NX1+NX2),s*(NX1+NX2)+(NX1+NX2)) == rk_xxx.getCols((s-1)*(NX1+NX2),s*(NX1+NX2)) );
229  }
230  if( NX3 > 0 ) {
231  loop->addFunctionCall( getNameOutputRHS(), rk_xxx, rk_eta.getAddress(0,delay*(NX1+NX2)) );
232  }
233 
234  // evaluate sensitivities:
235  if( NX2 > 0 ) {
236  loop->addFunctionCall( diffs_rhs.getName(), rk_xxx, rk_diffsNew2.getAddress(0,0) );
237  }
238  if( NX3 > 0 ) {
239  loop->addFunctionCall( getNameOutputDiffs(), rk_xxx, rk_diffsTemp3.getAddress(0,0) );
240  ExportForLoop loop1( i,0,NX3 );
241  ExportForLoop loop2( j,0,delay*(NX1+NX2) );
242  loop2.addStatement( rk_diffsNew3.getSubMatrix(i,i+1,j,j+1) == rk_diffsTemp3.getSubMatrix(i,i+1,j,j+1) );
243  loop1.addStatement( loop2 );
244  loop2 = ExportForLoop( j,0,NU );
245  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) );
246  loop1.addStatement( loop2 );
247  loop->addStatement( loop1 );
248  }
249 
250  // computation of the sensitivities using chain rule:
251  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
252  loop->addStatement( std::string( "if( run == 0 ) {\n" ) );
253  }
254  // PART 1
255  updateInputSystem(loop, i, j, tmp_index);
256  // PART 2
257  updateImplicitSystem(loop, i, j, tmp_index);
258  // PART 3
259  updateOutputSystem(loop, i, j, tmp_index);
260 
261  if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
262  loop->addStatement( std::string( "}\n" ) );
263  loop->addStatement( std::string( "else {\n" ) );
264  // PART 1
265  propagateInputSystem(loop, i, j, k, tmp_index);
266  // PART 2
267  propagateImplicitSystem(loop, i, j, k, tmp_index);
268  // PART 3
269  propagateOutputSystem(loop, i, j, k, tmp_index);
270  loop->addStatement( std::string( "}\n" ) );
271  }
272 
273  // end of the integrator loop.
274  if( !equidistantControlGrid() ) {
275  loop->addStatement( "}\n" );
276  }
277  else {
278  integrate.addStatement( *loop );
279  }
280 
281  // FILL IN ALL THE SENSITIVITIES OF THE DELAYED STATES BASED ON RK_DIFFSPREV + SPARSITY
282  if( NX1 > 0 ) {
283  DMatrix zeroR = zeros<double>(1, NX2);
284  ExportForLoop loop1( i,0,NX1 );
285  loop1.addStatement( rk_eta.getCols( i*NX+NX+NX1,i*NX+NX+NX1+NX2 ) == zeroR );
286  zeroR = zeros<double>(1, (delay-1)*(NX1+NX2)+NX3);
287  loop1.addStatement( rk_eta.getCols( i*NX+NX+NX1+NX2,i*NX+NX+NX ) == zeroR );
288  integrate.addStatement( loop1 );
289  for( uint s1 = 1; s1 < delay; s1++ ) {
290  ExportForLoop loop2( i,0,NX1 );
291  // STATES
292  zeroR = zeros<double>(1, NX2);
293  for( uint s2 = 0; s2 < s1; s2++ ) {
294  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 ) );
295  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 );
296  }
297  zeroR = zeros<double>(1, (delay-s1)*(NX1+NX2)+NX3);
298  loop2.addStatement( rk_eta.getCols( i*NX+NX+s1*(NX1+NX2)*NX+s1*(NX1+NX2),i*NX+NX+s1*(NX1+NX2)*NX+NX ) == zeroR );
299  // CONTROLS
300  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 ) );
301  integrate.addStatement( loop2 );
302  }
303  }
304  if( NX2 > 0 ) {
305  for( uint s = 0; s < delay; s++ ) {
306  ExportForLoop loop3( i,0,NX2 );
307  // STATES
308  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) ) );
309  DMatrix zeroR;
310  if( NX3 > 0 ) {
311  zeroR = zeros<double>(1, NX3);
312  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 );
313  }
314  // CONTROLS
315  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 ) );
316  integrate.addStatement( loop3 );
317  }
318  }
319 
320  LOG( LVL_DEBUG ) << "done" << endl;
321 
322  return SUCCESSFUL_RETURN;
323 }
324 
325 
327 
328  uint i, j;
329 
330  for( i = 1; i < delay; i++ ) {
332  }
333 
334  // PART 1:
335  for( i = 0; i < NX1; i++ ) {
337  for( j = 1; j < NX1; j++ ) {
338  if( acadoRoundAway(A11(i,j)) != 0 ) {
340  }
341  }
342  for( j = 0; j < NU; j++ ) {
343  if( acadoRoundAway(B11(i,j)) != 0 ) {
345  }
346  }
347  }
348 
349  // PART 2:
350  if( NX2 > 0 ) {
352  }
353 
354  // PART 3:
355  if( NX3 > 0 ) {
357  }
358 
359  return SUCCESSFUL_RETURN;
360 }
361 
362 
364 {
365  if( NX1 > 0 ) {
366  ExportForLoop loop01( index1,0,NX1 );
367  ExportForLoop loop02( index2,0,NX1 );
368  loop02.addStatement( tmp_index == index2+index1*NX );
369  loop02.addStatement( rk_eta.getCol( tmp_index+NX ) == rk_diffsNew1.getSubMatrix( index1,index1+1,index2,index2+1 ) );
370  loop01.addStatement( loop02 );
371 
372  if( NU > 0 ) {
373  ExportForLoop loop03( index2,0,NU );
374  loop03.addStatement( tmp_index == index2+index1*NU );
375  loop03.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX) ) == rk_diffsNew1.getSubMatrix( index1,index1+1,NX1+index2,NX1+index2+1 ) );
376  loop01.addStatement( loop03 );
377  }
378  block->addStatement( loop01 );
379  }
380 
381  return SUCCESSFUL_RETURN;
382 }
383 
384 
386 {
387  if( NX2 > 0 ) {
388  ExportForLoop loop01( index1,0,NX2 );
389  ExportForLoop loop02( index2,0,delay*(NX1+NX2) );
390  loop02.addStatement( tmp_index == index2+index1*NX );
391  loop02.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX ) == rk_diffsNew2.getSubMatrix( index1,index1+1,index2,index2+1 ) );
392  loop01.addStatement( loop02 );
393 
394  if( NU > 0 ) {
395  ExportForLoop loop03( index2,0,NU );
396  loop03.addStatement( tmp_index == index2+index1*NU );
397  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 !!
398  loop01.addStatement( loop03 );
399  }
400  block->addStatement( loop01 );
401  }
402  // ALGEBRAIC STATES: NONE
403 
404  return SUCCESSFUL_RETURN;
405 }
406 
407 
409 {
410  if( NX3 > 0 ) {
411  ExportForLoop loop01( index1,0,NX3 );
412  ExportForLoop loop02( index2,0,NX );
413  loop02.addStatement( tmp_index == index2+index1*NX );
414  loop02.addStatement( rk_eta.getCol( tmp_index+NX*(1+delay*(NX1+NX2)) ) == rk_diffsNew3.getSubMatrix( index1,index1+1,index2,index2+1 ) );
415  loop01.addStatement( loop02 );
416 
417  if( NU > 0 ) {
418  ExportForLoop loop03( index2,0,NU );
419  loop03.addStatement( tmp_index == index2+index1*NU );
420  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 ) );
421  loop01.addStatement( loop03 );
422  }
423  block->addStatement( loop01 );
424  }
425 
426  return SUCCESSFUL_RETURN;
427 }
428 
429 
431  const ExportIndex& index3, const ExportIndex& tmp_index )
432 {
433  if( NX1 > 0 ) {
434  ExportForLoop loop01( index1,0,NX1 );
435  ExportForLoop loop02( index2,0,NX1 );
436  loop02.addStatement( tmp_index == index2+index1*NX );
437  loop02.addStatement( rk_eta.getCol( tmp_index+NX ) == rk_diffsNew1.getSubMatrix( index1,index1+1,0,1 )*rk_diffsPrev1.getSubMatrix( 0,1,index2,index2+1 ) );
438  ExportForLoop loop03( index3,1,NX1 );
439  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 ) );
440  loop02.addStatement( loop03 );
441  loop01.addStatement( loop02 );
442 
443  if( NU > 0 ) {
444  ExportForLoop loop04( index2,0,NU );
445  loop04.addStatement( tmp_index == index2+index1*NU );
446  loop04.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX) ) == rk_diffsNew1.getSubMatrix( index1,index1+1,NX1+index2,NX1+index2+1 ) );
447  ExportForLoop loop05( index3,0,NX1 );
448  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 ) );
449  loop04.addStatement( loop05 );
450  loop01.addStatement( loop04 );
451  }
452  block->addStatement( loop01 );
453  }
454  return SUCCESSFUL_RETURN;
455 }
456 
457 
459  const ExportIndex& index3, const ExportIndex& tmp_index )
460 {
461  uint i;
462  if( NX2 > 0 ) {
463  ExportForLoop loop01( index1,0,NX2 );
464  for( uint s = 0; s < delay; s++ ) {
465  ExportForLoop loop02( index2,0,NX1 );
466  loop02.addStatement( tmp_index == index2+index1*NX );
467  loop02.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX+s*(NX1+NX2) ) == 0.0 );
468  for( i = 0; i < delay; i++ ) {
469  if( s < (delay-1) ) {
470  ExportForLoop loop03( index3,0,NX1 );
471  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 ) );
472  loop02.addStatement( loop03 );
473  }
474  ExportForLoop loop04( index3,0,NX2 );
475  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 ) );
476  loop02.addStatement( loop04 );
477  }
478  loop01.addStatement( loop02 );
479 
480  ExportForLoop loop05( index2,0,NX2 );
481  loop05.addStatement( tmp_index == index2+index1*NX );
482  loop05.addStatement( rk_eta.getCol( tmp_index+NX+NX1*NX+s*(NX1+NX2)+NX1 ) == 0.0 );
483  for( i = 0; i < delay; i++ ) {
484  ExportForLoop loop06( index3,0,NX2 );
485  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 ) );
486  loop05.addStatement( loop06 );
487  }
488  loop01.addStatement( loop05 );
489  }
490 
491  if( NU > 0 ) {
492  ExportForLoop loop07( index2,0,NU );
493  loop07.addStatement( tmp_index == index2+index1*NU );
494  loop07.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+NX1*NU ) == 0.0 );
495  for( i = 0; i < delay; i++ ) {
496  ExportForLoop loop08( index3,0,NX1 );
497  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 ) );
498  loop07.addStatement( loop08 );
499  ExportForLoop loop09( index3,0,NX2 );
500  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 ) );
501  loop07.addStatement( loop09 );
502  }
503  loop01.addStatement( loop07 );
504  }
505  block->addStatement( loop01 );
506  }
507  // ALGEBRAIC STATES: NO PROPAGATION OF SENSITIVITIES NEEDED
508 
509  return SUCCESSFUL_RETURN;
510 }
511 
512 
514  const ExportIndex& index3, const ExportIndex& tmp_index )
515 {
516  uint i;
517  if( NX3 > 0 ) {
518  ExportForLoop loop01( index1,0,NX3 );
519  for( uint s = 0; s < delay; s++ ) {
520  ExportForLoop loop02( index2,0,NX1 );
521  loop02.addStatement( tmp_index == index2+index1*NX );
522  loop02.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2) ) == 0.0 );
523  for( i = 0; i < delay; i++ ) {
524  if( s < (delay-1) ) {
525  ExportForLoop loop03( index3,0,NX1 );
526  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 ) );
527  loop02.addStatement( loop03 );
528  }
529  ExportForLoop loop04( index3,0,NX2 );
530  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 ) );
531  loop02.addStatement( loop04 );
532  }
533  ExportForLoop loop022( index3,0,NX3 );
534  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 ) );
535  loop02.addStatement( loop022 );
536  loop01.addStatement( loop02 );
537 
538  ExportForLoop loop05( index2,0,NX2 );
539  loop05.addStatement( tmp_index == index2+index1*NX );
540  loop05.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+s*(NX1+NX2)+NX1 ) == 0.0 );
541  for( i = 0; i < delay; i++ ) {
542  ExportForLoop loop06( index3,0,NX2 );
543  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 ) );
544  loop05.addStatement( loop06 );
545  }
546  ExportForLoop loop055( index3,0,NX3 );
547  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 ) );
548  loop05.addStatement( loop055 );
549  loop01.addStatement( loop05 );
550  }
551  ExportForLoop loop06( index2,0,NX3 );
552  loop06.addStatement( tmp_index == index2+index1*NX );
553  loop06.addStatement( rk_eta.getCol( tmp_index+NX+delay*(NX1+NX2)*NX+delay*(NX1+NX2) ) == 0.0 );
554  ExportForLoop loop061( index3,0,NX3 );
555  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 ) );
556  loop06.addStatement( loop061 );
557  loop01.addStatement( loop06 );
558 
559  if( NU > 0 ) {
560  ExportForLoop loop07( index2,0,NU );
561  loop07.addStatement( tmp_index == index2+index1*NU );
562  loop07.addStatement( rk_eta.getCol( tmp_index+NX*(1+NX)+delay*(NX1+NX2)*NU ) == 0.0 );
563  for( i = 0; i < delay; i++ ) {
564  ExportForLoop loop08( index3,0,NX1 );
565  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 ) );
566  loop07.addStatement( loop08 );
567  ExportForLoop loop09( index3,0,NX2 );
568  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 ) );
569  loop07.addStatement( loop09 );
570  }
571  ExportForLoop loop010( index3,0,NX3 );
572  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 ) );
573  loop07.addStatement( loop010 );
574  loop01.addStatement( loop07 );
575  }
576  block->addStatement( loop01 );
577  }
578 
579  return SUCCESSFUL_RETURN;
580 }
581 
582 
584 {
585 
586  return ACADOERROR( RET_INVALID_OPTION );
587 }
588 
589 
590 returnValue NARXExport::setModel( const std::string& _rhs, const std::string& _diffs_rhs ) {
591 
592  // You can't use this feature yet with NARX integrators !
593  return ACADOERROR( RET_INVALID_OPTION );
594 }
595 
596 
598  ExportStruct dataStruct
599  ) const
600 {
601 
602  return DiscreteTimeExport::getDataDeclarations( declarations, dataStruct );
603 }
604 
605 
606 returnValue NARXExport::setNARXmodel( const uint _delay, const DMatrix& _parms ) {
607 
608  NX2 = _parms.getNumRows();
609  delay = _delay;
610  parms = _parms;
611 
612  DifferentialState dummy;
613  dummy.clearStaticCounters();
614  uint n = _delay*(NX1+NX2); // IMPORTANT for NARX models where the state space is increased because of the delay
615  x = DifferentialState("", n, 1);
616 
617  OutputFcn narxFun;
618  OutputFcn narxDiff;
619  for( uint i = 0; i < NX2; i++ ) {
620  Expression expr;
621  expr = _parms(i,0);
622  if( _delay >= 1 ) {
623  IntermediateState sum;
624  for( uint j = 0; j < n; j++ ) {
625  sum = sum + _parms(i,j+1)*x(j);
626  }
627  expr = expr + sum;
628  uint indexParms = n+1;
629  for( uint j = 1; j < _delay; j++ ) {
630  IntermediateState tmp;
631  formNARXpolynomial(i, j, indexParms, 0, tmp);
632  expr = expr + tmp;
633  }
634  }
635 
636  narxFun << expr;
637  narxDiff << forwardDerivative( expr, x );
638  }
639  rhs.init( narxFun,"acado_NARX_fun",NX,NXA,NU );
640  diffs_rhs.init( narxDiff,"acado_NARX_diff",NX,NXA,NU );
641 
642  dummy.clearStaticCounters();
643  x = DifferentialState("", NX, 1);
644 
645  return SUCCESSFUL_RETURN;
646 }
647 
648 
650 {
651  if( !A3.isEmpty() ) {
652  if( A3.getNumRows() != M3.getNumRows() || M3.getNumRows() != M3.getNumCols() || A3.getNumRows() != A3.getNumCols() || A3.getNumRows() != _rhs.getDim() ) {
654  }
655  NX3 = A3.getNumRows();
656  M33 = M3;
657  A33 = A3;
658 
659  OutputFcn f;
660  f << _rhs;
661  OnlineData dummy0;
662  Control dummy1;
663  DifferentialState dummy2;
664  AlgebraicState dummy3;
666  dummy0.clearStaticCounters();
667  dummy1.clearStaticCounters();
668  dummy2.clearStaticCounters();
669  uint n = delay*(NX1+NX2);
670  x = DifferentialState("", n, 1);
671  u = Control("", NU, 1);
672  od = OnlineData("", NOD, 1);
673 
674  if( (uint)f.getNDX() > 0 ) {
675  return ACADOERROR( RET_INVALID_OPTION );
676  }
677  NDX3 = 0;
678  dummy4.clearStaticCounters();
680 
681  if( f.getNXA() > 0 ) {
682  return ACADOERROR( RET_INVALID_OPTION );
683  }
684  NXA3 = 0;
685  dummy3.clearStaticCounters();
686  z = AlgebraicState("", NXA3, 1);
687 
688  uint i;
689  OutputFcn g;
690  for( i = 0; i < _rhs.getDim(); i++ ) {
691  g << forwardDerivative( _rhs(i), x );
692 // g << forwardDerivative( _rhs(i), z );
693  g << forwardDerivative( _rhs(i), u );
694 // g << forwardDerivative( _rhs(i), dx );
695  }
696 
697  dummy2.clearStaticCounters();
698  x = DifferentialState("", NX, 1);
699 
700  DMatrix dependencyMat = _rhs.getDependencyPattern( x );
701  DVector dependency = dependencyMat.sumRow();
702  for( i = n; i < NX; i++ ) {
703  if( acadoRoundAway(dependency(i)) != 0 ) { // This expression should not depend on these differential states
705  }
706  }
707 
708  OutputFcn f_large;
709  DMatrix A3_large = expandOutputMatrix(A3);
710  f_large << _rhs + A3_large*x;
711 
712  return (rhs3.init(f_large, "rhs3", NX, NXA, NU, NP, NDX, NOD) &
713  diffs_rhs3.init(g, "diffs3", NX, NXA, NU, NP, NDX, NOD));
714  }
715 
716  return SUCCESSFUL_RETURN;
717 }
718 
719 
720 returnValue NARXExport::setLinearOutput( const DMatrix& M3, const DMatrix& A3, const std::string& _rhs3, const std::string& _diffs_rhs3 )
721 {
722  // You can't use this feature yet with NARX integrators !
723  return ACADOERROR( RET_INVALID_OPTION );
724 }
725 
726 
727 returnValue NARXExport::formNARXpolynomial( const uint num, const uint order, uint& base, const uint index, IntermediateState& result ) {
728 
729  uint n = delay*(NX1+NX2);
730  for( uint i = index; i < n; i++ ) {
731  if( order == 0 ) { // compute sum
732  result = result + parms(num,base)*x(i);
733  base = base+1;
734  }
735  else { // recursive call
736  IntermediateState tmp;
737  formNARXpolynomial( num, order-1, base, i, tmp );
738  result = result + tmp*x(i);
739  }
740  }
741 
742  return SUCCESSFUL_RETURN;
743 }
744 
745 
746 // PROTECTED:
747 
748 //
749 // Register the integrator
750 //
751 
753  const std::string &_commonHeaderName)
754 {
755  return new NARXExport(_userInteraction, _commonHeaderName);
756 }
757 
758 
759 
761 
762 // end of file.
ExportVariable rk_diffsPrev1
#define LOG(level)
Just define a handy macro for getting the logger.
Lowest level, the debug level.
virtual returnValue copy(const DiscreteTimeExport &arg)
returnValue updateImplicitSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index)
bool isEmpty() const
Definition: matrix.hpp:193
ExportVariable getRow(const ExportIndex &idx) const
Allows to setup and evaluate output functions based on SymbolicExpressions.
Definition: output_fcn.hpp:55
GenericMatrix & appendCols(const GenericMatrix &_arg)
Definition: matrix.cpp:83
virtual returnValue updateOutputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index)
ExportAcadoFunction diffs_rhs
int getNDX() const
Definition: function.cpp:217
ExportVariable rk_index
returnValue propagateInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index)
ExportVariable rk_diffsPrev3
Allows to pass back messages to the calling function.
DifferentialState x
returnValue setNARXmodel(const uint _delay, const DMatrix &_parms)
Expression forwardDerivative(const Expression &arg1, const Expression &arg2)
Block< Derived > block(Index startRow, Index startCol, Index blockRows, Index blockCols)
Definition: BlockMethods.h:56
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
ExportVariable rk_diffsPrev2
DMatrix expandOutputMatrix(const DMatrix &A3)
ExportVariable rhs_out
Allows to export code of a for-loop.
GenericMatrix & appendRows(const GenericMatrix &_arg)
Definition: matrix.cpp:62
const std::string getNameOutputDiffs() const
#define CLOSE_NAMESPACE_ACADO
ExportVariable rk_diffsNew3
int getNXA() const
Definition: function.cpp:212
ExportVariable getSubMatrix(const ExportIndex &rowIdx1, const ExportIndex &rowIdx2, const ExportIndex &colIdx1, const ExportIndex &colIdx2) const
Defines a scalar-valued index variable to be used for exporting code.
virtual ~NARXExport()
Definition: narx_export.cpp:62
ExportAcadoFunction rhs3
ExportVariable rhs_in
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
virtual returnValue setDoc(const std::string &_doc)
ExportVariable rk_eta
ExportAcadoFunction diffs_rhs3
ExportStruct
virtual ExportFunction & doc(const std::string &_doc)
returnValue prepareFullRhs()
const std::string getNameOutputRHS() const
ExportVariable getCols(const ExportIndex &idx1, const ExportIndex &idx2) const
int acadoRoundAway(double x)
NARXExport(UserInteraction *_userInteraction=0, const std::string &_commonHeaderName="")
Definition: narx_export.cpp:44
const std::string getNameRHS() const
ExportVariable reset_int
Allows to export a tailored discrete-time &#39;integrator&#39; for fast model predictive control.
ExportVariable rk_diffsTemp3
Encapsulates all user interaction for setting options, logging data and plotting results.
virtual returnValue propagateOutputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index)
Allows to export code of an arbitrary function.
returnValue addStatement(const ExportStatement &_statement)
returnValue setModel(const std::string &_rhs, const std::string &_diffs_rhs)
ExportFunction integrate
ExportFunction fullRhs
unsigned getNumRows() const
Definition: matrix.hpp:185
returnValue addLinebreak(uint num=1)
virtual returnValue setup()
Definition: narx_export.cpp:68
Allows to export a tailored integrator for fast model predictive control.
ExportFunction & setReturnValue(const ExportVariable &_functionReturnValue, bool _returnAsPointer=false)
ExportVariable rk_diffsNew2
unsigned getNumCols() const
Definition: matrix.hpp:189
virtual returnValue setDifferentialEquation(const Expression &rhs)
uint getNumIntervals() const
Allows to export a tailored polynomial NARX integrator for fast model predictive control.
Definition: narx_export.hpp:54
GenericVector< T > sumRow() const
Definition: matrix.cpp:114
ExportVariable rk_xxx
DifferentialStateDerivative dx
std::string getName() const
ExportVariable getRows(const ExportIndex &idx1, const ExportIndex &idx2) const
ExportAcadoFunction lin_input
IntegratorExport * createNARXExport(UserInteraction *_userInteraction, const std::string &_commonHeaderName)
#define BEGIN_NAMESPACE_ACADO
ExportVariable error_code
returnValue clearStaticCounters()
Definition: expression.hpp:398
ExportVariable rk_diffsNew1
virtual returnValue clear()
Allows to export code for a block of statements.
returnValue formNARXpolynomial(const uint num, const uint order, uint &base, const uint index, IntermediateState &result)
DMatrix parms
returnValue propagateImplicitSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &index3, const ExportIndex &tmp_index)
ExportArgument getAddress(const ExportIndex &_rowIdx, const ExportIndex &_colIdx=emptyConstExportIndex) const
virtual returnValue setLinearOutput(const DMatrix &M3, const DMatrix &A3, const Expression &rhs)
ExportVariable getCol(const ExportIndex &idx) const
returnValue init(const Function &_f, const std::string &_name="acadoFcn", const uint _numX=0, const uint _numXA=0, const uint _numU=0, const uint _numP=0, const uint _numDX=0, const uint _numOD=0)
virtual returnValue getDataDeclarations(ExportStatementBlock &declarations, ExportStruct dataStruct=ACADO_ANY) const
ExportFunction & addIndex(const ExportIndex &_index)
#define ACADOERROR(retval)
virtual bool equidistantControlGrid() const
Defines a matrix-valued variable to be used for exporting code.
ExportAcadoFunction rhs
returnValue addFunctionCall(const std::string &_fName, const ExportArgument &_argument1=emptyConstExportArgument, const ExportArgument &_argument2=emptyConstExportArgument, const ExportArgument &_argument3=emptyConstExportArgument, const ExportArgument &_argument4=emptyConstExportArgument, const ExportArgument &_argument5=emptyConstExportArgument, const ExportArgument &_argument6=emptyConstExportArgument, const ExportArgument &_argument7=emptyConstExportArgument, const ExportArgument &_argument8=emptyConstExportArgument, const ExportArgument &_argument9=emptyConstExportArgument)
std::string getName() const
Definition: export_data.cpp:86
returnValue updateInputSystem(ExportStatementBlock *block, const ExportIndex &index1, const ExportIndex &index2, const ExportIndex &tmp_index)
uint getDim() const


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