int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
Parameter a, b;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
NLP nlp;
nlp.minimize ( a*a + b*b );
nlp.subjectTo( 0.08 <= a );
nlp.subjectTo( 0.1 <= a + b + 0.3*a*a );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE NLP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(nlp);
algorithm.solve();
// PRINT OPTIMAL SOLUTION:
// -----------------------
Vector results;
algorithm.getParameters( results );
results.print( "optimal solution" );
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
Parameter x, y;
// DEFINE AN OPTIMAL CONTROL PROBLEM:
// ----------------------------------
NLP nlp;
nlp.minimize ( 100.0*( y - x*x )*( y - x*x ) + (1.0-x)*(1.0-x) );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE NLP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(nlp);
algorithm.set( KKT_TOLERANCE, 1e-12 );
algorithm.solve();
algorithm.getParameters("rosenbrock_result.txt");
return 0;
}
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
Parameter V ;
Parameter km;
// READ THE MEASUREMENT FROM A DATA FILE:
// --------------------------------------
Matrix m = readFromFile( "michaelis_menten_data.txt" );
// DEFINE A MEASUREMENT FUNCTION:
// ------------------------------
Function h; // the measurement function
int i;
for( i = 0; i < (int) m.getNumRows(); i++ )
h << V*m(i,0)/(km + m(i,0)) - m(i,1);
// DEFINE A PARAMETER ESTIMATION PROBLEM:
// --------------------------------------
NLP nlp;
nlp.minimizeLSQ( h );
nlp.subjectTo( 0.0 <= V <= 2.0 );
nlp.subjectTo( 0.0 <= km <= 2.0 );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE ESTIMATION PROBLEM:
// ------------------------------------------------------------------
ParameterEstimationAlgorithm algorithm(nlp);
algorithm.solve();
VariablesGrid parameters;
algorithm.getParameters( parameters );
return 0;
// GET THE VARIANCE COVARIANCE IN THE SOLUTION:
// ---------------------------------------------
Matrix var;
algorithm.getParameterVarianceCovariance( var );
double LSSE = 2.0*algorithm.getObjectiveValue();
double MSE = LSSE/( m.getNumRows() - 2.0 ); // m.getNumRows() == number of measurements
// 2 == number of parameters
var *= MSE; // rescale the variance-covariance with the MSE factor.
// PRINT THE RESULT ON THE TERMINAL:
// -----------------------------------------------------------------------
printf("\n\nResults for the parameters: \n");
printf("-----------------------------------------------\n");
printf(" V = %.3e +/- %.3e \n", parameters(0,0), sqrt( var(0,0) ) );
printf(" km = %.3e +/- %.3e \n", parameters(0,1), sqrt( var(1,1) ) );
printf("-----------------------------------------------\n\n\n");
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
Parameter y1,y2;
// DEFINE AN OPTIMIZATION PROBLEM:
// -------------------------------
NLP nlp;
nlp.minimize( 0, y1 );
nlp.minimize( 1, y2 );
nlp.subjectTo( 0.0 <= y1 <= 5.0 );
nlp.subjectTo( 0.0 <= y2 <= 5.2 );
nlp.subjectTo( 0.0 <= y2 - 5.0*exp(-y1) - 2.0*exp(-0.5*(y1-3.0)*(y1-3.0)) );
// DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE NLP:
// -----------------------------------------------------
MultiObjectiveAlgorithm algorithm(nlp);
algorithm.set( PARETO_FRONT_GENERATION, PFG_NORMAL_BOUNDARY_INTERSECTION );
algorithm.set( PARETO_FRONT_DISCRETIZATION, 41 );
algorithm.set( KKT_TOLERANCE, 1e-12 );
// Minimize individual objective function
algorithm.initializeParameters("initial_scalar2_2.txt");
algorithm.solveSingleObjective(1);
// Minimize individual objective function
algorithm.solveSingleObjective(0);
// Generate Pareto set
algorithm.solve();
// GET THE RESULT FOR THE PARETO FRONT AND PLOT IT:
// ------------------------------------------------
VariablesGrid paretoFront;
algorithm.getParetoFront( paretoFront );
algorithm.getWeights("scalar2_nbi_weights.txt");
GnuplotWindow window1;
window1.addSubplot( paretoFront, "Pareto Front y1 vs y2", "y1","y2", PM_POINTS );
window1.plot( );
FILE *file = fopen("scalar2_nbi_pareto.txt","w");
paretoFront.print();
file << paretoFront;
fclose(file);
// FILTER THE PARETO FRONT AND PLOT IT:
// ------------------------------------
algorithm.getParetoFrontWithFilter( paretoFront );
algorithm.getWeightsWithFilter("scalar2_nbi_weights_filtered.txt");
GnuplotWindow window2;
window2.addSubplot( paretoFront, "Pareto Front (with filter) y1 vs y2", "y1","y2", PM_POINTS );
window2.plot( );
FILE *file2 = fopen("scalar2_nbi_pareto_filtered.txt","w");
paretoFront.print();
file2 << paretoFront;
fclose(file2);
// PRINT INFORMATION ABOUT THE ALGORITHM:
// --------------------------------------
algorithm.printInfo();
return 0;
}
int main( int argc, char * const argv[] ){
//======================================================================
// PARSE THE INPUT ARGUMENTS:
// ----------------------------------
double IC[4];
/* arguments are passed to the main function by string
* there are 'int argc' arguments
* the first one is the name of the program
* the next ones are arguments passed in the call like
* program number1 number2
* in this stupid simple parser, we directly read doubles from the arguments
*/
int i=1;
while (i < argc) {
// load the i-th string argument passed to the main function
char* input = argv[i];
// parse the string to a double value
IC[i-1] = atof(input);
i++;
}
cout << "Initial Conditions" << endl;
cout << "------------" << endl;
for (i = 0; i < argc-1; ++i) {
cout << i+1 << ":\t" << IC[i] << endl;
}
//======================================================================
USING_NAMESPACE_ACADO
// DIFFERENTIAL STATES :
// -------------------------
Parameter x; // Position
Parameter y; //
Parameter z; //
// ------------------------- // -------------------------------------------
Parameter dx; // Speed
Parameter dy; //
Parameter dz; //
Parameter e11;
Parameter e12;
Parameter e13;
Parameter e21;
Parameter e22;
Parameter e23;
Parameter e31;
Parameter e32;
Parameter e33;
Parameter w1;
Parameter w2;
Parameter w3;
// ------------------------- // -------------------------------------------
Parameter r; // Kite distance
Parameter dr; // Kite distance / dt
//------------------------- // -------------------------------------------
Parameter delta; // Carousel
Parameter ddelta; //
Parameter ur;
Parameter up; // Ailerons
// Collect all the states in a vector
const int n_XD = 24; // Number of states
IntermediateState XD[n_XD]; // Vector collecting all states
XD[0] = x;
XD[1] = y;
XD[2] = z;
XD[3] = dx;
XD[4] = dy;
XD[5] = dz;
XD[6] = e11;
XD[7] = e12;
XD[8] = e13;
XD[9] = e21;
XD[10] = e22;
XD[11] = e23;
XD[12] = e31;
XD[13] = e32;
XD[14] = e33;
XD[15] = w1;
XD[16] = w2;
XD[17] = w3;
XD[18] = r;
XD[19] = dr;
XD[20] = delta;
XD[21] = ddelta;
XD[22] = ur;
XD[23] = up;
// CONTROL :
// -------------------------
Parameter dddelta; // Carousel acceleration
Parameter ddr;
Parameter dur;
Parameter dup; // Ailerons
// Collect all the controls in a vector
const int n_U = 4; // Number of controls
IntermediateState U[n_U]; // Vector collecting all states
U[0] = dddelta;
U[1] = ddr;
U[2] = dur;
U[3] = dup;
// PARAMETERS
// -----------------------
// Parameter SlackCLmax;
// Parameter SlackLambda;
// DEFINITION OF PI :
// ------------------------
double PI = 3.1415926535897932;
//TAIL LENGTH
double LT = 0.45;
//ROLL DAMPING
double RDfac = 1;
double RD0 = 1e-2;
double RD = RDfac*RD0;
// CONSTANTS :
// ------------------------
// PARAMETERS OF THE KITE :
// -----------------------------
double mk = 0.463; // mass of the kite // [ kg ]
// PHYSICAL CONSTANTS :
// -----------------------------
double g = 9.81; // gravitational constant // [ m /s^2]
double rho = 1.23; // density of the air // [ kg/m^3]
// PARAMETERS OF THE CABLE :
// -----------------------------
double rhoc = 1450.00; // density of the cable // [ kg/m^3]
double cc = 1.00; // frictional constant // [ ]
double dc = 1e-3; // diameter // [ m ]
double AQ = PI*dc*dc/4.0;
//CAROUSEL ARM LENGTH
double rA = 1.085; //(dixit Kurt)
//INERTIA MATRIX (Kurt's direct measurements)
// Note: low sensitivity to I1,2,3... high sensitivity to I31...
double I1 = 0.0163;
double I31 = 0.0006;
double I2 = 0.0078;
double I3 = 0.0229;
//WIND-TUNNEL PARAMETERS
//Lift (report p. 67)
//Sensitivity to CLA error low
double CLA = 5.064;
//Sensitivity to CLe error low
double CLe = 0.318;
//Sensitivity to CLr error low
double CLr = 0.85; //?!?!?!?!?
//HIGH sensitivity to CL0 !!
double CL0 = 0.239;
//Drag (report p. 70)
//Sensitivity to CDA error low
double CDA = -0.195;
double CDA2 = 4.268;
double CDB2 = 0;
//Sensitivity to CDe error low
double CDe = 0.044;
//Sensitivity to CDr error low
double CDr = 0.111;
//Sensitivity to CD0 error low
double CD0 = 0.026;
//Roll (report p. 72)
//HIGH sensitivity to CRB !!
double CRB = -0.062;
//HIGH sensitivity to CRAB !!
double CRAB = -0.271;
//Sensitivity to CRr error low
double CRr = -0.244;
//Pitch (report p. 74)
//HIGH sensitivity to CPA !!
double CPA = 0.293;
//Sensitivity to CPe error low
double CPe = -0.821;
//Sensitivity to CPr error low
double CPr = -0.647; //?!?!?!?!?
//HIGH sensitivity to CP0 !!
double CP0 = 0.03;
//Yaw (report p. 76)
//HIGH sensitivity to CYB !!
double CYB = 0.05;
//HIGH sensitivity to CYAB !!
double CYAB = 0.229;
double SPAN = 0.96;
double CHORD = 0.1;
// OTHER VARIABLES :
// ------------------------
IntermediateState mc; // mass of the cable
IntermediateState m ; // effective inertial mass
IntermediateState mgrav; // gravific mass
// IntermediateState dmc; // first derivative of m with respect to t
// ORIENTATION AND FORCES :
// ------------------------
IntermediateState wind ; // the wind at altitude
IntermediateState Cf ; // cable drag
IntermediateState CD ; // the aerodynamic drag coefficient
IntermediateState CL ; // the aerodynamic lift coefficient
IntermediateState CR ; // the aerodynamic roll coefficient
IntermediateState CP ; // the aerodynamic pitch coefficient
IntermediateState CY ; // the aerodynamic yaw coefficient
IntermediateState F [ 3]; // aero forces + gravity
IntermediateState FL [ 3]; // the lift force
IntermediateState FD [ 3]; // the drag force
IntermediateState Ff [ 3]; // the frictional force
// IntermediateState Fcable ; // force in the cable
IntermediateState er [ 3]; // X normed to 1
IntermediateState eTe [ 3]; //unrotated transversal vector (psi = 0)
IntermediateState eLe [ 3]; //unrotated lift vector (psi = 0)
IntermediateState we [ 3]; // effective wind vector
IntermediateState wE [ 3]; // effective wind vector
IntermediateState wp ;
IntermediateState wep [ 3]; // effective wind projected in the plane orthogonal to X
IntermediateState VKite ; //Kite (relative) speed
IntermediateState VKite2 ; //Squared (relative) kite speed
IntermediateState Vp;
IntermediateState VT [3];
IntermediateState alpha;
IntermediateState beta;
IntermediateState alphaTail;
IntermediateState T [3];
// TERMS ON RIGHT-HAND-SIDE
// OF THE DIFFERENTIAL
// EQUATIONS :
// ------------------------
IntermediateState de11;
IntermediateState de12;
IntermediateState de13;
IntermediateState de21;
IntermediateState de22;
IntermediateState de23;
IntermediateState de31;
IntermediateState de32;
IntermediateState de33;
// MODEL EQUATIONS :
// ===============================================================
// CROSS AREA OF THE CABLE :
// ---------------------------------------------------------------
// AQ = PI*dc*dc/4.0 ;
// THE EFECTIVE MASS' :
// ---------------------------------------------------------------
mc = rhoc*AQ*r ; // mass of the cable
m = mk + mc/3.0; // effective inertial mass
mgrav = mk + mc/2.0; // effective inertial mass
// ----------------------------- // ----------------------------
// dm = (rhoc*AQ/ 3.0)*dr; // time derivative of the mass
// WIND SHEAR MODEL :
// ---------------------------------------------------------------
wind = 0.;
// EFFECTIVE WIND IN THE KITE`S SYSTEM :
// ---------------------------------------------------------------
we[0] = -wind + dx;
we[1] = dy;
we[2] = dz;
VKite2 = (we[0]*we[0] + we[1]*we[1] + we[2]*we[2]);
VKite = sqrt(VKite2);
// CALCULATION OF THE FORCES :
// ---------------------------------------------------------------
// er
er[0] = x/r;
er[1] = y/r;
er[2] = z/r;
//Velocity accross X (cable drag)
wp = er[0]*we[0] + er[1]*we[1] + er[2]*we[2];
wep[0] = we[0] - wp*er[0];
wep[1] = we[1] - wp*er[1];
wep[2] = we[2] - wp*er[2];
//Aero coeff.
// LIFT DIRECTION VECTOR
// -------------------------
//Relative wind speed in Airfoil's referential 'E'
wE[0] = e11*we[0] + e21*we[1] + e31*we[2];
wE[1] = e12*we[0] + e22*we[1] + e32*we[2];
wE[2] = e13*we[0] + e23*we[1] + e33*we[2];
//Airfoil's transversal axis in fixed referential 'e'
eTe[0] = e12;
eTe[1] = e22;
eTe[2] = e32;
// Lift axis ** Normed to we !! **
eLe[0] = - eTe[1]*we[2] + eTe[2]*we[1];
eLe[1] = - eTe[2]*we[0] + eTe[0]*we[2];
eLe[2] = - eTe[0]*we[1] + eTe[1]*we[0];
// AERODYNAMIC COEEFICIENTS
// ----------------------------------
//VT = cross([w1;w2;w3],[-LT;0;0]) + wE;
VT[0] = wE[0];
VT[1] = -LT*w3 + wE[1];
VT[2] = LT*w2 + wE[2];
alpha = -wE[2]/wE[0];
//Note: beta & alphaTail are compensated for the tail motion induced by omega
beta = VT[1]/sqrt(VT[0]*VT[0] + VT[2]*VT[2]);
alphaTail = -VT[2]/VT[0];
CL = CLA*alpha + CLe*up + CLr*ur + CL0;
CD = CDA*alpha + CDA2*alpha*alpha + CDB2*beta*beta + CDe*up + CDr*ur + CD0;
CR = -RD*w1 + CRB*beta + CRAB*alphaTail*beta + CRr*ur;
CP = CPA*alphaTail + CPe*up + CPr*ur + CP0;
CY = CYB*beta + CYAB*alphaTail*beta;
Cf = rho*dc*r*VKite/8.0;
// THE FRICTION OF THE CABLE :
// ---------------------------------------------------------------
Ff[0] = -rho*dc*r*VKite*cc*wep[0]/8.0;
Ff[1] = -rho*dc*r*VKite*cc*wep[1]/8.0;
Ff[2] = -rho*dc*r*VKite*cc*wep[2]/8.0;
// LIFT :
// ---------------------------------------------------------------
FL[0] = rho*CL*eLe[0]*VKite/2.0;
FL[1] = rho*CL*eLe[1]*VKite/2.0;
FL[2] = rho*CL*eLe[2]*VKite/2.0;
// DRAG :
// ---------------------------------------------------------------
FD[0] = -rho*VKite*CD*we[0]/2.0;
FD[1] = -rho*VKite*CD*we[1]/2.0;
FD[2] = -rho*VKite*CD*we[2]/2.0;
// FORCES (AERO)
// ---------------------------------------------------------------
F[0] = FL[0] + FD[0] + Ff[0];
F[1] = FL[1] + FD[1] + Ff[1];
F[2] = FL[2] + FD[2] + Ff[2];
// TORQUES (AERO)
// ---------------------------------------------------------------
T[0] = 0.5*rho*VKite2*SPAN*CR;
T[1] = 0.5*rho*VKite2*CHORD*CP;
T[2] = 0.5*rho*VKite2*SPAN*CY;
// ATTITUDE DYNAMICS
// -----------------------------------------------------------
de11 = e12*w3 - e13*w2;
de12 = e13*w1 - e11*w3;
de13 = e11*w2 - e12*w1;
de21 = e22*w3 - e23*w2;
de22 = e23*w1 - e21*w3;
de23 = e21*w2 - e22*w1;
de31 = e32*w3 - e33*w2;
de32 = e33*w1 - e31*w3;
de33 = e31*w2 - e32*w1;
////////////////////////////////////////////////////////////////////////
// //
// AUTO-GENERATED EQUATIONS (S. Gros, HIGHWIND, OPTEC, KU Leuven) //
// //
////////////////////////////////////////////////////////////////////////
// Equations read:
// IMA = inv(MA)
// ddX = IMA*(Bx - CA*lambda)
// lambdaNum = CA^T*IMA*Bx - Blambda
// lambdaDen = CA^T*IMA*CA
// lambda = lambdaNum/lambdaDen
// Arm
IntermediateState xA;
IntermediateState dxA;
IntermediateState ddxA;
IntermediateState yA;
IntermediateState dyA;
IntermediateState ddyA;
xA = -rA*sin(delta);
dxA = -(ddelta*rA*cos(delta));
ddxA = -(dddelta*rA*cos(delta) - ddelta*ddelta*rA*sin(delta));
yA = rA*cos(delta);
dyA = -ddelta*rA*sin(delta);
ddyA = - rA*cos(delta)*ddelta*ddelta - dddelta*rA*sin(delta);
// BUILD DYNAMICS
IntermediateState lambdaNum;
lambdaNum = ddxA*xA - 2*dy*dyA - ddr*r - ddxA*x - 2*dx*dxA - ddyA*y + ddyA*yA - dr*dr + dx*dx + dxA*dxA + dy*dy + dyA*dyA + dz*dz + (F[0]*(x - xA))/m + (F[1]*(y - yA))/m + (z*(F[2] - g*mgrav))/m;
IntermediateState lambdaDen;
lambdaDen = x*x/m + xA*xA/m + y*y/m + yA*yA/m + z*z/m - (2*x*xA)/m - (2*y*yA)/m;
IntermediateState lambda;
lambda = lambdaNum/lambdaDen;
// IntermediateState ddX(6,1);
IntermediateState ddx = (F[0] - lambda*(x - xA))/m;
IntermediateState ddy = (F[1] - lambda*(y - yA))/m;
IntermediateState ddz = -(g*mgrav - F[2] + lambda*z)/m;
IntermediateState dw1 = (I31*(T[2] + w2*(I1*w1 + I31*w3) - I2*w1*w2))/(I31*I31 - I1*I3) - (I3*(T[0] - w2*(I31*w1 + I3*w3) + I2*w2*w3))/(I31*I31 - I1*I3);
IntermediateState dw2 = (T[1] + w1*(I31*w1 + I3*w3) - w3*(I1*w1 + I31*w3))/I2;
IntermediateState dw3 = (I31*(T[0] - w2*(I31*w1 + I3*w3) + I2*w2*w3))/(I31*I31 - I1*I3) - (I1*(T[2] + w2*(I1*w1 + I31*w3) - I2*w1*w2))/(I31*I31 - I1*I3);
// BUILD CONSTRAINTS
IntermediateState Const, dConst;
Const = - r*r/2 + x*x/2 - x*xA + xA*xA/2 + y*y/2 - y*yA + yA*yA/2 + z*z/2;
dConst = dx*x - dr*r - dxA*x - dx*xA + dxA*xA + dy*y - dyA*y - dy*yA + dyA*yA + dz*z;
///////////////////////////// END OF AUTO-GENERATED CODE //////////////////////////////////////////////////////
// Fcable = lambda*r;
// ===============================================================
// END OF MODEL EQUATIONS
// ===============================================================
IntermediateState Cost = 0;
for ( i=0; i < n_U; i++ ) {
Cost += U[i]*U[i];
}
// DEFINE THE NLP:
// ----------------------------------
NLP nlp;
nlp.minimize( Cost );
nlp.minimize( (CL-0.5)*(CL-0.5) );
// CONSTRAINTS:
// ---------------------------------
// State bounds
nlp.subjectTo( -0.2 <= ur <= 0.2 );
nlp.subjectTo( -0.2 <= up <= 0.2 );
nlp.subjectTo( 0.0 <= x );
nlp.subjectTo( rA <= y );
/* nlp.subjectTo( -1.0 <= e11 <= 1.0 );
nlp.subjectTo( -1.0 <= e12 <= 1.0 );
nlp.subjectTo( -1.0 <= e13 <= 1.0 );
nlp.subjectTo( -1.0 <= e21 <= 1.0 );
nlp.subjectTo( -1.0 <= e22 <= 1.0 );
nlp.subjectTo( -1.0 <= e23 <= 1.0 );
nlp.subjectTo( -1.0 <= e31 <= 1.0 );
nlp.subjectTo( -1.0 <= e32 <= 1.0 );
nlp.subjectTo( -1.0 <= e33 <= 1.0 );*/
nlp.subjectTo( delta == IC[2] );
nlp.subjectTo( ddelta == IC[3] );
// Flight envelope
nlp.subjectTo( 0 <= CL <= 1 );
// nlp.subjectTo( CL == 0.5 );
nlp.subjectTo( 1 <= lambda/10 );
nlp.subjectTo( -10*PI/180 <= beta <= 10*PI/180 );
// nlp.subjectTo( -10*PI/180 <= alpha <= 10*PI/180 );
nlp.subjectTo( -15*PI/180 <= alphaTail <= 15*PI/180 );
nlp.subjectTo( 0 <= wE[0] );
// Invariants
nlp.subjectTo( Const == 0 );
nlp.subjectTo( dConst == 0 );
nlp.subjectTo( e11*e11 + e12*e12 + e13*e13 - 1 == 0 );
nlp.subjectTo( e11*e21 + e12*e22 + e13*e23 == 0 );
nlp.subjectTo( e11*e31 + e12*e32 + e13*e33 == 0 );
nlp.subjectTo( e21*e21 + e22*e22 + e23*e23 - 1 == 0 );
nlp.subjectTo( e21*e31 + e22*e32 + e23*e33 == 0 );
nlp.subjectTo( e31*e31 + e32*e32 + e33*e33 - 1 == 0 );
// Equilibrium
// ROTATE BACK THE SYSTEM DYNAMICS:
// ---------------------------------------------------------------
nlp.subjectTo( z == IC[0] );
nlp.subjectTo( r == IC[1] );
// Rdot*X R*dX
// nlp.subjectTo( (-x)*ddelta + dy == 0 );
nlp.subjectTo( (-y)*ddelta - dx == 0 ); // You never have y=0
nlp.subjectTo( dz == 0 );
nlp.subjectTo( dr == 0 );
// mul(Rdotdot,X) + 2*mul(Rdot,Xdot) + mul(R,Xdotdot)
// nlp.subjectTo( ( ( -dddelta )*x + ( -ddelta*ddelta )*y ) + 2*(-dx)*ddelta + ddy == 0 );
nlp.subjectTo( ( ( ddelta*ddelta )*x + ( -dddelta )*y ) + 2*(-dy)*ddelta - ddx == 0 );
nlp.subjectTo( ddz == 0 );
nlp.subjectTo( e11*w1 + e12*w2 + e13*w3 == 0 );
nlp.subjectTo( e21*w1 + e22*w2 + e23*w3 == 0 );
nlp.subjectTo( e31*w1 + e32*w2 + e33*w3 - ddelta == 0 );
nlp.subjectTo( dw1 == 0 );
nlp.subjectTo( dw2 == 0 );
nlp.subjectTo( dw3 == 0 );
nlp.subjectTo( dddelta == 0 );
nlp.subjectTo( ddr == 0 );
nlp.subjectTo( dur == 0 );
nlp.subjectTo( dup == 0 );
// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
// ---------------------------------------------------
OptimizationAlgorithm algorithm(nlp);
algorithm.set ( MAX_NUM_ITERATIONS, 100 );
algorithm.set ( KKT_TOLERANCE , 1e-6 );
algorithm.initializeParameters("EQ_init.txt" );
algorithm.set( MIN_LINESEARCH_PARAMETER, 1e-4 );
algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
// algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );
algorithm.solve();
algorithm.getParameters("EQ_params.txt" );
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// -------------------------
Parameter y1,y2,y3;
// DEFINE AN OPTIMIZATION PROBLEM:
// -------------------------------
NLP nlp;
nlp.minimize( 0, y1 );
nlp.minimize( 1, y2 );
nlp.minimize( 2, y3 );
nlp.subjectTo( -5.0 <= y1 <= 5.0 );
nlp.subjectTo( -5.0 <= y2 <= 5.0 );
nlp.subjectTo( -5.0 <= y3 <= 5.0 );
nlp.subjectTo( y1*y1+y2*y2+y3*y3 <= 4.0 );
// DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE NLP:
// -----------------------------------------------------
MultiObjectiveAlgorithm algorithm(nlp);
algorithm.set( PARETO_FRONT_GENERATION, PFG_NORMALIZED_NORMAL_CONSTRAINT );
algorithm.set( PARETO_FRONT_DISCRETIZATION, 11 );
// Minimize individual objective function
algorithm.solveSingleObjective(0);
// Minimize individual objective function
algorithm.solveSingleObjective(1);
// Minimize individual objective function
algorithm.solveSingleObjective(2);
// Generate Pareto set
algorithm.solve();
algorithm.getWeights("scalar3_nnc_weights.txt");
// GET THE RESULT FOR THE PARETO FRONT AND PLOT IT:
// ------------------------------------------------
VariablesGrid paretoFront;
// algorithm.getParetoFrontWithFilter( paretoFront );
algorithm.getParetoFront( paretoFront );
//GnuplotWindow window;
//window.addSubplot3D( paretoFront, "Pareto Front y1 vs y2 vs y3","y1","y2", PM_POINTS );
//window.plot( );
FILE *file = fopen("scalar3_nnc_pareto.txt","w");
paretoFront.print();
file << paretoFront;
fclose(file);
// PRINT INFORMATION ABOUT THE ALGORITHM:
// --------------------------------------
algorithm.printInfo();
return 0;
}
/* >>> start tutorial code >>> */
int main( ){
USING_NAMESPACE_ACADO
// INTRODUCE THE VARIABLES:
// ------------------------
Parameter y1,y2;
// DEFINE AN OPTIMIZATION PROBLEM:
// -------------------------------
NLP nlp;
nlp.minimize( 0, y1 );
nlp.minimize( 1, y2 );
nlp.subjectTo( 0.0 <= y1 <= 5.0 );
nlp.subjectTo( 0.0 <= y2 <= 5.2 );
nlp.subjectTo( 0.0 <= y2 - 5.0*exp(-y1) - 2.0*exp(-0.5*(y1-3.0)*(y1-3.0)) );
// DEFINE A MULTI-OBJECTIVE ALGORITHM AND SOLVE THE NLP:
// -----------------------------------------------------
MultiObjectiveAlgorithm algorithm(nlp);
algorithm.set( PARETO_FRONT_GENERATION, PFG_WEIGHTED_SUM );
algorithm.set( PARETO_FRONT_DISCRETIZATION, 41 );
algorithm.set( KKT_TOLERANCE, 1e-12 );
// Generate Pareto set
algorithm.solve();
algorithm.getWeights("scalar2_ws_weights.txt");
// GET THE RESULT FOR THE PARETO FRONT AND PLOT IT:
// ------------------------------------------------
VariablesGrid paretoFront;
algorithm.getParetoFront( paretoFront );
GnuplotWindow window1;
window1.addSubplot( paretoFront, "Pareto Front y1 vs y2", "y1","y2", PM_POINTS );
window1.plot( );
paretoFront.print();
// FILTER THE PARETO FRONT AND PLOT IT:
// ------------------------------------
algorithm.getParetoFrontWithFilter( paretoFront );
GnuplotWindow window2;
window2.addSubplot( paretoFront, "Pareto Front (with filter) y1 vs y2", "y1","y2", PM_POINTS );
window2.plot( );
paretoFront.print();
// PRINT INFORMATION ABOUT THE ALGORITHM:
// --------------------------------------
algorithm.printInfo();
return 0;
}
int main(int argc, char **argv)
{
USING_NAMESPACE_ACADO
const double KKT_tol = 1e-6;
//READ THE DEMO LENGTHS & nBF FROM THE COMMAND LINE
std::deque<std::string> args(argv + 1, argv + argc + !argc);
const int nBF=atoi(args[0].c_str());
args.pop_front();
const int nD=(int)args.size();
int nS=0;
for(int i=0; i<nD;i++)
nS+=atoi(args[i].c_str());
//READ DATA
std::string path=DATA_PATH;
Matrix D = readFromFile((path+"demos.dat").c_str()); //d(:,0)=time, d(:,1)=x, d(:,2)=dx, d(:,3)=ddx;
Vector pI = readFromFile((path+"initial_guess.dat").c_str());
Matrix A = readFromFile((path+"inequality_constraint_matrix.dat").c_str());
Vector b = readFromFile((path+"inequality_constraint_vector.dat").c_str());
Matrix S = readFromFile((path+"scale_matrix.dat").c_str());
//RELEVANT INDEX SETS
std::vector<std::vector<int> > d_ind=getDemoInd(args);
std::vector<int> a_ind=getAInd(nBF,nD);
std::vector<int> b_ind=getBInd(nBF,nD);
std::vector<std::vector<int> > w_ind=getWInd(nBF,nD);
std::vector<int> r_ind=getRInd(nBF,nD);
std::vector<int> c_ind=getCInd(nBF,nD);
//PARAMETER & OBJECTIVE FUNCTION
Parameter p(2*nD+nBF*(2+nD)+1,1);
Matrix BM(nS,2*nD+nBF*(2+nD)+1); BM.setZero();
Expression B(BM);
double t,x,dx;
for (int d=0; d<nD; d++)
for(int s=0;s<(int)d_ind[d].size();s++)
{
t=D(d_ind[d][s],0);
x=D(d_ind[d][s],1);
dx=D(d_ind[d][s],2);
B(d_ind[d][s],a_ind[d])=x;
B(d_ind[d][s],b_ind[d])=dx;
for(int n=0;n<nBF;n++){
B(d_ind[d][s],w_ind[d][n])=(-0.5*(t-p(c_ind[n])*S(c_ind[n],c_ind[n])).getPowInt(2)/(p(r_ind[n])*p(r_ind[n])*S(r_ind[n],r_ind[n])*S(r_ind[n],r_ind[n]))).getExp();
// std::cout<<d<<std::endl;
//std::cout<< S(r_ind[d],r_ind[d])<<std::endl;
}
}
Expression f;
f<<B*S*p-D.getCol(3);
Expression ez(nS);
for (int i=0; i<nS; i++)
ez(i)=p(2*nD+nBF*(2+nD));
Vector e(nS); e.setAll(1.0);
Vector null(nS); null.setAll(0.0);
NLP nlp;
nlp.minimize(p(2*nD+nBF*(2+nD)));
nlp.subjectTo(f - ez <= null);
nlp.subjectTo(f + ez >= null);
//nlp.subjectTo(A*S*p <= b);
//ALGORITHM
ParameterEstimationAlgorithm algorithm(nlp);
VariablesGrid initial_guess(2*nD+nBF*(2+nD)+1,0.0,0.0,1 );
initial_guess.setVector( 0,S.getInverse()*pI );
algorithm.initializeParameters(initial_guess);
// OPTIONS
algorithm.set( KKT_TOLERANCE, KKT_tol);
algorithm.set( ABSOLUTE_TOLERANCE, 1e-4);
algorithm.set( PRINTLEVEL,HIGH);
algorithm.set( MAX_NUM_ITERATIONS, 2000 );
algorithm.set (PRINT_COPYRIGHT, NO);
// algorithm.set (PRINT_SCP_METHOD_PROFILE, YES);
algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
algorithm.set(GLOBALIZATION_STRATEGY, GS_LINESEARCH );
algorithm.set(LINESEARCH_TOLERANCE, 1e-2 );
algorithm.set(INFEASIBLE_QP_HANDLING,IQH_RELAX_L2);
algorithm.set(FEASIBILITY_CHECK,BT_TRUE);
// LOGGING
LogRecord logRecord( LOG_AT_EACH_ITERATION,(path+"log.dat").c_str(),PS_PLAIN);
logRecord << LOG_OBJECTIVE_VALUE;
algorithm << logRecord;
//SOLVING
double clock1 = clock();
algorithm.solve();
double clock2 = clock();
Vector solution;
algorithm.getParameters(solution);
// solution.print("optimal solution \n");
solution.printToFile((path+"solution.dat").c_str(),"",PS_PLAIN);
printf("\n computation time (ACADO) = %.16e \n", (clock2-clock1)/CLOCKS_PER_SEC);
return 0;
}
