USING_NAMESPACE_ACADO
#include <mex.h>
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
MatlabConsoleStreamBuf mybuf;
RedirectStream redirect(std::cout, mybuf);
clearAllStaticCounters( );
mexPrintf("\nACADO Toolkit for Matlab - Developed by David Ariens and Rien Quirynen, 2009-2013 \n");
mexPrintf("Support available at http://www.acadotoolkit.org/matlab \n \n");
if (nrhs != 0) {
mexErrMsgTxt("This problem expects 0 right hand side argument(s) since you have defined 0 MexInput(s)");
}
TIME autotime;
DifferentialState x;
DifferentialState y;
DifferentialState th;
DifferentialState L;
Control v;
Control w;
DifferentialEquation acadodata_f1;
acadodata_f1 << dot(x) == cos(th)*v;
acadodata_f1 << dot(y) == sin(th)*v;
acadodata_f1 << dot(th) == w;
acadodata_f1 << dot(L) == w*w;
OCP ocp1(0, 10, 20);
ocp1.minimizeMayerTerm(L);
ocp1.subjectTo(acadodata_f1);
ocp1.subjectTo(AT_START, x == (-1.000000E+00));
ocp1.subjectTo(AT_START, y == (-1.000000E+00));
ocp1.subjectTo(AT_START, th == 0.000000E+00);
ocp1.subjectTo(AT_END, x == 0.000000E+00);
ocp1.subjectTo(AT_END, y == 0.000000E+00);
OptimizationAlgorithm algo1(ocp1);
returnValue returnvalue = algo1.solve();
VariablesGrid out_states;
VariablesGrid out_parameters;
VariablesGrid out_controls;
VariablesGrid out_disturbances;
VariablesGrid out_algstates;
algo1.getDifferentialStates(out_states);
algo1.getControls(out_controls);
const char* outputFieldNames[] = {"STATES", "CONTROLS", "PARAMETERS", "DISTURBANCES", "ALGEBRAICSTATES", "CONVERGENCE_ACHIEVED"};
plhs[0] = mxCreateStructMatrix( 1,1,6,outputFieldNames );
mxArray *OutS = NULL;
double *outS = NULL;
OutS = mxCreateDoubleMatrix( out_states.getNumPoints(),1+out_states.getNumValues(),mxREAL );
outS = mxGetPr( OutS );
for( int i=0; i<out_states.getNumPoints(); ++i ) {
outS[0*out_states.getNumPoints() + i] = out_states.getTime(i);
for( int j=0; j<out_states.getNumValues(); ++j ) {
outS[(1+j)*out_states.getNumPoints() + i] = out_states(i, j);
}
}
mxSetField( plhs[0],0,"STATES",OutS );
mxArray *OutC = NULL;
double *outC = NULL;
OutC = mxCreateDoubleMatrix( out_controls.getNumPoints(),1+out_controls.getNumValues(),mxREAL );
outC = mxGetPr( OutC );
for( int i=0; i<out_controls.getNumPoints(); ++i ) {
outC[0*out_controls.getNumPoints() + i] = out_controls.getTime(i);
for( int j=0; j<out_controls.getNumValues(); ++j ) {
outC[(1+j)*out_controls.getNumPoints() + i] = out_controls(i, j);
}
}
mxSetField( plhs[0],0,"CONTROLS",OutC );
mxArray *OutP = NULL;
double *outP = NULL;
OutP = mxCreateDoubleMatrix( out_parameters.getNumPoints(),1+out_parameters.getNumValues(),mxREAL );
outP = mxGetPr( OutP );
for( int i=0; i<out_parameters.getNumPoints(); ++i ) {
outP[0*out_parameters.getNumPoints() + i] = out_parameters.getTime(i);
for( int j=0; j<out_parameters.getNumValues(); ++j ) {
outP[(1+j)*out_parameters.getNumPoints() + i] = out_parameters(i, j);
}
}
mxSetField( plhs[0],0,"PARAMETERS",OutP );
mxArray *OutW = NULL;
double *outW = NULL;
OutW = mxCreateDoubleMatrix( out_disturbances.getNumPoints(),1+out_disturbances.getNumValues(),mxREAL );
outW = mxGetPr( OutW );
for( int i=0; i<out_disturbances.getNumPoints(); ++i ) {
outW[0*out_disturbances.getNumPoints() + i] = out_disturbances.getTime(i);
for( int j=0; j<out_disturbances.getNumValues(); ++j ) {
outW[(1+j)*out_disturbances.getNumPoints() + i] = out_disturbances(i, j);
}
}
mxSetField( plhs[0],0,"DISTURBANCES",OutW );
mxArray *OutZ = NULL;
double *outZ = NULL;
OutZ = mxCreateDoubleMatrix( out_algstates.getNumPoints(),1+out_algstates.getNumValues(),mxREAL );
outZ = mxGetPr( OutZ );
for( int i=0; i<out_algstates.getNumPoints(); ++i ) {
outZ[0*out_algstates.getNumPoints() + i] = out_algstates.getTime(i);
for( int j=0; j<out_algstates.getNumValues(); ++j ) {
outZ[(1+j)*out_algstates.getNumPoints() + i] = out_algstates(i, j);
}
}
mxSetField( plhs[0],0,"ALGEBRAICSTATES",OutZ );
mxArray *OutConv = NULL;
if ( returnvalue == SUCCESSFUL_RETURN ) {
OutConv = mxCreateDoubleScalar( 1 );
}
else {
OutConv = mxCreateDoubleScalar( 0 );
}
mxSetField( plhs[0],0,"CONVERGENCE_ACHIEVED",OutConv );
clearAllStaticCounters( );
}
int main( ){
double clock1 = clock();
double t_start = 0.0;
double t_end = 600.0;
int intervals = 5 ;
int i;
TIME t;
DifferentialState x("", NXD, 1);
AlgebraicState z("", NXA, 1);
Control u("", NU, 1);
Parameter p("", NP, 1);
IntermediateState is(1+NXD+NXA+NU+NP);
is(0) = t;
for (i=0; i < NXD; ++i) is(1+i) = x(i);
for (i=0; i < NXA; ++i) is(1+NXD+i) = z(i);
for (i=0; i < NU; ++i) is(1+NXD+NXA+i) = u(i);
for (i=0; i < NP; ++i) is(1+NXD+NXA+NU+i) = p(i);
CFunction hydroscalModel( NXD+NXA, ffcn_model );
// Define a Right-Hand-Side:
// -------------------------
DifferentialEquation f;
f << hydroscalModel(is);
// DEFINE INITIAL VALUES:
// ----------------------
double xd[NXD] = { 2.1936116177990631E-01, // X_0
3.3363028623863722E-01, // X_1
3.7313133250625952E-01, // X_2
3.9896472354654333E-01, // X_3
4.1533719381260475E-01, // X_4
4.2548399372287182E-01, // X_5
4.3168379354213621E-01, // X_6
4.3543569751236455E-01, // X_7
4.3768918647214428E-01, // X_8
4.3903262905928286E-01, // X_9
4.3982597315656735E-01, // X_10
4.4028774979047969E-01, // X_11
4.4055002518902953E-01, // X_12
4.4069238917008052E-01, // X_13
4.4076272408112094E-01, // X_14
4.4078980543461005E-01, // X_15
4.4079091412311144E-01, // X_16
4.4077642312834125E-01, // X_17
4.4075255679998443E-01, // X_18
4.4072304911231042E-01, // X_19
4.4069013958173919E-01, // X_20
6.7041926189645151E-01, // X_21
7.3517997375758948E-01, // X_22
7.8975978943631409E-01, // X_23
8.3481725159539033E-01, // X_24
8.7125377077380739E-01, // X_25
9.0027275078767721E-01, // X_26
9.2312464536394301E-01, // X_27
9.4096954980798608E-01, // X_28
9.5481731262797742E-01, // X_29
9.6551271145368878E-01, // X_30
9.7374401773010488E-01, // X_31
9.8006186072166701E-01, // X_32
9.8490109485675337E-01, // X_33
9.8860194771099286E-01, // X_34
9.9142879342008328E-01, // X_35
9.9358602331847468E-01, // X_36
9.9523105632238640E-01, // X_37
9.9648478785701988E-01, // X_38
9.9743986301741971E-01, // X_39
9.9816716097314861E-01, // X_40
9.9872084014280071E-01, // X_41
3.8633811956730968E+00, // n_1
3.9322260498028840E+00, // n_2
3.9771965626392531E+00, // n_3
4.0063070333869728E+00, // n_4
4.0246026844143410E+00, // n_5
4.0358888958821835E+00, // n_6
4.0427690398786789E+00, // n_7
4.0469300433477020E+00, // n_8
4.0494314648020326E+00, // n_9
4.0509267560029381E+00, // n_10
4.0518145583397631E+00, // n_11
4.0523364846379799E+00, // n_12
4.0526383977460299E+00, // n_13
4.0528081437632766E+00, // n_14
4.0528985491134542E+00, // n_15
4.0529413510270169E+00, // n_16
4.0529556049324462E+00, // n_17
4.0529527471448805E+00, // n_18
4.0529396392278008E+00, // n_19
4.0529203970496912E+00, // n_20
3.6071164950918582E+00, // n_21
3.7583754503438387E+00, // n_22
3.8917148481441974E+00, // n_23
4.0094300698741563E+00, // n_24
4.1102216725798293E+00, // n_25
4.1944038520620675E+00, // n_26
4.2633275166560596E+00, // n_27
4.3188755452109175E+00, // n_28
4.3630947909857642E+00, // n_29
4.3979622247841386E+00, // n_30
4.4252580012497740E+00, // n_31
4.4465128947193868E+00, // n_32
4.4630018314791968E+00, // n_33
4.4757626150015568E+00, // n_34
4.4856260094946823E+00, // n_35
4.4932488551808500E+00, // n_36
4.4991456959629330E+00, // n_37
4.5037168116896273E+00, // n_38
4.5072719605639726E+00, // n_39
4.5100498969782414E+00 // n_40
};
double ud[ NU] = { 4.1833910982822058E+00, // L_vol
2.4899344742988991E+00 // Q
};
double pd[ NP] = { 1.5458567140000001E-01, // n^{ref}_{tray} = 0.155 l
1.7499999999999999E-01, // (not in use?)
3.4717208398678062E-01, // \alpha_{rect} = 35 %
6.1895708603484367E-01, // \alpha_{strip} = 62 %
1.6593025789999999E-01, // W_{tray} = 0.166 l^{-.5} s^{.1}
5.0695122527590109E-01, // Q_{loss} = 0.51 kW
8.5000000000000000E+00, // n^v_0 = 8.5 l
1.7000000000000001E-01, // n^v_{N+1} = 0.17 l
9.3885430857029321E+04, // P_{top} = 939 h Pa
2.5000000000000000E+02, // \Delta P_{strip}= 2.5 h Pa and \Delta P_{rect} = 1.9 h Pa
1.4026000000000000E+01, // F_{vol} = 14.0 l h^{-1}
3.2000000000000001E-01, // X_F = 0.32
7.1054000000000002E+01, // T_F = 71 oC
4.7163089489100003E+01, // T_C = 47.2 oC
4.1833910753991770E+00, // (not in use?)
2.4899344810136301E+00, // (not in use?)
1.8760537088149468E+02 // (not in use?)
};
DVector x0(NXD, xd);
DVector p0(NP, pd);
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
OCP ocp( t_start, t_end, intervals );
// LSQ Term on temperature deviations and controls
Function h;
// for( i = 0; i < NXD; i++ )
// h << 0.001*x(i);
h << 0.1 * ( z(94) - 88.0 ); // Temperature tray 14
h << 0.1 * ( z(108) - 70.0 ); // Temperature tray 28
h << 0.01 * ( u(0) - ud[0] ); // L_vol
h << 0.01 * ( u(1) - ud[1] ); // Q
ocp.minimizeLSQ( h );
// W.r.t. differential equation
ocp.subjectTo( f );
// Fix states
ocp.subjectTo( AT_START, x == x0 );
// Fix parameters
ocp.subjectTo( p == p0 );
// Path constraint on controls
ocp.subjectTo( ud[0] - 2.0 <= u(0) <= ud[0] + 2.0 );
ocp.subjectTo( ud[1] - 2.0 <= u(1) <= ud[1] + 2.0 );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(ocp);
algorithm.initializeAlgebraicStates("hydroscal_algebraic_states.txt");
algorithm.set( INTEGRATOR_TYPE, INT_BDF );
algorithm.set( MAX_NUM_ITERATIONS, 5 );
algorithm.set( KKT_TOLERANCE, 1e-3 );
algorithm.set( INTEGRATOR_TOLERANCE, 1e-4 );
algorithm.set( ABSOLUTE_TOLERANCE , 1e-6 );
algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );
algorithm.set( LINEAR_ALGEBRA_SOLVER, SPARSE_LU );
algorithm.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
//algorithm.set( LEVENBERG_MARQUARDT, 1e-3 );
algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY_LIFTED );
//algorithm.set( DYNAMIC_SENSITIVITY, FORWARD_SENSITIVITY );
//algorithm.set( CONSTRAINT_SENSITIVITY, FORWARD_SENSITIVITY );
//algorithm.set( ALGEBRAIC_RELAXATION,ART_EXPONENTIAL ); //results in an extra step but steps are quicker
algorithm.solve();
double clock2 = clock();
printf("total computation time = %.16e \n", (clock2-clock1)/CLOCKS_PER_SEC );
// PLOT RESULTS:
// ---------------------------------------------------
VariablesGrid out_states;
algorithm.getDifferentialStates( out_states );
out_states.print( "OUT_states.m","STATES",PS_MATLAB );
VariablesGrid out_controls;
algorithm.getControls( out_controls );
out_controls.print( "OUT_controls.m","CONTROLS",PS_MATLAB );
VariablesGrid out_algstates;
algorithm.getAlgebraicStates( out_algstates );
out_algstates.print( "OUT_algstates.m","ALGSTATES",PS_MATLAB );
GnuplotWindow window;
window.addSubplot( out_algstates(94), "Temperature tray 14" );
window.addSubplot( out_algstates(108), "Temperature tray 28" );
window.addSubplot( out_controls(0), "L_vol" );
window.addSubplot( out_controls(1), "Q" );
window.plot( );
return 0;
}
