45 void ffcn_model(
double *x,
double *f,
void *user_data ){
52 double dtheta = x[ 5];
81 double rhoc = 1450.00;
112 double PI = 3.1415926535897932;
140 double a_pseudo [ 3];
147 double regularisation ;
168 dm = (rhoc*AQ/ 3.0)*dr;
175 w =
log(h/hr) /
log(h0/hr) *w0 ;
181 we[0] = w*
sin(theta)*
cos(phi) - dr ;
182 we[1] = -w*
sin(phi) - r*
sin(theta)*dphi ;
183 we[2] = -w*
cos(theta)*
cos(phi) + r *dtheta;
189 nwep =
pow( we[1]*we[1] + we[2]*we[2], 0.5 );
190 nwe =
pow( we[0]*we[0] + we[1]*we[1] + we[2]*we[2], 0.5 );
191 eta =
asin( we[0]*
tan(Psi)/ nwep ) ;
196 ewep[1] = we[1] / nwep ;
197 ewep[2] = we[2] / nwep ;
202 et [1] = (-
cos(Psi)*
sin(eta))*ewep[1] - (
cos(Psi)*
cos(eta))*ewep[2];
203 et [2] = (-
cos(Psi)*
sin(eta))*ewep[2] + (
cos(Psi)*
cos(eta))*ewep[1];
212 Caer = (rho*A/2.0 )*nwe ;
213 Cf = (rho*dc/8.0)*r*nwe ;
226 Fg [0] = Cg *
cos(theta) ;
228 Fg [2] = Cg *
sin(theta) ;
234 Faer[0] = Caer*( CL*(we[1]*et[2]-we[2]*et[1]) + CD*we[0] ) ;
235 Faer[1] = Caer*( CL*(we[2]*et[0]-we[0]*et[2]) + CD*we[1] ) ;
236 Faer[2] = Caer*( CL*(we[0]*et[1]-we[1]*et[0]) + CD*we[2] ) ;
243 Ff [1] = Cf * cc* we[1] ;
244 Ff [2] = Cf * cc* we[2] ;
251 F [0] = Fg[0] + Faer[0] ;
252 F [1] = Faer[1] + Ff[1] ;
253 F [2] = Fg[2] + Faer[2] + Ff[2] ;
260 a_pseudo [0] = - ddr0
262 +
sin(theta)*
sin(theta)*dphi *dphi )
265 a_pseudo [1] = - 2.0*
cos(theta)/
sin(theta)*dphi*dtheta
269 a_pseudo [2] =
cos(theta)*
sin(theta)*dphi*dphi
279 ddr = F[0]/m + a_pseudo[0] ;
280 ddphi = F[1]/(m*r*
sin(theta)) + a_pseudo[1] ;
281 ddtheta = -F[2]/(m*r ) + a_pseudo[2] ;
290 dn = ( dphi*ddtheta - dtheta*ddphi ) /
291 (2.0*PI*(dphi*dphi + dtheta*dtheta) ) ;
306 regularisation = 5.0e2 * ddr0 * ddr0
307 + 1.0e8 * dPsi * dPsi
310 + 2.5e7 * ddphi * ddphi
311 + 2.5e7 * ddtheta * ddtheta
312 + 2.5e6 * dtheta * dtheta;
327 f[9] = (-power + regularisation)*1.0e-6;
400 OCP ocp( 0.0, 18.0, 18 );
415 ocp.
subjectTo( 0.0, theta , -theta , 0.0 );
417 ocp.
subjectTo( 0.0, dphi , -dphi , 0.0 );
418 ocp.
subjectTo( 0.0, dtheta, -dtheta, 0.0 );
429 ocp.
subjectTo( -0.065 <= dPsi <= 0.065 );
438 window.
addSubplot( phi,
"POSITION ANGLE phi [rad]" );
439 window.
addSubplot( theta,
"POSITION ANGLE theta [rad]" );
440 window.
addSubplot( Psi,
"ROLL ANGLE psi [rad]" );
441 window.
addSubplot( CL,
"LIFT COEFFICIENT CL" );
443 window.
addSubplot( phi,theta,
"Kite Orbit",
"theta [rad]",
"phi [rad]" );
USING_NAMESPACE_ACADO IntermediateState sin(const Expression &arg)
returnValue initializeControls(const char *fileName)
User-interface to formulate and solve optimal control problems and static NLPs.
#define USING_NAMESPACE_ACADO
IntermediateState asin(const Expression &arg)
IntermediateState pow(const Expression &arg1, const Expression &arg2)
IntermediateState tan(const Expression &arg)
returnValue subjectTo(const DifferentialEquation &differentialEquation_)
returnValue minimizeMayerTerm(const Expression &arg)
returnValue addSubplot(PlotWindowSubplot &_subplot)
IntermediateState cos(const Expression &arg)
returnValue set(OptionsName name, int value)
returnValue initializeDifferentialStates(const char *fileName, BooleanType autoinit=BT_FALSE)
Data class for defining optimal control problems.
USING_NAMESPACE_ACADO void ffcn_model(double *x, double *f, void *user_data)
Provides an interface to Gnuplot for plotting algorithmic outputs.
virtual returnValue solve()
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
IntermediateState log(const Expression &arg)