95 double rhoc = 1450.00;
125 double PI = 3.1415926535897932;
181 dm = (rhoc*AQ/ 3.0)*dr;
187 h = r*
cos(theta)+60.0 ;
189 w =
log(h/hr) /
log(h0/hr) *(w0+w_extra) ;
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;
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 ) ;
210 ewep[1] = we[1] / nwep ;
211 ewep[2] = we[2] / nwep ;
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];
226 Caer = (rho*A/2.0 )*nwe ;
227 Cf = (rho*dc/8.0)*r*nwe ;
240 Fg [0] = Cg *
cos(theta) ;
242 Fg [2] = Cg *
sin(theta) ;
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] ) ;
257 Ff [1] = Cf * cc* we[1] ;
258 Ff [2] = Cf * cc* we[2] ;
265 F [0] = Fg[0] + Faer[0] ;
266 F [1] = Faer[1] + Ff[1] ;
267 F [2] = Fg[2] + Faer[2] + Ff[2] ;
274 a_pseudo [0] = - ddr0
276 +
sin(theta)*
sin(theta)*dphi *dphi )
279 a_pseudo [1] = - 2.0*
cos(theta)/
sin(theta)*dphi*dtheta
283 a_pseudo [2] =
cos(theta)*
sin(theta)*dphi*dphi
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] ;
304 dn = ( dphi*ddtheta - dtheta*ddphi ) /
305 (2.0*PI*(dphi*dphi + dtheta*dtheta) ) ;
320 regularisation = 5.0e2 * ddr0 * ddr0
321 + 1.0e8 * dPsi * dPsi
324 + 2.5e7 * ddphi * ddphi;
325 + 2.5e7 * ddtheta * ddtheta;
326 + 2.5e6 * dtheta * dtheta;
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 ;
349 f <<
dot(Psi) == dPsi ;
350 f <<
dot(CL) == dCL ;
351 f <<
dot(W) == (-power + regularisation)*1.0e-6;
354 model_response << r ;
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 ;
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));
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));
393 measurements.
setAll( 0.0 );
399 const double t_end = 10.0;
400 OCP ocp( t_start, t_end, 10 );
401 ocp.
minimizeLSQ( Q,model_response, measurements ) ;
413 ocp.
subjectTo( -0.065 <= dPsi <= 0.065 );
426 VariablesGrid disturbance; disturbance.
read(
"my_wind_disturbance_controlsfree.txt" );
428 exit( EXIT_FAILURE );
432 double samplingTime = 1.0;
435 exit( EXIT_FAILURE );
437 exit( EXIT_FAILURE );
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;
465 Controller controller( algorithm, reference );
469 double simulationStart = 0.0;
470 double simulationEnd = 10.0;
475 exit( EXIT_FAILURE );
477 exit( EXIT_FAILURE );
484 diffStates.
print(
"diffStates.txt" );
489 interStates.
print(
"interStates.txt" );
494 sampledProcessOutput.
print(
"sampledOut.txt" );
499 feedbackControl.
print(
"controls.txt" );
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" );
513 window.
addSubplot( feedbackControl(0),
"CONTROL 1 DDR0" );
514 window.
addSubplot( feedbackControl(1),
"CONTROL 1 DPSI" );
515 window.
addSubplot( feedbackControl(2),
"CONTROL 1 DCL" );
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
Calculates the control inputs of the Process based on the Process outputs.
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.
Allows to define a static periodic reference trajectory that the ControlLaw aims to track...
Allows to setup and evaluate output functions based on SymbolicExpressions.
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_)
returnValue addSubplot(PlotWindowSubplot &_subplot)
IntermediateState cos(const Expression &arg)
returnValue getSampledProcessOutput(VariablesGrid &_sampledProcessOutput)
returnValue set(OptionsName name, int value)
returnValue minimizeLSQ(const DMatrix &S, const Function &h, const DVector &r)
returnValue init(const DVector &x0_, const DVector &p_=emptyConstVector)
returnValue minimizeLSQEndTerm(const DMatrix &S, const Function &m, const DVector &r)
returnValue initializeDifferentialStates(const char *fileName, BooleanType autoinit=BT_FALSE)
Data class for defining optimal control problems.
Expression dot(const Expression &arg)
returnValue read(std::istream &stream)
returnValue setProcessDisturbance(const Curve &_processDisturbance)
void setAll(const T &_value)
Allows to run closed-loop simulations of dynamic systems.
Simulates the process to be controlled based on a dynamic model.
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)