71 b4 =
new double [
dim];
72 b5 =
new double [
dim];
75 for( run1 = 0; run1 <
dim; run1++ ){
76 A[run1] =
new double[
dim];
92 b4 =
new double [
dim];
93 b5 =
new double [
dim];
96 for( run1 = 0; run1 <
dim; run1++ ){
97 A[run1] =
new double[
dim];
148 k = 0;
k2 = 0;
l = 0;
l2 = 0;
x = 0;
190 eta4 =
new double [
m];
191 eta5 =
new double [
m];
195 for( run1 = 0; run1 <
m; run1++ ){
203 k =
new double*[
dim];
204 k2 =
new double*[
dim];
205 l =
new double*[
dim];
206 l2 =
new double*[
dim];
216 for( run1 = 0; run1 <
dim; run1++ ){
217 k [run1] =
new double[
m];
218 k2 [run1] =
new double[
m];
220 for( run2 = 0; run2 <
m; run2++ ){
221 k [run1][run2] = 0.0;
222 k2[run1][run2] = 0.0;
229 l [run1][run2] = 0.0;
230 l2[run1][run2] = 0.0;
238 for( run1 = 0; run1 <
m; run1++ ){
247 for( run1 = 0; run1 <
mu; run1++ ){
253 for( run1 = 0; run1 <
mp; run1++ ){
259 for( run1 = 0; run1 <
mui; run1++ ){
265 for( run1 = 0; run1 <
mpi; run1++ ){
271 for( run1 = 0; run1 <
mw; run1++ ){
278 for( run1 = 0; run1 <
m; run1++ )
330 for( run1 = 0; run1 <
dim; run1++ ){
355 for( run1 = 0; run1 <
dim; run1++ ){
356 if(
k[run1] != NULL )
358 if(
k2[run1] != NULL )
360 if(
l[run1] != NULL )
362 if(
l2[run1] != NULL )
426 if(
Y[run1] != NULL )
475 A =
new double*[
dim];
476 b4 =
new double [
dim];
477 b5 =
new double [
dim];
478 c =
new double [
dim];
480 for( run1 = 0; run1 <
dim; run1++ ){
482 b4[run1] = arg.
b4[run1];
483 b5[run1] = arg.
b5[run1];
484 c [run1] = arg.
c [run1];
486 A[run1] =
new double[
dim];
487 for( run2 = 0; run2 <
dim; run2++ )
488 A[run1][run2] = arg.
A[run1][run2];
493 eta4 =
new double [
m];
494 eta5 =
new double [
m];
498 for( run1 = 0; run1 <
m; run1++ ){
506 k =
new double*[
dim];
507 k2 =
new double*[
dim];
508 l =
new double*[
dim];
509 l2 =
new double*[
dim];
513 x[run1] = arg.
x[run1];
519 for( run1 = 0; run1 <
dim; run1++ ){
520 k [run1] =
new double[
m];
521 k2 [run1] =
new double[
m];
523 for( run2 = 0; run2 <
m; run2++ ){
524 k [run1][run2] = arg.
k [run1][run2];
525 k2[run1][run2] = arg.
k2[run1][run2];
532 l [run1][run2] = arg.
l [run1][run2];
533 l2[run1][run2] = arg.
l2[run1][run2];
540 h = (
double*)calloc(arg.
maxAlloc,
sizeof(
double));
541 for( run1 = 0; run1 < arg.
maxAlloc; run1++ ){
542 h[run1] = arg.
h[run1];
558 for( run1 = 0; run1 <
m; run1++ ){
567 for( run1 = 0; run1 <
mu; run1++ ){
573 for( run1 = 0; run1 <
mp; run1++ ){
579 for( run1 = 0; run1 <
mui; run1++ ){
585 for( run1 = 0; run1 <
mpi; run1++ ){
591 for( run1 = 0; run1 <
mw; run1++ ){
665 Y[run1][run2] = arg.
Y[run1][run2];
705 h = (
double*)realloc(
h,
maxAlloc*
sizeof(
double));
749 if(
h[0] < 10.0*
EPS )
759 printf(
"x0dim = %d m=%d \n",(
int) x0.
getDim(),
m);
763 for( run1 = 0; run1 <
m; run1++ ){
764 eta4[run1] = x0(run1);
765 eta5[run1] = x0(run1);
766 xStore(0,run1) = x0(run1);
770 for( run1 = 0; run1 <
m; run1++ ){
779 for( run1 = 0; run1 <
mp; run1++ ){
788 for( run1 = 0; run1 <
mu; run1++ ){
798 for( run1 = 0; run1 <
mw; run1++ ){
814 for( run1 = 0; run1 <
m; run1++ )
824 cout <<
"RK: t = " <<
t <<
"\t";
826 for( run1 = 0; run1 <
m; run1++ ){
827 cout <<
"x[" << run1 <<
"] = " << scientific <<
eta4[run1] <<
" ";
846 for( run1 = 0; run1 <
mn; run1++ )
878 cout <<
"\n Results at t = " <<
t <<
"\t";
880 for( run1 = 0; run1 <
m; run1++ ){
881 cout <<
"x[" << run1 <<
"] = " << scientific <<
eta4[ run1 ];
888 int printIntegratorProfile = 0;
898 cout <<
"RK: number of steps: " <<
count - 1 << endl;
916 if( order < 1 || order > 2 ){
947 etaG =
new double[
m];
949 if( xSeed.
getDim() != 0 ){
950 for( run2 = 0; run2 <
m; run2++ ){
956 if( pSeed.
getDim() != 0 ){
957 for( run2 = 0; run2 <
mp; run2++ ){
964 if( uSeed.
getDim() != 0 ){
965 for( run2 = 0; run2 <
mu; run2++ ){
972 if( wSeed.
getDim() != 0 ){
974 for( run2 = 0; run2 <
mw; run2++ ){
1003 if(
etaG2 != NULL ){
1008 if(
etaG3 != NULL ){
1028 if( xSeed.
getDim() != 0 ){
1029 for( run2 = 0; run2 <
m; run2++ ){
1034 if( pSeed.
getDim() != 0 ){
1035 for( run2 = 0; run2 <
mp; run2++ ){
1041 if( uSeed.
getDim() != 0 ){
1042 for( run2 = 0; run2 <
mu; run2++ ){
1048 if( wSeed.
getDim() != 0 ){
1049 for( run2 = 0; run2 <
mw; run2++ ){
1063 if( order < 1 || order > 2 ){
1095 if( seed.
getDim() != 0 ){
1096 for( run2 = 0; run2 <
m; run2++ ){
1097 bseed(run2) = seed(run2);
1120 if(
etaH2 != NULL ){
1125 if(
etaH3 != NULL ){
1142 if( seed.
getDim() != 0 ){
1143 for( run2 = 0; run2 <
m; run2++ ){
1144 bseed2(run2) = seed(run2);
1169 for( run1 = 0; run1 <
m; run1++ ){
1181 for( run1 = 0; run1 <
m; run1++ ){
1189 for( run1 = 0; run1 <
m; run1++ ){
1203 for( run1 = 0; run1 <
m; run1++ ){
1217 int oldCount =
count;
1241 double a_my=0.0, b_my=0.0, c_my=0.0;
1244 for( run2 = 0; run2 < (int)(
lyap.
x1).getDim(); run2++ ){
1246 if (
etaG[run2] >= 1.0)
1252 if (
etaG[run2] >= 1.0)
1258 if (
etaG[run2] >= 1.0)
1264 if (b_my>=1.0 || c_my<=0.0){
1273 for( run2 = 0; run2 <
m; run2++ ){
1274 Y[run2][counter_my]=
etaG[run2];
1282 int counter_my1=counter_my-dimxmy12;
1284 int counter_my2 = counter_my-dimxmy12-counter_my1*(
lyap.
x1).
getDim();
1285 for( run2 = 0; run2 < dimxmy12; run2++ )
1288 for( run4 = 0; run4 < (int)(
lyap.
x1).getDim(); run4++ )
1289 for( run5 = 0; run5 < (int)(
lyap.
x1).getDim(); run5++ )
1291 etaG[dimxmy12+(run4)*(
lyap.
x1).
getDim()+run5] =
Y[run4][counter_my1]*
Y[run5][counter_my2];
1303 for( run1 = 0; run1 <
m; run1++ )
1307 for( run1 = 0; run1 <
m; run1++ )
1352 int number_of_rejected_steps = 0;
1360 while( E >=
TOL*
h[0] ){
1363 cout <<
"STEP REJECTED: error estimate = " << scientific << E << endl
1364 <<
" required local tolerance = " <<
TOL * h[0] << endl;
1367 number_of_rejected_steps++;
1369 for( run1 = 0; run1 <
m; run1++ ){
1399 count3 += number_of_rejected_steps;
1405 double *etaG_ =
new double[
m];
1406 double *etaG3_ =
new double[
m];
1414 for( run1 = 0; run1 <
m; run1++ )
1415 etaG_[run1] =
etaG[run1];
1440 for( run1 = 0; run1 <
m; run1++ )
1441 etaG3_[run1] =
etaG3[run1];
1480 cout <<
"RK: t = " << scientific <<
t <<
" h = " <<
h[ 0 ] <<
" ";
1493 h = (
double*)realloc(
h,
maxAlloc*
sizeof(
double));
1502 for( jj = i1+1; jj <= i2; jj++ ){
1508 for( run1 = 0; run1 <
mn; run1++ )
1522 for( run1 = 0; run1 <
m; run1++ ){
1547 for( run1 = 0; run1 <
m; run1++ )
1556 if( E < Emin ) E = Emin ;
1557 if( E < 10.0*
EPS ) E = 10.0*
EPS;
1591 if( (
int) xEnd[0].
getDim() !=
m )
1594 for( run1 = 0; run1 <
m; run1++ )
1595 xEnd[0](run1) =
eta4[run1];
1610 if( order == 1 &&
nFDirs2 == 0 ){
1611 for( run1 = 0; run1 <
m; run1++ ){
1612 Dx[0](run1,0) =
etaG[run1];
1619 for( run1 = 0; run1 <
m; run1++ ){
1620 Dx[0](run1,0) =
etaG3[run1];
1624 if( order == 1 &&
nFDirs2 > 0 ){
1625 for( run1 = 0; run1 <
m; run1++ ){
1626 Dx[0](run1,0) =
etaG2[run1];
1630 if( order < 1 || order > 2 ){
1646 if( order == 1 &&
nBDirs2 == 0 ){
1648 if( Dx_x0.
getDim() != 0 ){
1649 for( run2 = 0; run2 <
m; run2++ )
1652 if( Dx_p.
getDim() != 0 ){
1653 for( run2 = 0; run2 <
mp; run2++ ){
1657 if( Dx_u.
getDim() != 0 ){
1658 for( run2 = 0; run2 <
mu; run2++ ){
1662 if( Dx_w.
getDim() != 0 ){
1663 for( run2 = 0; run2 <
mw; run2++ ){
1669 if( order == 1 &&
nBDirs2 > 0 ){
1671 if( Dx_x0.
getDim() != 0 ){
1672 for( run2 = 0; run2 <
m; run2++ ){
1676 if( Dx_u.
getDim() != 0 ){
1677 for( run2 = 0; run2 <
mu; run2++ ){
1681 if( Dx_p.
getDim() != 0 ){
1682 for( run2 = 0; run2 <
mp; run2++ ){
1686 if( Dx_w.
getDim() != 0 ){
1687 for( run2 = 0; run2 <
mw; run2++ ){
1696 if( Dx_x0.
getDim() != 0 ){
1697 for( run2 = 0; run2 <
m; run2++ ){
1701 if( Dx_u.
getDim() != 0 ){
1702 for( run2 = 0; run2 <
mu; run2++ ){
1706 if( Dx_p.
getDim() != 0 ){
1707 for( run2 = 0; run2 <
mp; run2++ ){
1711 if( Dx_w.
getDim() != 0 ){
1712 for( run2 = 0; run2 <
mw; run2++ ){
1718 if( order < 1 || order > 2 ){
1756 int run1, run2, run3;
1761 for( run1 = 0; run1 <
dim; run1++ ){
1763 for( run2 = 0; run2 <
m; run2++ ){
1765 for( run3 = 0; run3 < run1; run3++ ){
1766 x[diff_index[run2]] =
x[diff_index[run2]] +
1767 A[run1][run3]*h[0]*
k[run3][run2];
1784 for( run1 = 0; run1 <
m; run1++ ){
1793 for( run1 = 0; run1 <
dim; run1++ ){
1794 for( run2 = 0; run2 <
m; run2++ ){
1796 eta5[run2] =
eta5[run2] +
b5[run1]*
h[0]*k[run1][run2];
1804 for( run2 = 0; run2 <
m; run2++ ){
1820 int run1, run2, run3;
1825 for( run1 = 0; run1 <
dim; run1++ ){
1827 for( run2 = 0; run2 <
m; run2++ ){
1829 for( run3 = 0; run3 < run1; run3++ ){
1830 x[diff_index[run2]] =
x[diff_index[run2]] +
1831 A[run1][run3]*h[0]*
k[run3][run2];
1848 for( run1 = 0; run1 <
m; run1++ ){
1855 for( run1 = 0; run1 <
dim; run1++ ){
1856 for( run2 = 0; run2 <
m; run2++ ){
1858 eta5[run2] =
eta5[run2] +
b5[run1]*
h[0]*k[run1][run2];
1866 for( run2 = 0; run2 <
m; run2++ ){
1879 int run1, run2, run3;
1883 for( run1 = 0; run1 <
dim; run1++ ){
1884 for( run2 = 0; run2 <
m; run2++ ){
1886 for( run3 = 0; run3 < run1; run3++ ){
1887 G[diff_index[run2]] =
G[diff_index[run2]] +
1888 A[run1][run3]*
h[0]*
k[run3][run2];
1899 for( run1 = 0; run1 <
dim; run1++ ){
1900 for( run2 = 0; run2 <
m; run2++ ){
1910 int run1, run2, run3;
1914 for( run1 = 0; run1 <
dim; run1++ ){
1915 for( run2 = 0; run2 <
m; run2++ ){
1917 G3[diff_index[run2]] =
etaG3[run2];
1918 for( run3 = 0; run3 < run1; run3++ ){
1919 G2[diff_index[run2]] =
G2[diff_index[run2]] +
1920 A[run1][run3]*
h[0]*
k[run3][run2];
1921 G3[diff_index[run2]] =
G3[diff_index[run2]] +
1922 A[run1][run3]*
h[0]*
k2[run3][run2];
1926 if(
rhs[0].AD_forward2( number_+run1,
G2,
G3,
k[run1],
k2[run1] )
1936 for( run1 = 0; run1 <
dim; run1++ ){
1937 for( run2 = 0; run2 <
m; run2++ ){
1948 int run1, run2, run3;
1951 for( run1 = 0; run1 <
dim; run1++ ){
1952 for( run2 = 0; run2 <
ndir; run2++ ){
1953 l[run1][run2] = 0.0;
1956 for( run1 = dim-1; run1 >= 0; run1--){
1957 for( run2 = 0; run2 <
m; run2++ ){
1959 for( run3 = run1+1; run3 <
dim; run3++ ){
1960 H[run2] =
H[run2] +
A[run3][run1]*
h[0]*
l[run3][diff_index[run2]];
1972 for( run1 = 0; run1 <
dim; run1++ ){
1973 for( run2 = 0; run2 <
ndir; run2++ ){
1974 etaH[run2] =
etaH[run2] +
l[run1][run2];
1982 int run1, run2, run3;
1985 for( run1 = 0; run1 <
dim; run1++ ){
1986 for( run2 = 0; run2 <
ndir; run2++ ){
1987 l [run1][run2] = 0.0;
1988 l2[run1][run2] = 0.0;
1992 for( run1 = dim-1; run1 >= 0; run1--){
1993 for( run2 = 0; run2 <
m; run2++ ){
1995 H3[run2] =
b4[run1]*
h[0]*
etaH3[diff_index[run2]];
1996 for( run3 = run1+1; run3 <
dim; run3++ ){
1997 H2[run2] =
H2[run2] +
A[run3][run1]*
h[0]*
l[run3][diff_index[run2]];
1998 H3[run2] =
H3[run2] +
A[run3][run1]*
h[0]*
l2[run3][diff_index[run2]];
2001 if(
rhs[0].AD_backward2( number_+run1,
H2,
H3,
l[run1],
l2[run1] )
2011 for( run1 = 0; run1 <
dim; run1++ ){
2012 for( run2 = 0; run2 <
ndir; run2++ ){
2025 for( run1 = 0; run1 <
m; run1++ ){
2026 cout <<
"x[" << run1 <<
"] = " <<
eta4[run1] <<
" ";
2039 cout <<
"RK: Forward Sensitivities:\n";
2040 for( run1 = 0; run1 <
m; run1++ ){
2041 cout << scientific <<
etaG[run1] <<
" ";
2052 cout <<
"RK: First Order Forward Sensitivities:\n";
2053 for( run1 = 0; run1 <
m; run1++ ){
2054 cout << scientific <<
etaG2[run1] <<
" ";
2058 cout <<
"RK: Second Order Forward Sensitivities:\n";
2059 for( run1 = 0; run1 <
m; run1++ ){
2060 cout << scientific <<
etaG3[run1] <<
" ";
2070 cout <<
"RK: Backward Sensitivities:\n" <<
"w.r.t. the states:\n";
2071 for( run2 = 0; run2 <
m; run2++ ){
2077 cout <<
"w.r.t. the controls:\n";
2078 for( run2 = 0; run2 <
mu; run2++ ){
2084 cout <<
"w.r.t. the parameters:\n";
2085 for( run2 = 0; run2 <
mp; run2++ ){
2091 cout <<
"w.r.t. the disturbances:\n";
2092 for( run2 = 0; run2 <
mw; run2++ ){
2105 cout <<
"RK: First order Backward Sensitivities:\n";
2107 cout <<
"w.r.t. the states:\n";
2108 for( run2 = 0; run2 <
m; run2++ ){
2114 cout <<
"w.r.t. the controls:\n";
2115 for( run2 = 0; run2 <
mu; run2++ ){
2121 cout <<
"w.r.t. the parameters:\n";
2122 for( run2 = 0; run2 <
mp; run2++ ){
2128 cout <<
"w.r.t. the disturbances:\n" << scientific;
2129 for( run2 = 0; run2 <
mw; run2++ ){
2135 cout <<
"RK: Second order Backward Sensitivities:\n";
2137 cout <<
"w.r.t. the states:\n" << scientific;;
2138 for( run2 = 0; run2 <
m; run2++ ){
2144 cout <<
"w.r.t. the controls:\n" << scientific;
2145 for( run2 = 0; run2 <
mu; run2++ ){
2152 cout <<
"w.r.t. the parameters:\n";
2153 for( run2 = 0; run2 <
mp; run2++ ){
2160 cout <<
"w.r.t. the disturbances:\n";
2161 for( run2 = 0; run2 <
mw; run2++ ){
2173 for( run1 = 0; run1 <
m; run1++ ){
2175 double cc = e1[run1];
2176 double bb = d1[run1];
2177 double aa = (e2[run1] - bb*
h[0] - cc)/(
h[0]*
h[0]);
2181 poly( jj, run1 ) = aa*tt*tt + bb*tt + cc;
returnValue setLast(LogName _name, int lastValue, double time=-INFTY)
int getStateEnumerationIndex(int index_)
virtual returnValue setProtectedBackwardSeed(const DVector &seed, const int &order)
virtual ~IntegratorLYAPUNOV()
IntermediateState sqrt(const Expression &arg)
void printIntermediateResults()
virtual returnValue unfreeze()
virtual returnValue setDxInitialization(double *dx0)
void constructAll(const IntegratorLYAPUNOV &arg)
void determineEtaHBackward2(int number)
void determineEtaGForward2(int number)
virtual returnValue freezeMesh()
double getTime(uint pointIdx) const
virtual returnValue step(int number)
int * int_parameter_index
virtual int getNumberOfRejectedSteps() const
double getFirstTime() const
virtual returnValue init(const DifferentialEquation &rhs_)
virtual returnValue freezeAll()
BEGIN_NAMESPACE_ACADO const double EPS
void init(unsigned _dim=0)
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Allows to pass back messages to the calling function.
virtual returnValue getProtectedX(DVector *xEnd) const
AlgorithmicBase & operator=(const AlgorithmicBase &rhs)
IntermediateState pow(const Expression &arg1, const Expression &arg2)
virtual returnValue setProtectedForwardSeed(const DVector &xSeed, const DVector &pSeed, const DVector &uSeed, const DVector &wSeed, const int &order)
Allows to conveniently handle (one-dimensional) grids consisting of time points.
RealClock functionEvaluation
virtual IntegratorLYAPUNOV & operator=(const IntegratorLYAPUNOV &arg)
#define CLOSE_NAMESPACE_ACADO
void initializeVariables()
virtual returnValue getProtectedBackwardSensitivities(DVector &Dx_x0, DVector &Dx_p, DVector &Dx_u, DVector &Dx_w, int order) const
virtual double getStepSize() const
Abstract base class for all kinds of algorithms for integrating differential equations (ODEs or DAEs)...
Lyapunov getLyapunovObject() const
uint getFloorIndex(double time) const
void interpolate(int jj, double *e1, double *d1, double *e2, VariablesGrid &poly)
void determineEtaHBackward(int number)
double scale(VariableType variableType_, int index_) const
returnValue acadoPrintCopyrightNotice(const std::string &subpackage)
Derived & setZero(Index size)
int index(VariableType variableType_, int index_) const
virtual returnValue start()
void determineEtaGForward(int number)
virtual returnValue setBackwardSeed2(const DVector &seed)
virtual returnValue evaluate(const DVector &x0, const DVector &xa, const DVector &p, const DVector &u, const DVector &w, const Grid &t_)
virtual returnValue stop()
virtual int getNumberOfSteps() const
double getLastTime() const
#define ACADOWARNING(retval)
#define BEGIN_NAMESPACE_ACADO
virtual returnValue stop()
virtual returnValue evaluateSensitivities()
virtual returnValue printRunTimeProfile() const
virtual returnValue getProtectedForwardSensitivities(DMatrix *Dx, int order) const
virtual int getDim() const
returnValue setForwardSeed2(const DVector &xSeed, const DVector &pSeed, const DVector &uSeed, const DVector &wSeed)
Base class for all kinds of Runge-Kutta schemes for integrating ODEs.
int getNumberOfVariables() const
#define ACADOERROR(retval)
virtual returnValue getTime(double &_elapsedTime)
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
DifferentialEquation * rhs