examples/integrator/cstr.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 #include <acado_integrators.hpp>
36 #include <acado_gnuplot.hpp>
37 
38 
39 const double k10 = 1.287e12;
40 const double k20 = 1.287e12;
41 const double k30 = 9.043e09;
42 const double E1 = -9758.3;
43 const double E2 = -9758.3;
44 const double E3 = -8560.0;
45 const double H1 = 4.2;
46 const double H2 = -11.0;
47 const double H3 = -41.85;
48 const double rho = 0.9342;
49 const double Cp = 3.01;
50 const double kw = 4032.0;
51 const double AR = 0.215;
52 const double VR = 10.0;
53 const double mK = 5.0;
54 const double CPK = 2.0;
55 
56 const double cA0 = 5.1;
57 const double theta0 = 104.9;
58 
59 const double FFs = 14.19; /* Feed Flow normed by VR: (dV/dt / VR)*/
60 const double QdotKs = -1113.50;
61 
62 const double cAs = 2.1402105301746182e00;
63 const double cBs = 1.0903043613077321e00;
64 const double thetas = 1.1419108442079495e02;
65 const double thetaKs = 1.1290659291045561e02;
66 
67 
68 const double TIMEUNITS_PER_HOUR = 3600.0;
69 
70 
71 const double R_OMEGA = 90.0;
72 
73 
75 
76 
78  const Control &u ){
79 
81 
82  IntermediateState cA = x(0);
83  IntermediateState cB = x(1);
84  IntermediateState theta = x(2);
85  IntermediateState thetaK = x(3);
86 
87  IntermediateState k1, k2, k3;
88 
89  k1 = k10*exp(E1/(273.15 +theta));
90  k2 = k20*exp(E2/(273.15 +theta));
91  k3 = k30*exp(E3/(273.15 +theta));
92 
93  rhs(0) = (1/TIMEUNITS_PER_HOUR)*(u(0)*(cA0-cA) - k1*cA - k3*cA*cA);
94  rhs(1) = (1/TIMEUNITS_PER_HOUR)* (- u(0)*cB + k1*cA - k2*cB);
95  rhs(2) = (1/TIMEUNITS_PER_HOUR)*(u(0)*(theta0-theta) - (1/(rho*Cp)) *(k1*cA*H1 + k2*cB*H2 + k3*cA*cA*H3)+(kw*AR/(rho*Cp*VR))*(thetaK -theta));
96  rhs(3) = (1/TIMEUNITS_PER_HOUR)*((1/(mK*CPK))*(u(1) + kw*AR*(theta-thetaK)));
97 
98  return rhs;
99 }
100 
101 
102 
103 int main( ){
104 
105 
106  // Define a Right-Hand-Side:
107  // -------------------------
108  DifferentialState x("", 4, 1), P("", 4, 4);
109  Control u("", 2, 1);
110 
111  IntermediateState rhs = cstrModel( x, u );
112 
113  DMatrix Q = zeros<double>(4,4);
114 
115  Q(0,0) = 0.2;
116  Q(1,1) = 1.0;
117  Q(2,2) = 0.5;
118  Q(3,3) = 0.2;
119 
120 
121  DMatrix R = zeros<double>(2,2);
122 
123  R(0,0) = 0.5;
124  R(1,1) = 5e-7;
125 
127  f << dot(x) == rhs;
128  f << dot(P) == getRiccatiODE( rhs, x, u, P, Q, R );
129 
130 
131 
132  // Define an integrator:
133  // ---------------------
134 
135  IntegratorRK45 integrator( f );
136  integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
137  integrator.set( PRINT_INTEGRATOR_PROFILE, YES );
138 
139  // Define an initial value:
140  // ------------------------
141  //double x_ss[4] = { 2.14, 1.09, 114.2, 112.9 };
142  double x_start[20] = { 1.0, 0.5, 100.0, 100.0, 1.0, 0.0, 0.0, 0.0,
143  0.0, 1.0, 0.0, 0.0,
144  0.0, 0.0, 1.0, 0.0,
145  0.0, 0.0, 0.0, 1.0 };
146 
147  double u_start[2] = { 14.19, -1113.5 };
148 // double u_start[2] = { 10.00, -7000.0 };
149 
150  Grid timeInterval( 0.0, 5000.0, 100 );
151 
152  integrator.freezeAll();
153  integrator.integrate( timeInterval, x_start, 0 ,0, u_start );
154 
155 
156  // GET THE RESULTS
157  // ---------------
158 
159  VariablesGrid differentialStates;
160  integrator.getX( differentialStates );
161 
162  DVector PP = differentialStates.getLastVector();
163  DMatrix PPP(4,4);
164  for( int i=0; i<4; ++i )
165  for( int j=0; j<4; ++j )
166  PPP(i,j) = PP(4+i*4+j);
167  PPP.print( "P1.txt","",PS_PLAIN );
168 // PPP.printToFile( "P2.txt","",PS_PLAIN );
169 
170  GnuplotWindow window;
171  window.addSubplot( differentialStates(0), "cA [mol/l]" );
172  window.addSubplot( differentialStates(1), "cB [mol/l]" );
173  window.addSubplot( differentialStates(2), "theta [C]" );
174  window.addSubplot( differentialStates(3), "thetaK [C]" );
175 
176  window.addSubplot( differentialStates(4 ), "P11" );
177  window.addSubplot( differentialStates(9 ), "P22" );
178  window.addSubplot( differentialStates(14), "P33" );
179  window.addSubplot( differentialStates(19), "P44" );
180 
181  window.plot();
182 
183  return 0;
184 }
185 
186 
187 
const double mK
const double CPK
returnValue getX(DVector &xEnd) const
Expression getRiccatiODE(const Expression &rhs, const DifferentialState &x, const Control &u, const DifferentialState &P, const DMatrix &Q, const DMatrix &R)
const double QdotKs
virtual returnValue plot(PlotFrequency _frequency=PLOT_IN_ANY_CASE)
returnValue set(OptionsName name, int value)
const double Cp
const double thetaKs
const double rho
#define USING_NAMESPACE_ACADO
Provides a time grid consisting of vector-valued optimization variables at each grid point...
const double cBs
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
const double E3
returnValue addSubplot(PlotWindowSubplot &_subplot)
const double theta0
const double TIMEUNITS_PER_HOUR
const double E1
#define YES
Definition: acado_types.hpp:51
virtual returnValue print(std::ostream &_stream=std::cout, const std::string &_name=DEFAULT_LABEL, const std::string &_startString=DEFAULT_START_STRING, const std::string &_endString=DEFAULT_END_STRING, uint _width=DEFAULT_WIDTH, uint _precision=DEFAULT_PRECISION, const std::string &_colSeparator=DEFAULT_COL_SEPARATOR, const std::string &_rowSeparator=DEFAULT_ROW_SEPARATOR) const
Definition: matrix.cpp:254
const double thetas
const double AR
DVector getLastVector() const
USING_NAMESPACE_ACADO IntermediateState cstrModel(const DifferentialState &x, const Control &u)
const double E2
virtual returnValue freezeAll()
void rhs(const real_t *x, real_t *f)
Expression dot(const Expression &arg)
const double cAs
const double H3
Implements the Runge-Kutta-45 scheme for integrating ODEs.
const double kw
const double H1
const double FFs
const double H2
const double k20
const double VR
IntermediateState exp(const Expression &arg)
const double k30
const double k10
Provides an interface to Gnuplot for plotting algorithmic outputs.
const double cA0
#define R
returnValue integrate(double t0, double tend, double *x0, double *xa=0, double *p=0, double *u=0, double *w=0)
Definition: integrator.cpp:207
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
const double R_OMEGA


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