fourtankNMPC.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 
34 //#include <acado_optimal_control.hpp>
35 #include <acado_toolkit.hpp>
36 #include <acado_gnuplot.hpp>
37 
39 
40 int main( )
41 {
42  // INTRODUCE THE VARIABLES:
43  // -------------------------
44 
45  DifferentialState h1,h2,h3,h4;
46  Control u1,u2;
47  // Parameter T;
48  // Disturbance w;
49  // Parameter p,q;
50  const double t_start = 0.0;
51  const double samplingTime=30.0;
52  double A1=380.1327, A2=380.1327, A3=380.1327, A4=380.1327, a1=1.2272, a2=1.2272, a3=1.2272, a4=1.2272,g=981.0,gamma1=0.45, gamma2=0.4;
53  double x_ss1=20.0, x_ss2=15.0, u_max1=400.0, u_max2=400.0, x_max1=40.0, x_max2=40.0, x_max3=40.0, x_max4=40.0;
55  int T=10;
56  //
57  const double t_end = t_start+T*samplingTime;
58 
59  // DEFINE A DIFFERENTIAL EQUATION:
60  // -------------------------------
61 
62  // f << -dot(x) -x*x + p + u*u + w;
63  f << dot(h1) == (1.0/A1)*(gamma1*u1+a3*sqrt(2.0*g*h3)-a1*sqrt(2.0*g*h1));
64  f << dot(h2)== (1.0/A2)*(gamma2*u2+a4*sqrt(2.0*g*h4)-a2*sqrt(2.0*g*h2));
65  f << dot(h3)==(1.0/A3)*((1.0-gamma2)*u2-a3*sqrt(2.0*g*h3));
66  f << dot(h4)==(1.0/A4)*((1.0-gamma1)*u1-a4*sqrt(2.0*g*h4));
67 
68 
69  // DEFINE THE LEAST SQUARE FUNCTION
70 
71  Function h;
72  h << h1;
73  h << h2;
74  DMatrix Q(2,2);
75  Q.setIdentity();
76 
77  DVector r(2);
78  r(0)=x_ss1;
79  r(1)=x_ss2;
80 
81  // DEFINE AN OPTIMAL CONTROL PROBLEM:
82  // ----------------------------------
83 
84  OCP ocp(t_start,t_end,T);
85  ocp.minimizeLSQ(Q,h,r);
86 
87  ocp.subjectTo(f);
88  //ocp.subjectTo( AT_START, x1 == 0.0 );
89  //ocp.subjectTo( AT_START, x2 == 0.0 );
90  //ocp.subjectTo( AT_START, x3 == 0.0 );
91  //ocp.subjectTo( AT_START, x4 == 0.0 );
92  //ocp.subjectTo( AT_END, x1 == 0.0 );
93  //ocp.subjectTo( AT_END, x2 == 0.0 );
94  //ocp.subjectTo( AT_END, x3 == 10.0 );
95  //ocp.subjectTo( AT_END, x4 == 0.0 );
96  ocp.subjectTo( 0.0 <= u1 <= u_max1 );
97  ocp.subjectTo( 0.0 <= u2 <= u_max2 );
98  ocp.subjectTo( 0.0 <= h1 <= x_max1 );
99  ocp.subjectTo( 0.0 <= h2 <= x_max2 );
100  ocp.subjectTo( 0.0 <= h3 <= x_max3 );
101  ocp.subjectTo( 0.0 <= h4 <= x_max4 );
102 
103  // SETTING UP THE PROCESS
104 
105  OutputFcn identity;
106  DynamicSystem dynamicSystem(f,identity);
107 
108  GaussianNoise noise( 4,0.0,0.1 );
109 
110  Sensor sensor( 4 );
111  if (sensor.setOutputNoise( noise,samplingTime ) != SUCCESSFUL_RETURN)
112  exit( EXIT_FAILURE );
113 
114  Process process(dynamicSystem,INT_RK45);
115  //process.setSensor( sensor );
116 
117  // SETTING UP THE MPC CONTROLLER
118 
119  RealTimeAlgorithm algorithm(ocp,samplingTime);
120 // algorithm.set( USE_REALTIME_ITERATIONS, YES );
121  algorithm.set( MAX_NUM_ITERATIONS, 2 );
122  algorithm.set(LEVENBERG_MARQUARDT, 1e-5);
123 
124  StaticReferenceTrajectory zeroReference;
125 
126  Controller controller(algorithm,zeroReference);
127 
128  // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
129  // ---------------------------------------------------
130  //OptimizationAlgorithm algorithm(ocp);
131 
132  double simStartTime = 0.0;
133  double simEndTime = 1200.0;
134  SimulationEnvironment sim(simStartTime,simEndTime,process,controller);
135 
136  DVector x0(4);
137  x0.setZero( );
138  x0(0)=0.1;
139  x0(1)=0.1;
140  x0(2)=0.1;
141  x0(3)=0.1;
142  if (sim.init( x0 ) != SUCCESSFUL_RETURN)
143  exit( EXIT_FAILURE );
144  if (sim.run( ) != SUCCESSFUL_RETURN)
145  exit( EXIT_FAILURE );
146 
147  VariablesGrid diffStates;
148  sim.getProcessDifferentialStates(diffStates);
149  VariablesGrid feedbackControl;
150  sim.getFeedbackControl (feedbackControl);
151 
152  GnuplotWindow window;
153  window.addSubplot( diffStates(0), "h1");
154  window.addSubplot( diffStates(1), "h2");
155  window.addSubplot( diffStates(2), "h3");
156  window.addSubplot( diffStates(3), "h4");
157  window.addSubplot( feedbackControl(0), "F1");
158  window.addSubplot( feedbackControl(1), "F2");
159  window.plot();
160 
161  diffStates.print("result.txt","diffstates",PS_MATLAB);
162 
163  //algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
164 
165  //algorithm << window;
166 
167  //double t1 = acadoGetTime();
168  //algorithm.solve();
169  //printf(" %.16e \n ", acadoGetTime() - t1 );
170 
171  return EXIT_SUCCESS;
172 }
Calculates the control inputs of the Process based on the Process outputs.
Definition: controller.hpp:71
returnValue print(std::ostream &stream=std::cout, const char *const name=DEFAULT_LABEL, const char *const startString=DEFAULT_START_STRING, const char *const endString=DEFAULT_END_STRING, uint width=DEFAULT_WIDTH, uint precision=DEFAULT_PRECISION, const char *const colSeparator=DEFAULT_COL_SEPARATOR, const char *const rowSeparator=DEFAULT_ROW_SEPARATOR) const
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
IntermediateState sqrt(const Expression &arg)
USING_NAMESPACE_ACADO typedef TaylorVariable< Interval > T
Allows to setup and evaluate output functions based on SymbolicExpressions.
Definition: output_fcn.hpp:55
virtual returnValue plot(PlotFrequency _frequency=PLOT_IN_ANY_CASE)
returnValue getProcessDifferentialStates(VariablesGrid &_diffStates)
Stores a DifferentialEquation together with an OutputFcn.
#define USING_NAMESPACE_ACADO
Provides a time grid consisting of vector-valued optimization variables at each grid point...
returnValue getFeedbackControl(Curve &_feedbackControl) const
#define A1
Allows to simulate the behaviour of sensors within the Process.
Definition: sensor.hpp:54
USING_NAMESPACE_ACADO int main()
User-interface to formulate and solve model predictive control problems.
returnValue subjectTo(const DifferentialEquation &differentialEquation_)
Definition: ocp.cpp:153
returnValue addSubplot(PlotWindowSubplot &_subplot)
returnValue set(OptionsName name, int value)
Definition: options.cpp:126
returnValue minimizeLSQ(const DMatrix &S, const Function &h, const DVector &r)
Definition: ocp.cpp:244
returnValue init(const DVector &x0_, const DVector &p_=emptyConstVector)
#define A2
returnValue setOutputNoise(const Noise &_noise, double _noiseSamplingTime)
Definition: sensor.cpp:81
Derived & setZero(Index size)
Data class for defining optimal control problems.
Definition: ocp.hpp:89
Allows to define a static reference trajectory that the ControlLaw aims to track. ...
Expression dot(const Expression &arg)
const double t_end
Generates pseudo-random Gaussian noise for simulating the Process.
const double t_start
Allows to run closed-loop simulations of dynamic systems.
Simulates the process to be controlled based on a dynamic model.
Definition: process.hpp:71
Provides an interface to Gnuplot for plotting algorithmic outputs.
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:34