dev_powerkite_on.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 
34 #include <acado_toolkit.hpp>
35 #include <acado_gnuplot.hpp>
36 
38 
39 int main( )
40 {
41 
42 // DIFFERENTIAL STATES :
43 // -------------------------
44  DifferentialState r; // the length r of the cable
45  DifferentialState phi; // the angle phi
46  DifferentialState theta; // the angle theta (grosser wert- naeher am boden/kleiner wert - weiter oben)
47 // ------------------------- // -------------------------------------------
48  DifferentialState dr; // first derivative of r0 with respect to t
49  DifferentialState dphi; // first derivative of phi with respect to t
50  DifferentialState dtheta; // first derivative of theta with respect to t
51 // ------------------------- // -------------------------------------------
52  DifferentialState n; // winding number(rauslassen)
53 // ------------------------- // -------------------------------------------
54  DifferentialState Psi; // the roll angle Psi
55  DifferentialState CL; // the aerodynamic lift coefficient
56 // ------------------------- // -------------------------------------------
57  DifferentialState W; // integral over the power at the generator
58  // ( = ENERGY )(rauslassen)
59 
60 
61  // MEASUREMENT FUNCTION :
62 // -------------------------
63  Function model_response ; // the measurement function
64 
65 
66 // CONTROL :
67 // -------------------------
68  Control ddr0; // second derivative of r0 with respect to t
69  Control dPsi; // first derivative of Psi with respect to t
70  Control dCL; // first derivative of CL with respect to t
71 
72 
73  Disturbance w_extra;
74 
75 
76 // PARAMETERS :
77 // ------------------------
78  // PARAMETERS OF THE KITE :
79  // -----------------------------
80  double mk = 2000.00; // mass of the kite // [ kg ](maybe change to 5000kg)
81  double A = 500.00; // effective area // [ m^2 ]
82  double V = 720.00; // volume // [ m^3 ]
83  double cd0 = 0.04; // aerodynamic drag coefficient // [ ]
84  // ( cd0: without downwash )
85  double K = 0.04; // induced drag constant // [ ]
86 
87 
88  // PHYSICAL CONSTANTS :
89  // -----------------------------
90  double g = 9.81; // gravitational constant // [ m /s^2]
91  double rho = 1.23; // density of the air // [ kg/m^3]
92 
93  // PARAMETERS OF THE CABLE :
94  // -----------------------------
95  double rhoc = 1450.00; // density of the cable // [ kg/m^3]
96  double cc = 1.00; // frictional constant // [ ]
97  double dc = 0.05614; // diameter // [ m ]
98 
99 
100  // PARAMETERS OF THE WIND :
101  // -----------------------------
102  double w0 = 10.00; // wind velocity at altitude h0 // [ m/s ]
103  double h0 = 100.00; // the altitude h0 // [ m ]
104  double hr = 0.10; // roughness length // [ m ]
105 
106 
107 
108 // OTHER VARIABLES :
109 // ------------------------
110 
111  double AQ ; // cross sectional area
112 
113  IntermediateState mc; // mass of the cable
114  IntermediateState m ; // effective inertial mass
115  IntermediateState m_; // effective gravitational mass
116 
118 
119  IntermediateState dm; // first derivative of m with respect to t
120 
121 
122 // DEFINITION OF PI :
123 // ------------------------
124 
125  double PI = 3.1415926535897932;
126 
127 
128 // ORIENTATION AND FORCES :
129 // ------------------------
130 
131  IntermediateState h ; // altitude of the kite
132  IntermediateState w ; // the wind at altitude h
133  IntermediateState we [ 3]; // effective wind vector
134  IntermediateState nwe ; // norm of the effective wind vector
135  IntermediateState nwep ; // -
136  IntermediateState ewep [ 3]; // projection of ewe
137  IntermediateState eta ; // angle eta: cf. [2]
138  IntermediateState et [ 3]; // unit vector from the left to the right wing tip
139  IntermediateState Caer ;
140  IntermediateState Cf ; // simple constants
141  IntermediateState CD ; // the aerodynamic drag coefficient
142  IntermediateState Fg [ 3]; // the gravitational force
143  IntermediateState Faer [ 3]; // the aerodynamic force
144  IntermediateState Ff [ 3]; // the frictional force
145  IntermediateState F [ 3]; // the total force
146 
147 
148 // TERMS ON RIGHT-HAND-SIDE
149 // OF THE DIFFERENTIAL
150 // EQUATIONS :
151 // ------------------------
152 
153  IntermediateState a_pseudo [ 3]; // the pseudo accelaration
154  IntermediateState dn ; // the time derivate of the kite's winding number
155  IntermediateState ddr ; // second derivative of s with respect to t
156  IntermediateState ddphi ; // second derivative of phi with respect to t
157  IntermediateState ddtheta ; // second derivative of theta with respect to t
158  IntermediateState power ; // the power at the generator
159 // ------------------------ ------ // ----------------------------------------------
160  IntermediateState regularisation ; // regularisation of controls
161 
162 
163 
164 // MODEL EQUATIONS :
165 // ===============================================================
166 
167 
168 
169 // SPRING CONSTANT OF THE CABLE :
170 // ---------------------------------------------------------------
171 
172  AQ = PI*dc*dc/4.0 ;
173 
174 // THE EFECTIVE MASS' :
175 // ---------------------------------------------------------------
176 
177  mc = rhoc*AQ*r ; // mass of the cable
178  m = mk + mc / 3.0; // effective inertial mass
179  m_ = mk + mc / 2.0; // effective gravitational mass
180 // ----------------------------- // ----------------------------
181  dm = (rhoc*AQ/ 3.0)*dr; // time derivative of the mass
182 
183 
184 // WIND SHEAR MODEL :
185 // ---------------------------------------------------------------
186 // for startup +60m
187  h = r*cos(theta)+60.0 ;
188 // h = r*cos(theta) ;
189  w = log(h/hr) / log(h0/hr) *(w0+w_extra) ;
190 
191 
192 // EFFECTIVE WIND IN THE KITE`S SYSTEM :
193 // ---------------------------------------------------------------
194 
195  we[0] = w*sin(theta)*cos(phi) - dr ;
196  we[1] = -w*sin(phi) - r*sin(theta)*dphi ;
197  we[2] = -w*cos(theta)*cos(phi) + r *dtheta;
198 
199 
200 // CALCULATION OF THE KITE`S TRANSVERSAL AXIS :
201 // ---------------------------------------------------------------
202 
203  nwep = pow( we[1]*we[1] + we[2]*we[2], 0.5 );
204  nwe = pow( we[0]*we[0] + we[1]*we[1] + we[2]*we[2], 0.5 );
205  eta = asin( we[0]*tan(Psi)/ nwep ) ;
206 
207 // ---------------------------------------------------------------
208 
209 // ewep[0] = 0.0 ;
210  ewep[1] = we[1] / nwep ;
211  ewep[2] = we[2] / nwep ;
212 
213 // ---------------------------------------------------------------
214 
215  et [0] = sin(Psi) ;
216  et [1] = (-cos(Psi)*sin(eta))*ewep[1] - (cos(Psi)*cos(eta))*ewep[2];
217  et [2] = (-cos(Psi)*sin(eta))*ewep[2] + (cos(Psi)*cos(eta))*ewep[1];
218 
219 
220 
221 
222 // CONSTANTS FOR SEVERAL FORCES :
223 // ---------------------------------------------------------------
224 
225  Cg = (V*rho-m_)*g ;
226  Caer = (rho*A/2.0 )*nwe ;
227  Cf = (rho*dc/8.0)*r*nwe ;
228 
229 
230 // THE DRAG-COEFFICIENT :
231 // ---------------------------------------------------------------
232 
233  CD = cd0 + K*CL*CL ;
234 
235 
236 
237 // SUM OF GRAVITATIONAL AND LIFTING FORCE :
238 // ---------------------------------------------------------------
239 
240  Fg [0] = Cg * cos(theta) ;
241 // Fg [1] = Cg * 0.0 ;
242  Fg [2] = Cg * sin(theta) ;
243 
244 
245 // SUM OF THE AERODYNAMIC FORCES :
246 // ---------------------------------------------------------------
247 
248  Faer[0] = Caer*( CL*(we[1]*et[2]-we[2]*et[1]) + CD*we[0] ) ;
249  Faer[1] = Caer*( CL*(we[2]*et[0]-we[0]*et[2]) + CD*we[1] ) ;
250  Faer[2] = Caer*( CL*(we[0]*et[1]-we[1]*et[0]) + CD*we[2] ) ;
251 
252 
253 // THE FRICTION OF THE CABLE :
254 // ---------------------------------------------------------------
255 
256 // Ff [0] = Cf * cc* we[0] ;
257  Ff [1] = Cf * cc* we[1] ;
258  Ff [2] = Cf * cc* we[2] ;
259 
260 
261 
262 // THE TOTAL FORCE :
263 // ---------------------------------------------------------------
264 
265  F [0] = Fg[0] + Faer[0] ;
266  F [1] = Faer[1] + Ff[1] ;
267  F [2] = Fg[2] + Faer[2] + Ff[2] ;
268 
269 
270 
271 // THE PSEUDO ACCELERATION:
272 // ---------------------------------------------------------------
273 
274  a_pseudo [0] = - ddr0
275  + r*( dtheta*dtheta
276  + sin(theta)*sin(theta)*dphi *dphi )
277  - dm/m*dr ;
278 
279  a_pseudo [1] = - 2.0*cos(theta)/sin(theta)*dphi*dtheta
280  - 2.0*dr/r*dphi
281  - dm/m*dphi ;
282 
283  a_pseudo [2] = cos(theta)*sin(theta)*dphi*dphi
284  - 2.0*dr/r*dtheta
285  - dm/m*dtheta ;
286 
287 
288 
289 
290 // THE EQUATIONS OF MOTION:
291 // ---------------------------------------------------------------
292 
293  ddr = F[0]/m + a_pseudo[0] ;
294  ddphi = F[1]/(m*r*sin(theta)) + a_pseudo[1] ;
295  ddtheta = -F[2]/(m*r ) + a_pseudo[2] ;
296 
297 
298 
299 
300 
301 // THE DERIVATIVE OF THE WINDING NUMBER :
302 // ---------------------------------------------------------------
303 
304  dn = ( dphi*ddtheta - dtheta*ddphi ) /
305  (2.0*PI*(dphi*dphi + dtheta*dtheta) ) ;
306 
307 
308 
309 // THE POWER AT THE GENERATOR :
310 // ---------------------------------------------------------------
311 
312  power = m*ddr*dr ;
313 
314 
315 
316 // REGULARISATION TERMS :
317 // ---------------------------------------------------------------
318 
319 
320  regularisation = 5.0e2 * ddr0 * ddr0
321  + 1.0e8 * dPsi * dPsi
322  + 1.0e5 * dCL * dCL
323  + 2.5e5 * dn * dn
324  + 2.5e7 * ddphi * ddphi;
325  + 2.5e7 * ddtheta * ddtheta;
326  + 2.5e6 * dtheta * dtheta;
327 // ---------------------------
328 
329 
330 
331 
332 // REFERENCE TRAJECTORY:
333 // ---------------------------------------------------------------
334  VariablesGrid myReference; myReference.read( "ref_w_zeros.txt" );// read the measurements
335  PeriodicReferenceTrajectory reference( myReference );
336 
337 
338 // THE "RIGHT-HAND-SIDE" OF THE ODE:
339 // ---------------------------------------------------------------
341 
342  f << dot(r) == dr ;
343  f << dot(phi) == dphi ;
344  f << dot(theta) == dtheta ;
345  f << dot(dr) == ddr0 ;
346  f << dot(dphi) == ddphi ;
347  f << dot(dtheta) == ddtheta ;
348  f << dot(n) == dn ;
349  f << dot(Psi) == dPsi ;
350  f << dot(CL) == dCL ;
351  f << dot(W) == (-power + regularisation)*1.0e-6;
352 
353 
354  model_response << r ; // the state r is measured
355  model_response << phi ;
356  model_response << theta;
357  model_response << dr ;
358  model_response << dphi ;
359  model_response << dtheta;
360  model_response << ddr0;
361  model_response << dPsi;
362  model_response << dCL ;
363 
364 
365  DVector x_scal(9);
366 
367  x_scal(0) = 60.0;
368  x_scal(1) = 1.0e-1;
369  x_scal(2) = 1.0e-1;
370  x_scal(3) = 40.0;
371  x_scal(4) = 1.0e-1;
372  x_scal(5) = 1.0e-1;
373  x_scal(6) = 60.0;
374  x_scal(7) = 1.5e-1;
375  x_scal(8) = 2.5;
376 
377 
378  DMatrix Q(9,9);
379  Q.setIdentity();
380  DMatrix Q_end(9,9);
381  Q_end.setIdentity();
382  int i;
383  for( i = 0; i < 6; i++ ){
384  Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i));
385  Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i));
386  }
387  for( i = 6; i < 9; i++ ){
388  Q(i,i) = (1.0e-1/x_scal(i))*(1.0e-1/x_scal(i));
389  Q_end(i,i) = (5.0e-1/x_scal(i))*(5.0e-1/x_scal(i));
390  }
391 
392  DVector measurements(9);
393  measurements.setAll( 0.0 );
394 
395 
396  // DEFINE AN OPTIMAL CONTROL PROBLEM:
397  // ----------------------------------
398  const double t_start = 0.0;
399  const double t_end = 10.0;
400  OCP ocp( t_start, t_end, 10 );
401  ocp.minimizeLSQ( Q,model_response, measurements ) ; // fit h to the data
402  ocp.minimizeLSQEndTerm( Q_end,model_response, measurements ) ;
403  ocp.subjectTo( f );
404 
405 
406  ocp.subjectTo( -0.34 <= phi <= 0.34 );
407  ocp.subjectTo( 0.85 <= theta <= 1.45 );
408  ocp.subjectTo( -40.0 <= dr <= 10.0 );
409  ocp.subjectTo( -0.29 <= Psi <= 0.29 );
410  ocp.subjectTo( 0.1 <= CL <= 1.50 );
411  ocp.subjectTo( -0.7 <= n <= 0.90 );
412  ocp.subjectTo( -25.0 <= ddr0 <= 25.0 );
413  ocp.subjectTo( -0.065 <= dPsi <= 0.065 );
414  ocp.subjectTo( -3.5 <= dCL <= 3.5 );
415  ocp.subjectTo( -60.0 <= cos(theta)*r );
416  ocp.subjectTo( w_extra == 0.0 );
417 
418 
419 // SETTING UP THE (SIMULATED) PROCESS:
420  // -----------------------------------
421  OutputFcn identity;
422  DynamicSystem dynamicSystem( f,identity );
423  Process process( dynamicSystem,INT_RK45 );
424 
425 
426  VariablesGrid disturbance; disturbance.read( "my_wind_disturbance_controlsfree.txt" );
427  if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
428  exit( EXIT_FAILURE );
429 
430  // SETUP OF THE ALGORITHM AND THE TUNING OPTIONS:
431  // ----------------------------------------------
432  double samplingTime = 1.0;
433  RealTimeAlgorithm algorithm( ocp, samplingTime );
434  if (algorithm.initializeDifferentialStates("p_s_ref.txt" ) != SUCCESSFUL_RETURN)
435  exit( EXIT_FAILURE );
436  if (algorithm.initializeControls ("p_c_ref.txt" ) != SUCCESSFUL_RETURN)
437  exit( EXIT_FAILURE );
438 
439  algorithm.set( MAX_NUM_ITERATIONS, 2 );
440  algorithm.set( KKT_TOLERANCE , 1e-4 );
442  algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
444 // algorithm.set( USE_IMMEDIATE_FEEDBACK, YES );
445  algorithm.set( USE_REALTIME_SHIFTS, YES );
446  algorithm.set(LEVENBERG_MARQUARDT, 1e-5);
447 
448 
449  DVector x0(10);
450  x0(0) = 1.8264164528775887e+03;
451  x0(1) = -5.1770453309520573e-03;
452  x0(2) = 1.2706440287266794e+00;
453  x0(3) = 2.1977888424944396e+00;
454  x0(4) = 3.1840786108641383e-03;
455  x0(5) = -3.8281200674676448e-02;
456  x0(6) = 0.0000000000000000e+00;
457  x0(7) = -1.0372313936413566e-02;
458  x0(8) = 1.4999999999999616e+00;
459  x0(9) = 0.0000000000000000e+00;
460 
461 
462  // SETTING UP THE NMPC CONTROLLER:
463  // -------------------------------
464 
465  Controller controller( algorithm, reference );
466 
467  // SETTING UP THE SIMULATION ENVIRONMENT, RUN THE EXAMPLE...
468  // ----------------------------------------------------------
469  double simulationStart = 0.0;
470  double simulationEnd = 10.0;
471 
472  SimulationEnvironment sim( simulationStart, simulationEnd, process, controller );
473 
474  if (sim.init( x0 ) != SUCCESSFUL_RETURN)
475  exit( EXIT_FAILURE );
476  if (sim.run( ) != SUCCESSFUL_RETURN)
477  exit( EXIT_FAILURE );
478 
479  // ...AND PLOT THE RESULTS
480  // ----------------------------------------------------------
481 
482  VariablesGrid diffStates;
483  sim.getProcessDifferentialStates( diffStates );
484  diffStates.print( "diffStates.txt" );
485  diffStates.print( "diffStates.m","DIFFSTATES",PS_MATLAB );
486 
487  VariablesGrid interStates;
488  sim.getProcessIntermediateStates( interStates );
489  interStates.print( "interStates.txt" );
490  interStates.print( "interStates.m","INTERSTATES",PS_MATLAB );
491 
492  VariablesGrid sampledProcessOutput;
493  sim.getSampledProcessOutput( sampledProcessOutput );
494  sampledProcessOutput.print( "sampledOut.txt" );
495  sampledProcessOutput.print( "sampledOut.m","OUT",PS_MATLAB );
496 
497  VariablesGrid feedbackControl;
498  sim.getFeedbackControl( feedbackControl );
499  feedbackControl.print( "controls.txt" );
500  feedbackControl.print( "controls.m","CONTROL",PS_MATLAB );
501 
502  GnuplotWindow window;
503  window.addSubplot( sampledProcessOutput(0), "DIFFERENTIAL STATE: r" );
504  window.addSubplot( sampledProcessOutput(1), "DIFFERENTIAL STATE: phi" );
505  window.addSubplot( sampledProcessOutput(2), "DIFFERENTIAL STATE: theta" );
506  window.addSubplot( sampledProcessOutput(3), "DIFFERENTIAL STATE: dr" );
507  window.addSubplot( sampledProcessOutput(4), "DIFFERENTIAL STATE: dphi" );
508  window.addSubplot( sampledProcessOutput(5), "DIFFERENTIAL STATE: dtheta" );
509  window.addSubplot( sampledProcessOutput(7), "DIFFERENTIAL STATE: Psi" );
510  window.addSubplot( sampledProcessOutput(8), "DIFFERENTIAL STATE: CL" );
511  window.addSubplot( sampledProcessOutput(9), "DIFFERENTIAL STATE: W" );
512 
513  window.addSubplot( feedbackControl(0), "CONTROL 1 DDR0" );
514  window.addSubplot( feedbackControl(1), "CONTROL 1 DPSI" );
515  window.addSubplot( feedbackControl(2), "CONTROL 1 DCL" );
516  window.plot( );
517 
518  GnuplotWindow window2;
519  window2.addSubplot( interStates(1) );
520  window2.plot();
521 
522  return 0;
523 }
524 
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
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
Allows to define a static periodic reference trajectory that the ControlLaw aims to track...
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...
USING_NAMESPACE_ACADO int main()
IntermediateState asin(const Expression &arg)
IntermediateState pow(const Expression &arg1, const Expression &arg2)
returnValue getFeedbackControl(Curve &_feedbackControl) const
IntermediateState tan(const Expression &arg)
User-interface to formulate and solve model predictive control problems.
returnValue subjectTo(const DifferentialEquation &differentialEquation_)
Definition: ocp.cpp:153
returnValue addSubplot(PlotWindowSubplot &_subplot)
IntermediateState cos(const Expression &arg)
returnValue getSampledProcessOutput(VariablesGrid &_sampledProcessOutput)
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 YES
Definition: acado_types.hpp:51
returnValue minimizeLSQEndTerm(const DMatrix &S, const Function &m, const DVector &r)
Definition: ocp.cpp:297
returnValue initializeDifferentialStates(const char *fileName, BooleanType autoinit=BT_FALSE)
Data class for defining optimal control problems.
Definition: ocp.hpp:89
Expression dot(const Expression &arg)
returnValue read(std::istream &stream)
const double t_end
returnValue setProcessDisturbance(const Curve &_processDisturbance)
Definition: process.cpp:289
const double t_start
void setAll(const T &_value)
Definition: vector.hpp:160
Allows to run closed-loop simulations of dynamic systems.
Simulates the process to be controlled based on a dynamic model.
Definition: process.hpp:71
returnValue getProcessIntermediateStates(VariablesGrid &_interStates)
virtual returnValue initializeControls(const VariablesGrid &_u_init)
Provides an interface to Gnuplot for plotting algorithmic outputs.
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
IntermediateState log(const Expression &arg)


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