integrator_bdf.hpp
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 
34 #ifndef ACADO_TOOLKIT_INTEGRATOR_BDF_HPP
35 #define ACADO_TOOLKIT_INTEGRATOR_BDF_HPP
36 
38 
40 
41 
52 class IntegratorBDF : public Integrator{
53 
54 
56 friend class ShootingMethod;
57 
58 //
59 // PUBLIC MEMBER FUNCTIONS:
60 //
61 
62 public:
63 
65  IntegratorBDF( );
66 
68  IntegratorBDF( const DifferentialEquation &rhs_ );
69 
71  IntegratorBDF( const IntegratorBDF& arg );
72 
74  virtual ~IntegratorBDF( );
75 
77  virtual IntegratorBDF& operator=( const IntegratorBDF& arg );
78 
79 
81  virtual Integrator* clone() const;
82 
83 
84  // ================================================================================
85 
94  virtual returnValue init( const DifferentialEquation &rhs_ );
95 
96 
108  inline returnValue init( const DifferentialEquation &rhs_,
109  const Transition &trs_ );
110 
111 
112  // ================================================================================
113 
123  virtual returnValue freezeMesh();
124 
125 
134  virtual returnValue freezeAll();
135 
136 
144  virtual returnValue unfreeze();
145 
146 
147  // ================================================================================
148 
149 
172  virtual returnValue step( int number );
173 
174 
183  virtual returnValue stop();
184 
185 
190  virtual returnValue setDxInitialization( double *dx0 );
194 
195 
196 
197  // ================================================================================
198 
199 
200 
204  virtual int getNumberOfSteps() const;
205 
206 
210  virtual int getNumberOfRejectedSteps() const;
211 
212 
213 
215  virtual double getStepSize() const;
216 
217 
218 
219 //
220 // PROTECTED MEMBER FUNCTIONS:
221 //
222 protected:
223 
224 
225 
229  virtual returnValue evaluate( const DVector &x0 ,
230  const DVector &xa ,
231  const DVector &p ,
232  const DVector &u ,
233  const DVector &w ,
234  const Grid &t_ );
235 
236  // ================================================================================
237 
238 
245 
246 
247  // ================================================================================
248 
249 
256  const DVector &pSeed ,
258  const DVector &uSeed ,
260  const DVector &wSeed ,
262  const int &order );
264 
265 
266  // ================================================================================
267 
268 
273  virtual returnValue setProtectedBackwardSeed( const DVector &seed ,
274  const int &order );
276 
277 
278  // ================================================================================
279 
280 
281 
285  virtual returnValue getProtectedX( DVector *xEnd ) const;
288 
289 
297  int order ) const;
298 
299 
300 
313  DVector &Dx_p ,
314  DVector &Dx_u ,
315  DVector &Dx_w ,
316  int order ) const;
317 
318 
319  // ================================================================================
320 
322  virtual int getDim() const;
323 
324 
326  virtual int getDimX() const;
327 
328 
331 
337 
338 
342  returnValue rk_start_solve( int stepnumber );
343 
344 
349  double determinePredictor( int number, BooleanType ini );
350 
351 
356  returnValue determineCorrector( int number, BooleanType ini );
357 
358 
362 
363 
366  void printBDFfinalResults();
367 
368 
371  void printBDFfinalResults2(DMatrix &div);
372 
373 
377 
378 
383  returnValue decomposeJacobian(int index, DMatrix &J );
384 
385 
389  double applyNewtonStep( int index, double *etakplus1, const double *etak, const DMatrix &J, const double *FFF );
390 
391 
396  void applyMTranspose( int index, double *seed1, DMatrix &J, double *seed2 );
397 
398 
403  const DVector &pSeed ,
405  const DVector &uSeed ,
407  const DVector &wSeed );
409 
410 
413  virtual returnValue setBackwardSeed2( const DVector &seed );
415 
416 
419  void determineRKEtaGForward();
420 
421 
425 
426 
430 
431 
435 
436 
439  void determineBDFEtaGForward( int number );
440 
443  void determineBDFEtaHBackward( int number );
444 
447  void determineBDFEtaGForward2( int number );
448 
451  void determineBDFEtaHBackward2( int number );
452 
453 
456  void deleteAll();
457 
458 
461  void constructAll( const IntegratorBDF& arg );
462 
463 
464 
471  void allocateMemory( );
472 
473 
479  void initializeVariables();
480 
481 
483  void relaxAlgebraic( double *residuum, double timePoint );
484 
485 
486  void prepareDividedDifferences( DMatrix &div );
487 
488  void interpolate( int number_, DMatrix &div, VariablesGrid &poly );
489 
490  void logCurrentIntegratorStep( const DVector& currentX = emptyConstVector,
491  const DVector& currentXA = emptyConstVector
492  );
493 
494 
495  void copyBackward( DVector &Dx_x0,
496  DVector &Dx_p ,
497  DVector &Dx_u ,
498  DVector &Dx_w ,
499  const DMatrix &div ) const;
500 
501 
502 // DATA MEMBERS:
503 //
504 protected:
505 
506 
507  // STORAGE OF THE ALGEBRAIC RESIDUUM :
508  // -----------------------------------
511 
512 
513  // BUTCHER-
514  // TABLEAU:
515  // ----------
516  int dim ;
517  double **A ;
518  double *b4 ;
519  double *b5 ;
520  double *c ;
523  // RK-STARTER:
524  // -----------
525  double *eta4 ;
526  double *eta5 ;
527  double *eta4_ ;
528  double *eta5_ ;
529  double ***k ;
530  double ***k2 ;
531  double ***l ;
532  double ***l2 ;
533  double *iseed ;
536  // BDF-METHOD:
537  // -----------
538  int nstep ;
539  double **psi ;
540  double *psi_ ;
541  double **gamma ;
549  DMatrix **M ;
550 // std::vector< Eigen::HouseholderQR< DMatrix::Base > > qr;
551  int *M_index ;
552  int nOfM ;
553  int maxNM ;
556  double **eta ;
557  double **eta2 ;
558  double t ;
559  double *x ;
561  double *F ;
562  double *F2 ;
563 
564 
565  // OTHERS:
566  // -------
567  double *initial_guess ;
568  double rel_time_scale ;
571  // SENSITIVITIES:
572  // --------------
573  int ndir ;
581  double *G ;
582  double *etaG ;
588  double *G2 ;
589  double *G3 ;
590  double *etaG2 ;
591  double *etaG3 ;
601  double *H ;
602  double **etaH ;
607  double ***kH ;
608  double **zH ;
610  double *H2 ;
611  double *H3 ;
612  double **etaH2 ;
613  double **etaH3 ;
622  double ***kH2 ;
623  double **zH2 ;
624  double ***kH3 ;
625  double **zH3 ;
628  // STORAGE:
629  // --------
630  int maxAlloc ;
634  // STATISTICS:
635  // -----------
640 
641 };
642 
643 
645 
646 
647 
648 #include <acado/integrator/integrator_bdf.ipp>
649 
650 
651 #endif // ACADO_TOOLKIT_INTEGRATOR_BDF_HPP
652 
653 // end of file.
double * initial_guess
virtual returnValue getProtectedBackwardSensitivities(DVector &Dx_x0, DVector &Dx_p, DVector &Dx_u, DVector &Dx_w, int order) const
void determineRKEtaGForward()
void printBDFfinalResults()
virtual returnValue getProtectedForwardSensitivities(DMatrix *Dx, int order) const
void constructAll(const IntegratorBDF &arg)
virtual int getNumberOfRejectedSteps() const
double applyNewtonStep(int index, double *etakplus1, const double *etak, const DMatrix &J, const double *FFF)
returnValue setForwardSeed2(const DVector &xSeed, const DVector &pSeed, const DVector &uSeed, const DVector &wSeed)
Allows real time measurements based on the system&#39;s clock.
Definition: real_clock.hpp:53
double determinePredictor(int number, BooleanType ini)
Implements the backward-differentiation formula for integrating DAEs.
RealClock jacComputation
virtual returnValue freezeAll()
virtual returnValue stop()
void determineBDFEtaHBackward(int number)
void logCurrentIntegratorStep(const DVector &currentX=emptyConstVector, const DVector &currentXA=emptyConstVector)
Discretizes a DifferentialEquation by means of single or multiple shooting.
void determineRKEtaGForward2()
Provides a time grid consisting of vector-valued optimization variables at each grid point...
virtual IntegratorBDF & operator=(const IntegratorBDF &arg)
Allows to pass back messages to the calling function.
RealClock correctorTime
void determineBDFEtaGForward(int number)
returnValue decomposeJacobian(int index, DMatrix &J)
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
virtual int getDim() const
void determineRKEtaHBackward()
virtual returnValue evaluate(const DVector &x0, const DVector &xa, const DVector &p, const DVector &u, const DVector &w, const Grid &t_)
void printRKIntermediateResults()
virtual ~IntegratorBDF()
returnValue rk_start_solve(int stepnumber)
#define CLOSE_NAMESPACE_ACADO
virtual Integrator * clone() const
Abstract base class for all kinds of algorithms for integrating differential equations (ODEs or DAEs)...
Definition: integrator.hpp:61
double * initialAlgebraicResiduum
void determineBDFEtaHBackward2(int number)
virtual returnValue evaluateSensitivities()
returnValue determineCorrector(int number, BooleanType ini)
double relaxationConstant
friend class SimulationByIntegration
static const DVector emptyConstVector
Definition: vector.hpp:336
virtual int getDimX() const
virtual returnValue step(int number)
virtual returnValue unfreeze()
void determineRKEtaHBackward2()
returnValue rk_start()
void printBDFIntermediateResults()
virtual returnValue setProtectedForwardSeed(const DVector &xSeed, const DVector &pSeed, const DVector &uSeed, const DVector &wSeed, const int &order)
RealClock jacDecomposition
void initializeVariables()
void interpolate(int number_, DMatrix &div, VariablesGrid &poly)
void relaxAlgebraic(double *residuum, double timePoint)
virtual returnValue setProtectedBackwardSeed(const DVector &seed, const int &order)
void printBDFfinalResults2(DMatrix &div)
virtual returnValue getProtectedX(DVector *xEnd) const
void prepareDividedDifferences(DMatrix &div)
virtual returnValue setBackwardSeed2(const DVector &seed)
void applyMTranspose(int index, double *seed1, DMatrix &J, double *seed2)
Allows to setup and evaluate transition functions based on SymbolicExpressions.
Definition: transition.hpp:53
void initializeButcherTableau()
virtual int getNumberOfSteps() const
virtual returnValue init(const DifferentialEquation &rhs_)
#define BEGIN_NAMESPACE_ACADO
virtual returnValue freezeMesh()
virtual returnValue setDxInitialization(double *dx0)
void determineBDFEtaGForward2(int number)
void copyBackward(DVector &Dx_x0, DVector &Dx_p, DVector &Dx_u, DVector &Dx_w, const DMatrix &div) const
virtual double getStepSize() const
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.


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