C++ GnuplotWindow::addSubplot方法代码示例

int main( ){

    USING_NAMESPACE_ACADO

    // DEFINE A RIGHT-HAND-SIDE:
    // -------------------------
    DifferentialState         x;
    AlgebraicState            z;
    Parameter               p,q;


    IntermediateState is(4);
     is(0) = x;
     is(1) = z;
     is(2) = p;
     is(3) = q;

    CFunction simpledaeModel( 2, ffcn_model );

    // Define a Right-Hand-Side:
    // -------------------------

    DifferentialEquation f;

    f << simpledaeModel(is);



    // DEFINE AN INTEGRATOR:
    // ---------------------
    IntegratorBDF integrator(f);


    // DEFINE INITIAL VALUES:
    // ----------------------
    double x0   =  1.0;
    double z0   =  1.000000;

    double pp[2] = { 1.0, 1.0 };

    Grid interval( 0.0, 1.0, 100 );


    // START THE INTEGRATION:
    // ----------------------
    integrator.integrate( interval, &x0, &z0, pp );

    VariablesGrid differentialStates;
    VariablesGrid algebraicStates   ;
    VariablesGrid intermediateStates;

    integrator.getX ( differentialStates );
    integrator.getXA( algebraicStates    );
    integrator.getI ( intermediateStates );

    GnuplotWindow window;
        window.addSubplot( differentialStates(0) );
        window.addSubplot( algebraicStates   (0) );

    window.plot();


    return 0;
}

//.........这里部分代码省略.........

    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    DifferentialEquation f;

	f << dot(xB) == vB;
	f << dot(xW) == vW;
	f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB;
	f << dot(vW) == (  kS*xB - (kT+kS)*xW + kT*R - F ) / mW;


    // DEFINE LEAST SQUARE FUNCTION:
    // -----------------------------
    Function h;

    h << xB;
    h << xW;
	h << vB;
    h << vW;

    Matrix Q(4,4);
    Q.setIdentity();
	Q(0,0) = 10.0;
	Q(1,1) = 10.0;

    Vector r(4);
    r.setAll( 0.0 );


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    const double t_start = 0.0;
    const double t_end   = 1.0;

    OCP ocp( t_start, t_end, 20 );

    ocp.minimizeLSQ( Q, h, r );

	ocp.subjectTo( f );

	ocp.subjectTo( -500.0 <= F <= 500.0 );
	ocp.subjectTo( R == 0.0 );



    // SETTING UP THE (SIMULATED) PROCESS:
    // -----------------------------------
	OutputFcn identity;
	DynamicSystem dynamicSystem( f,identity );

	Process process( dynamicSystem,INT_RK45 );

	VariablesGrid disturbance = readFromFile( "road.txt" );
	process.setProcessDisturbance( disturbance );


    // SETTING UP THE MPC CONTROLLER:
    // ------------------------------
	RealTimeAlgorithm alg( ocp,0.05 );
	alg.set( MAX_NUM_ITERATIONS, 2 );
	
	StaticReferenceTrajectory zeroReference;

	Controller controller( alg,zeroReference );


    // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
    // ----------------------------------------------------------
	SimulationEnvironment sim( 0.0,3.0,process,controller );

	Vector x0(4);
	x0(0) = 0.01;
	x0(1) = 0.0;
	x0(2) = 0.0;
	x0(3) = 0.0;

	sim.init( x0 );
	sim.run( );


    // ...AND PLOT THE RESULTS
    // ----------------------------------------------------------
	VariablesGrid sampledProcessOutput;
	sim.getSampledProcessOutput( sampledProcessOutput );

	VariablesGrid feedbackControl;
	sim.getFeedbackControl( feedbackControl );

	GnuplotWindow window;
	window.addSubplot( sampledProcessOutput(0), "Body Position [m]" );
	window.addSubplot( sampledProcessOutput(1), "Wheel Position [m]" );
	window.addSubplot( sampledProcessOutput(2), "Body Velocity [m/s]" );
	window.addSubplot( sampledProcessOutput(3), "Wheel Velocity [m/s]" );
	window.addSubplot( feedbackControl(1),      "Damping Force [N]" );
	window.addSubplot( feedbackControl(0),      "Road Excitation [m]" );
	window.plot( );


    return 0;
}

int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState         x;
    DifferentialState         l;
    AlgebraicState            z;
    Control                   u;
    DifferentialEquation      f;
//     Disturbance R;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    f << dot(x) == -x + 0.5*x*x + u + 0.5*z  ;
    f << dot(l) ==  x*x + 3.0*u*u         ;
    f <<      0 ==  z + exp(z) - 1.0 + x     ;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0.0, 5.0, 10 );
    ocp.minimizeMayerTerm( l );

    ocp.subjectTo( f );
//     ocp.subjectTo( R == 0.0 );


    // SETTING UP THE (SIMULATED) PROCESS:
    // -----------------------------------
	OutputFcn identity;
	DynamicSystem dynamicSystem( f,identity );

	Process process( dynamicSystem,INT_BDF );

	//VariablesGrid disturbance = readFromFile( "dae_simulation_disturbance.txt" );
	//process.setProcessDisturbance( disturbance );


    // SETTING UP THE MPC CONTROLLER:
    // ------------------------------
	RealTimeAlgorithm alg( ocp,0.5 );

	StaticReferenceTrajectory zeroReference;
	Controller controller( alg,zeroReference );


    // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
    // ----------------------------------------------------------
	SimulationEnvironment sim( 0.0,15.0,process,controller );

	Vector x0(2);
	x0(0) = 1;
	x0(1) = 0;

	sim.init( x0 );
	sim.run( );


    // ...AND PLOT THE RESULTS
    // ----------------------------------------------------------
	VariablesGrid diffStates;
	sim.getProcessDifferentialStates( diffStates );
	diffStates.printToFile( "diffStates.txt" );
	diffStates.printToFile( "diffStates.m","DIFFSTATES",PS_MATLAB );

	VariablesGrid sampledProcessOutput;
    sim.getSampledProcessOutput( sampledProcessOutput );
    sampledProcessOutput.printToFile( "sampledOut.txt" );
    sampledProcessOutput.printToFile( "sampledOut.m","OUT",PS_MATLAB );

    VariablesGrid feedbackControl;
    sim.getFeedbackControl( feedbackControl );
	feedbackControl.printToFile( "controls.txt" );
	feedbackControl.printToFile( "controls.m","CONTROL",PS_MATLAB );

	VariablesGrid algStates;
	sim.getProcessAlgebraicStates( algStates );
	algStates.printToFile( "algStates.txt" );
	algStates.printToFile( "algStates.m","ALGSTATES",PS_MATLAB );


    GnuplotWindow window;
		window.addSubplot( diffStates(0), "DIFFERENTIAL STATE: x" );
		window.addSubplot( diffStates(1), "DIFFERENTIAL STATE: l" );
		window.addSubplot( algStates(0),            "ALGEBRAIC STATE: z"    );
		window.addSubplot( feedbackControl(0),      "CONTRUL: u"            );
    window.plot( );


    return 0;
}

//.........这里部分代码省略.........
						9.3885430857029321E+04,   // P_{top}         = 939 h Pa
						2.5000000000000000E+02,   // \Delta P_{strip}= 2.5 h Pa and \Delta P_{rect} = 1.9 h Pa
						1.4026000000000000E+01,   // F_{vol}         = 14.0 l h^{-1}
						3.2000000000000001E-01,   // X_F             = 0.32
						7.1054000000000002E+01,   // T_F             = 71 oC
						4.7163089489100003E+01,   // T_C             = 47.2 oC
						4.1833910753991770E+00,   // (not in use?)
						2.4899344810136301E+00,   // (not in use?)
						1.8760537088149468E+02    // (not in use?)
		             };

	DVector x0(NXD, xd);
	DVector p0(NP,  pd);


	// DEFINE AN OPTIMAL CONTROL PROBLEM:
	// ----------------------------------
	OCP ocp( t_start, t_end, intervals );

	// LSQ Term on temperature deviations and controls
	Function h;


//     for( i = 0; i < NXD; i++ )
// 		h << 0.001*x(i);


	h << 0.1 * ( z(94)  - 88.0    );   // Temperature tray 14
	h << 0.1 * ( z(108) - 70.0    );   // Temperature tray 28
	h << 0.01 * ( u(0)   - ud[0]   );   // L_vol
	h << 0.01 * ( u(1)   - ud[1]   );   // Q
	ocp.minimizeLSQ( h );

	// W.r.t. differential equation
	ocp.subjectTo( f );

	// Fix states
	ocp.subjectTo( AT_START, x == x0 );

	// Fix parameters
	ocp.subjectTo( p == p0 );

	// Path constraint on controls
	ocp.subjectTo( ud[0] - 2.0 <=  u(0)  <= ud[0] + 2.0 );
	ocp.subjectTo( ud[1] - 2.0 <=  u(1)  <= ud[1] + 2.0 );



	// DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
	// ---------------------------------------------------
	OptimizationAlgorithm algorithm(ocp);

	algorithm.initializeAlgebraicStates("hydroscal_algebraic_states.txt");

	algorithm.set( INTEGRATOR_TYPE, 		 INT_BDF 			);
	algorithm.set( MAX_NUM_ITERATIONS, 		 5	 				);
	algorithm.set( KKT_TOLERANCE, 			 1e-3 				);
	algorithm.set( INTEGRATOR_TOLERANCE, 	 1e-4 				);
	algorithm.set( ABSOLUTE_TOLERANCE  , 	 1e-6 				);
	algorithm.set( PRINT_SCP_METHOD_PROFILE, YES 				);
	algorithm.set( LINEAR_ALGEBRA_SOLVER, 	 SPARSE_LU 			);
	algorithm.set( DISCRETIZATION_TYPE, 	 MULTIPLE_SHOOTING 	);

    //algorithm.set( LEVENBERG_MARQUARDT, 1e-3 );

    algorithm.set( DYNAMIC_SENSITIVITY,  FORWARD_SENSITIVITY_LIFTED );
	//algorithm.set( DYNAMIC_SENSITIVITY,  FORWARD_SENSITIVITY );
	//algorithm.set( CONSTRAINT_SENSITIVITY,  FORWARD_SENSITIVITY );
	//algorithm.set( ALGEBRAIC_RELAXATION,ART_EXPONENTIAL );    //results in an extra step but steps are quicker


	algorithm.solve();

	double clock2 = clock();
	printf("total computation time = %.16e \n", (clock2-clock1)/CLOCKS_PER_SEC  );


	// PLOT RESULTS:
	// ---------------------------------------------------
	VariablesGrid out_states;
	algorithm.getDifferentialStates( out_states );
	out_states.print( "OUT_states.m","STATES",PS_MATLAB );

	VariablesGrid out_controls;
	algorithm.getControls( out_controls );
	out_controls.print( "OUT_controls.m","CONTROLS",PS_MATLAB );

	VariablesGrid out_algstates;
	algorithm.getAlgebraicStates( out_algstates );
	out_algstates.print( "OUT_algstates.m","ALGSTATES",PS_MATLAB );

	GnuplotWindow window;
	window.addSubplot( out_algstates(94),  "Temperature tray 14" );
	window.addSubplot( out_algstates(108), "Temperature tray 28" );
	window.addSubplot( out_controls(0),    "L_vol"               );
	window.addSubplot( out_controls(1),    "Q"                   );
	window.plot( );

    return 0;
}

/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // ------------------------------------
    DifferentialState                v,s,m;
    Control                          u    ;

    const double t_start =    0.0;
    const double t_end   =   10.0;
    const double h       =   0.01;

    DiscretizedDifferentialEquation  f(h) ;


    // DEFINE A DISCRETE-TIME SYTSEM:
    // -------------------------------
    f << next(s) == s + h*v;
    f << next(v) == v + h*(u-0.02*v*v)/m;
    f << next(m) == m - h*0.01*u*u;

	
	Function eta;
	eta << u;

    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 50 );

    //ocp.minimizeLagrangeTerm( u*u );
	ocp.minimizeLSQ( eta );
    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, s ==  0.0 );
    ocp.subjectTo( AT_START, v ==  0.0 );
    ocp.subjectTo( AT_START, m ==  1.0 );

    ocp.subjectTo( AT_END  , s == 10.0 );
    ocp.subjectTo( AT_END  , v ==  0.0 );

    ocp.subjectTo( -0.01 <= v <= 1.3 );


    // DEFINE A PLOT WINDOW:
    // ---------------------
    GnuplotWindow window;
        window.addSubplot( s,"DifferentialState s" );
        window.addSubplot( v,"DifferentialState v" );
        window.addSubplot( m,"DifferentialState m" );
        window.addSubplot( u,"Control u" );
        window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );
        window.addSubplot( 0.5 * m * v*v,"Kinetic Energy" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);
	algorithm.set( INTEGRATOR_TYPE, INT_DISCRETE );
    algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
    algorithm.set( KKT_TOLERANCE, 1e-10 );

    algorithm << window;
    algorithm.solve();

    return 0;
}

int main( ){

  USING_NAMESPACE_ACADO;

  // Parameters
  double h_hw = 10;    // water level
  double A_hw = 1.0;   // amplitude of the waves
  double T_hw = 5.0;   // duration of a wave
  double h_b  = 3.0;   // height of the buoy
  double rho  = 1000;  // density of water
  double A    = 1.0;   // bottom area of the buoy
  double m    = 100;   // mass of the buoy
  double g    = 9.81;  // gravitational constant

  // Free parameter
  Control u;

  // Variables
  DifferentialState h; // Position of the buoy
  DifferentialState v; // Velocity of the buoy
  DifferentialState w; // Produced wave energy
  TIME t;

  // Differential equation
  DifferentialEquation f;
  
  // Height of the wave
  IntermediateState hw;
  hw = h_hw + A_hw*sin(2*M_PI*t/T_hw);
  
  f << dot(h) ==  v;
  f << dot(v) ==  rho*A*(hw-h)/m - g - u;
  f << dot(w) ==  u*v;

  // DEFINE AN OPTIMAL CONTROL PROBLEM:
  // ----------------------------------

  const double t_start    =  0.0        ;
  const double t_end      =  15;

  OCP ocp( t_start, t_end, 100 );
  ocp.maximizeMayerTerm( w );
  ocp.subjectTo( f );
  
//   double x_start[3];
//   x_start[0] = h_hw - 0*A_hw;
//   x_start[1] = 0;
//   x_start[2] = 0;

  ocp.subjectTo( AT_START, h - (h_hw-A_hw) ==  0.0 );
  ocp.subjectTo( AT_START, v ==  0.0 );
  ocp.subjectTo( AT_START, w ==  0.0 );
  
  ocp.subjectTo( -h_b <= h-hw <= 0.0 );
  ocp.subjectTo( 0.0 <= u <= 100.0 );
  
  

      // DEFINE A PLOT WINDOW:
    // ---------------------
  GnuplotWindow window;
  window.addSubplot( h,"Height of buoy" );
  window.addSubplot( v,"Velocity of buoy" );
  window.addSubplot( w,"Objective function " );
  window.addSubplot( u,"Resistance" );
  window.addSubplot( hw,"Wave height" );
  //  window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );

  // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
  // ---------------------------------------------------
  OptimizationAlgorithm algorithm(ocp);
  algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
  algorithm.set( MAX_NUM_ITERATIONS, 100 );
  //  algorithm.set( KKT_TOLERANCE, 1e-10 );
  
  algorithm << window;
  algorithm.solve();

  return 0;
}

//.........这里部分代码省略.........
    // SET AN INITIAL GUESS FOR THE FIRST MPC LOOP (NEXT LOOPS WILL USE AS INITIAL GUESS THE SOLUTION FOUND AT THE PREVIOUS MPC LOOP)
    Grid timeGrid(0.0,T,nb_nodes+1);
    VariablesGrid x_init(16, timeGrid);
    // init with static
    for (int i = 0 ; i<nb_nodes+1 ; i++ ) {
            x_init(i,0) = 0.;
            x_init(i,1) = 0.;
            x_init(i,2) = 0.;
            x_init(i,3) = 0.;
            x_init(i,4) = 0.;
            x_init(i,5) = 0.;
            x_init(i,6) = 0.;
            x_init(i,7) = 0.;
            x_init(i,8) = 0.;
            x_init(i,9) = 0.;
            x_init(i,10) = 0.;
            x_init(i,11) = 0.;
            x_init(i,12) = 58.; //58. is the propeller rotation speed so the total thrust balance the weight of the quad
            x_init(i,13) = 58.;
            x_init(i,14) = 58.;
            x_init(i,15) = 58.;
        }
    alg.initializeDifferentialStates(x_init);

    // SET OPTION AND PLOTS WINDOW
    // ---------------------------
    // Linesearch is an algorithm which will try several points along the descent direction to choose a better step length.
    // It looks like activating this option produice more stable trajectories.198
    alg.set( GLOBALIZATION_STRATEGY, GS_LINESEARCH );

    alg.set(INTEGRATOR_TYPE, INT_RK45);

    // You can uncomment those lines to see how the predicted trajectory involve along time
    // (but be carefull because you will have 1 ploting window per MPC loop)
//    GnuplotWindow window1(PLOT_AT_EACH_ITERATION);
//    window1.addSubplot( z,"DifferentialState z" );
//    window1.addSubplot( x,"DifferentialState x" );
//    window1.addSubplot( theta,"DifferentialState theta" );
//    window1.addSubplot( 16./((x+3)*(x+3)+4*(z-5)*(z-5)),"Dist obs1" );
//    window1.addSubplot( 16./((x-3)*(x-3)+4*(z-9)*(z-9)),"Dist obs2" );
//    window1.addSubplot( 16./((x+2)*(x+2)+4*(z-15)*(z-15)),"Dist obs3" );
//    alg<<window1;


    // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
    // ----------------------------------------------------------
    // The first argument is the starting time, the second the end time.
    SimulationEnvironment sim( 0.0,10.,process,controller );

    //Setting the state of the sytem at the beginning of the simulation.
    DVector x0(16);
    x0.setZero();
    x0(0) = 0.;
    x0(12) = 58.;
    x0(13) = 58.;
    x0(14) = 58.;
    x0(15) = 58.;

    t = clock();
    if (sim.init( x0 ) != SUCCESSFUL_RETURN)
        exit( EXIT_FAILURE );
    if (sim.run( ) != SUCCESSFUL_RETURN)
        exit( EXIT_FAILURE );
    t = clock() - t;
    std::cout << "total time : " << (((float)t)/CLOCKS_PER_SEC)<<std::endl;

    // ...SAVE THE RESULTS IN FILES
    // ----------------------------------------------------------

    std::ofstream file;
    file.open("/tmp/log_state.txt",std::ios::out);
    std::ofstream file2;
    file2.open("/tmp/log_control.txt",std::ios::out);

    VariablesGrid sampledProcessOutput;
    sim.getSampledProcessOutput( sampledProcessOutput );
    sampledProcessOutput.print(file);

    VariablesGrid feedbackControl;
    sim.getFeedbackControl( feedbackControl );
    feedbackControl.print(file2);


    // ...AND PLOT THE RESULTS
    // ----------------------------------------------------------

    GnuplotWindow window;
    window.addSubplot( sampledProcessOutput(0), "x " );
    window.addSubplot( sampledProcessOutput(1), "y " );
    window.addSubplot( sampledProcessOutput(2), "z " );
    window.addSubplot( sampledProcessOutput(6),"phi" );
    window.addSubplot( sampledProcessOutput(7),"theta" );
    window.addSubplot( sampledProcessOutput(8),"psi" );
    window.plot( );

    graphics::corbaServer::ClientCpp client = graphics::corbaServer::ClientCpp();
    client.createWindow("window");

    return 0;
}

/* >>> start tutorial code >>> */
int main( ){


    USING_NAMESPACE_ACADO


    // DEFINE VALRIABLES:
    // ---------------------------
    DifferentialStateVector  q(2);   // the generalized coordinates of the pendulum
    DifferentialStateVector dq(2);   // the associated velocities


    const double L1    = 1.00;       // length of the first pendulum
    const double L2    = 1.00;       // length of the second pendulum
    const double m1    = 1.00;       // mass of the first pendulum
    const double m2    = 1.00;       // mass of the second pendulum
    const double g     = 9.81;       // gravitational constant
    const double alpha = 0.10;       // a friction constant

    const double J_11 = (m1+m2)*L1*L1;   // auxiliary variable (inertia comp.)
    const double J_22 =  m2    *L2*L2;   // auxiliary variable (inertia comp.)
    const double J_12 =  m2    *L1*L2;   // auxiliary variable (inertia comp.)

    const double E1   = -(m1+m2)*g*L1;   // auxiliary variable (pot energy 1)
    const double E2   = - m2    *g*L2;   // auxiliary variable (pot energy 2)

    IntermediateState  c1;
    IntermediateState  c2;
    IntermediateState  c3;

    IntermediateState   T;
    IntermediateState   V;
    IntermediateStateVector Q;


    // COMPUTE THE KINETIC ENERGY T AND THE POTENTIAL V:
    // -------------------------------------------------

    c1 = cos(q(0));
    c2 = cos(q(1));
    c3 = cos(q(0)+q(1));

    T  = 0.5*J_11*dq(0)*dq(0) + 0.5*J_22*dq(1)*dq(1) + J_12*c3*dq(0)*dq(1);
    V  = E1*c1 + E2*c2;
    Q  = (-alpha*dq);


    // AUTOMATICALLY DERIVE THE EQUATIONS OF MOTION BASED ON THE LAGRANGIAN FORMALISM:
    // -------------------------------------------------------------------------------
    DifferentialEquation  f;
    LagrangianFormalism( f, T - V, Q, q, dq );


    // Define an integrator:
    // ---------------------
    IntegratorBDF integrator( f );


    // Define an initial value:
    // ------------------------

    double x_start[4] = { 0.0, 0.5, 0.0, 0.1 };

    double t_start    =   0.0;
    double t_end      =   3.0;

    // START THE INTEGRATION
    // ----------------------
    integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
    integrator.set( INTEGRATOR_TOLERANCE, 1e-12 );

    integrator.freezeAll();
    integrator.integrate( x_start, t_start, t_end );


    VariablesGrid xres;
    integrator.getTrajectory(&xres,NULL,NULL,NULL,NULL,NULL);

    GnuplotWindow window;
        window.addSubplot( xres(0), "The excitation angle of pendulum 1" );
        window.addSubplot( xres(1), "The excitation angle of pendulum 2" );
        window.addSubplot( xres(2), "The angular velocity of pendulum 1" );
        window.addSubplot( xres(3), "The angular velocity of pendulum 2" );
    window.plot();


    return 0;
}

/* >>> start tutorial code >>> */
int main( ){


    USING_NAMESPACE_ACADO

    // Define a Right-Hand-Side:
    // -------------------------
    DifferentialState     x;
    DifferentialEquation  f;
    TIME t;

    f << dot(x) == -x + sin(0.01*t);


    // Define an initial value:
    // ------------------------

	Vector xStart( 1 );
	xStart(0) = 1.0;

    double tStart    =   0.0;
    double tEnd      =   1000.0;

	Grid timeHorizon( tStart,tEnd,2 );
	Grid timeGrid( tStart,tEnd,20 );


    // Define an integration algorithm:
    // --------------------------------

	IntegrationAlgorithm intAlg;
	
	intAlg.addStage( f, timeHorizon );

	intAlg.set( INTEGRATOR_TYPE, INT_BDF );
    intAlg.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
    intAlg.set( INTEGRATOR_TOLERANCE, 1.0e-3 );
	intAlg.set( PRINT_INTEGRATOR_PROFILE, YES );
	intAlg.set( PLOT_RESOLUTION, HIGH );


	GnuplotWindow window;
	window.addSubplot( x,"x" );
	
	intAlg << window;


    // START THE INTEGRATION
    // ----------------------

    intAlg.integrate( timeHorizon, xStart );


    // GET THE RESULTS
    // ---------------

	VariablesGrid differentialStates;
	intAlg.getX( differentialStates );
	
	differentialStates.print( "x" );

	Vector xEnd;
	intAlg.getX( xEnd );
	
	xEnd.print( "xEnd" );


    return 0;
}

/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState     v,s,m;
    Control               u    ;
    DifferentialEquation  f    ;

    const double t_start =  0.0;
    const double t_end   = 10.0;

    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------

    f << dot(s) == v;
    f << dot(v) == (u-0.02*v*v)/m;
    f << dot(m) == -0.01*u*u;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 20 );
    ocp.minimizeLagrangeTerm( u*u );
    ocp.subjectTo( f );

	ocp.subjectTo( AT_START, s ==  0.0 );
    ocp.subjectTo( AT_START, v ==  0.0 );
    ocp.subjectTo( AT_START, m ==  1.0 );
    ocp.subjectTo( AT_END  , s == 10.0 );
    ocp.subjectTo( AT_END  , v ==  0.0 );

    ocp.subjectTo( -0.01 <= v <= 1.3 );
	
	ocp.subjectTo( u*u >= -1.0 );


    // DEFINE A PLOT WINDOW:
    // ---------------------
    GnuplotWindow window;
        window.addSubplot( s,"DifferentialState s" );
        window.addSubplot( v,"DifferentialState v" );
        window.addSubplot( m,"DifferentialState m" );
        window.addSubplot( u,"Control u" );
        window.addSubplot( PLOT_KKT_TOLERANCE,"KKT Tolerance" );
//         window.addSubplot( 0.5 * m * v*v,"Kinetic Energy" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);

// 	algorithm.set( INTEGRATOR_TYPE, INT_BDF );

//     algorithm.set( INTEGRATOR_TOLERANCE, 1e-6 );
//     algorithm.set( KKT_TOLERANCE, 1e-3 );

  //algorithm.set( DYNAMIC_SENSITIVITY,  FORWARD_SENSITIVITY );



    algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );
    algorithm.set( MAX_NUM_ITERATIONS, 20 );
    algorithm.set( KKT_TOLERANCE, 1e-10 );
// 	algorithm.set( MAX_NUM_INTEGRATOR_STEPS, 4 );

    algorithm << window;
    algorithm.solve();

// 	BlockMatrix sens;
// 	algorithm.getSensitivitiesX( sens );
// 	sens.print();

    return 0;
}

int main( )
{
    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------
	DifferentialState xB; //Body Position
	DifferentialState xW; //Wheel Position
	DifferentialState vB; //Body Velocity
	DifferentialState vW; //Wheel Velocity

	Control F;
	Disturbance R;

	double mB = 350.0;
	double mW = 50.0;
	double kS = 20000.0;
	double kT = 200000.0;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    DifferentialEquation f;

	f << dot(xB) == vB;
	f << dot(xW) == vW;
	f << dot(vB) == ( -kS*xB + kS*xW + F ) / mB;
	f << dot(vW) == (  kS*xB - (kT+kS)*xW + kT*R - F ) / mW;


    // DEFINE LEAST SQUARE FUNCTION:
    // -----------------------------
    Function h;

    h << xB;
    h << xW;
	h << vB;
    h << vW;
	h << F;

    DMatrix Q(5,5); // LSQ coefficient matrix
	Q(0,0) = 10.0;
	Q(1,1) = 10.0;
	Q(2,2) = 1.0;
	Q(3,3) = 1.0;
	Q(4,4) = 1.0e-8;

    DVector r(5); // Reference
    r.setAll( 0.0 );


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    const double tStart = 0.0;
    const double tEnd   = 1.0;

    OCP ocp( tStart, tEnd, 20 );

    ocp.minimizeLSQ( Q, h, r );

	ocp.subjectTo( f );

	ocp.subjectTo( -200.0 <= F <= 200.0 );
	ocp.subjectTo( R == 0.0 );


    // SETTING UP THE REAL-TIME ALGORITHM:
    // -----------------------------------
	RealTimeAlgorithm alg( ocp,0.025 );
	alg.set( MAX_NUM_ITERATIONS, 1 );
	alg.set( PLOT_RESOLUTION, MEDIUM );

	GnuplotWindow window;
	  window.addSubplot( xB, "Body Position [m]" );
	  window.addSubplot( xW, "Wheel Position [m]" );
	  window.addSubplot( vB, "Body Velocity [m/s]" );
	  window.addSubplot( vW, "Wheel Velocity [m/s]" );
	  window.addSubplot( F,  "Damping Force [N]" );
	  window.addSubplot( R,  "Road Excitation [m]" );

	alg << window;


    // SETUP CONTROLLER AND PERFORM A STEP:
    // ------------------------------------
	StaticReferenceTrajectory zeroReference( "ref.txt" );

	Controller controller( alg,zeroReference );

	DVector y( 4 );
	y.setZero( );
	y(0) = 0.01;

	if (controller.init( 0.0,y ) != SUCCESSFUL_RETURN)
		exit( 1 );
	if (controller.step( 0.0,y ) != SUCCESSFUL_RETURN)
		exit( 1 );

    return EXIT_SUCCESS;
//.........这里部分代码省略.........

int main( ){

    USING_NAMESPACE_ACADO


    // INTRODUCE THE VARIABLES:
    // -------------------------
    const int N = 2;

    DifferentialState        x, y("", N, 1);
    Control                   u;
    DifferentialEquation      f;

    const double t_start =  0.0;
    const double t_end   = 10.0;


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------

    f << dot(x) == -x + 0.9*x*x + u;

    int i;
    for( i = 0; i < N; i++ )
        f << dot( y(i) ) == -y(i) + 0.5*y(i)*y(i) + u;


    // DEFINE LEAST SQUARE FUNCTION:
    // -----------------------------

    Function h,m;

    h <<     x;
    h << 2.0*u;

    m << 10.0*x  ;
    m <<  0.1*x*x;

    DMatrix S(2,2);
    DVector r(2);

    S.setIdentity();
    r.setAll( 0.1 );


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 5 );

    ocp.minimizeLSQ       ( S, h, r );
    ocp.minimizeLSQEndTerm( S, m, r );

    ocp.subjectTo( f );
    ocp.subjectTo( AT_START, x == 1.0 );

    for( i = 0; i < N; i++ )
        ocp.subjectTo( AT_START, y(i) == 1.0 );


    // Additionally, flush a plotting object
    GnuplotWindow window;
        window.addSubplot( x,"DifferentialState x" );
        window.addSubplot( u,"Control u" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);
    algorithm << window;

//    algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );
//    algorithm.set( DYNAMIC_SENSITIVITY,  FORWARD_SENSITIVITY_LIFTED );
//    algorithm.set( HESSIAN_APPROXIMATION, CONSTANT_HESSIAN );
//    algorithm.set( HESSIAN_APPROXIMATION, FULL_BFGS_UPDATE );
//    algorithm.set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE );
    algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
//    algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON_WITH_BLOCK_BFGS );
//    algorithm.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN );

    // Necessary to use with GN Hessian approximation.
    algorithm.set( LEVENBERG_MARQUARDT, 1e-10 );

    algorithm.solve();

    return 0;
}

//.........这里部分代码省略.........
    power        =   m*ddr*dr                                     ;



// REGULARISATION TERMS :
// ---------------------------------------------------------------


    regularisation =    5.0e2 * ddr0    * ddr0
                        + 1.0e8 * dPsi    * dPsi
                        + 1.0e5 * dCL     * dCL
                        + 2.5e5 * dn      * dn
                        + 2.5e7 * ddphi   * ddphi;
    + 2.5e7 * ddtheta * ddtheta;
    + 2.5e6 * dtheta  * dtheta;
//                   ---------------------------



// THE "RIGHT-HAND-SIDE" OF THE ODE:
// ---------------------------------------------------------------
    DifferentialEquation f;

    f  << dot(r)      ==  dr                             ;
    f  << dot(phi)    ==  dphi                           ;
    f  << dot(theta)  ==  dtheta                         ;
    f  << dot(dr)     ==  ddr0                           ;
    f  << dot(dphi)   ==  ddphi                          ;
    f  << dot(dtheta) ==  ddtheta                        ;
    f  << dot(n)      ==  dn                             ;
    f  << dot(Psi)    ==  dPsi                           ;
    f  << dot(CL)     ==  dCL                            ;
    f  << dot(W)      == (-power + regularisation)*1.0e-6;


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( 0.0, 18.0, 18 );
    ocp.minimizeMayerTerm( W );
    ocp.subjectTo( f );


    // INITIAL VALUE CONSTRAINTS:
    // ---------------------------------
    ocp.subjectTo( AT_START, n == 0.0 );
    ocp.subjectTo( AT_START, W == 0.0 );


    // PERIODIC BOUNDARY CONSTRAINTS:
    // ----------------------------------------
    ocp.subjectTo( 0.0, r     , -r     , 0.0 );
    ocp.subjectTo( 0.0, phi   , -phi   , 0.0 );
    ocp.subjectTo( 0.0, theta , -theta , 0.0 );
    ocp.subjectTo( 0.0, dr    , -dr    , 0.0 );
    ocp.subjectTo( 0.0, dphi  , -dphi  , 0.0 );
    ocp.subjectTo( 0.0, dtheta, -dtheta, 0.0 );
    ocp.subjectTo( 0.0, Psi   , -Psi   , 0.0 );
    ocp.subjectTo( 0.0, CL    , -CL    , 0.0 );

    ocp.subjectTo( -0.34   <= phi   <= 0.34   );
    ocp.subjectTo(  0.85   <= theta <= 1.45   );
    ocp.subjectTo( -40.0   <= dr    <= 10.0   );
    ocp.subjectTo( -0.29   <= Psi   <= 0.29   );
    ocp.subjectTo(  0.1    <= CL    <= 1.50   );
    ocp.subjectTo( -0.7    <= n     <= 0.90   );
    ocp.subjectTo( -25.0   <= ddr0  <= 25.0   );
    ocp.subjectTo( -0.065  <= dPsi  <= 0.065  );
    ocp.subjectTo( -3.5    <= dCL   <= 3.5    );


    // CREATE A PLOT WINDOW AND VISUALIZE THE RESULT:
    // ----------------------------------------------

    GnuplotWindow window;
    window.addSubplot( r,    "CABLE LENGTH  r [m]" );
    window.addSubplot( phi,  "POSITION ANGLE  phi [rad]" );
    window.addSubplot( theta,"POSITION ANGLE theta [rad]" );
    window.addSubplot( Psi,  "ROLL ANGLE  psi [rad]" );
    window.addSubplot( CL,   "LIFT COEFFICIENT  CL" );
    window.addSubplot( W,    "ENERGY  W [MJ]" );
    window.addSubplot( F[0], "FORCE IN CABLE [N]" );
    window.addSubplot( phi,theta, "Kite Orbit","theta [rad]","phi [rad]" );

    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------

    OptimizationAlgorithm algorithm(ocp);

    algorithm.initializeDifferentialStates("powerkite_states.txt"    );
    algorithm.initializeControls          ("powerkite_controls.txt"  );
    algorithm.set                         ( MAX_NUM_ITERATIONS, 100  );
    algorithm.set                         ( KKT_TOLERANCE    , 1e-2 );

    algorithm << window;

    algorithm.set( PRINT_SCP_METHOD_PROFILE, YES );

    algorithm.solve();
    return 0;
}

//.........这里部分代码省略.........


	VariablesGrid disturbance; disturbance.read( "my_wind_disturbance_controlsfree.txt" );
	if (process.setProcessDisturbance( disturbance ) != SUCCESSFUL_RETURN)
		exit( EXIT_FAILURE );

    // SETUP OF THE ALGORITHM AND THE TUNING OPTIONS:
    // ----------------------------------------------
    double samplingTime = 1.0;
    RealTimeAlgorithm  algorithm( ocp, samplingTime );
    if (algorithm.initializeDifferentialStates("p_s_ref.txt"    ) != SUCCESSFUL_RETURN)
    	exit( EXIT_FAILURE );
    if (algorithm.initializeControls          ("p_c_ref.txt"  ) != SUCCESSFUL_RETURN)
    	exit( EXIT_FAILURE );

    algorithm.set( MAX_NUM_ITERATIONS, 2  );
    algorithm.set( KKT_TOLERANCE    , 1e-4 );
    algorithm.set( HESSIAN_APPROXIMATION,GAUSS_NEWTON);
    algorithm.set( INTEGRATOR_TOLERANCE, 1e-6           );
	algorithm.set( GLOBALIZATION_STRATEGY,GS_FULLSTEP );
// 	algorithm.set( USE_IMMEDIATE_FEEDBACK, YES );
	algorithm.set( USE_REALTIME_SHIFTS, YES );
	algorithm.set(LEVENBERG_MARQUARDT, 1e-5);


    DVector x0(10);
    x0(0) =  1.8264164528775887e+03;
    x0(1) = -5.1770453309520573e-03;
    x0(2) =  1.2706440287266794e+00;
    x0(3) =  2.1977888424944396e+00;
    x0(4) =  3.1840786108641383e-03;
    x0(5) = -3.8281200674676448e-02;
    x0(6) =  0.0000000000000000e+00;
    x0(7) = -1.0372313936413566e-02;
    x0(8) =  1.4999999999999616e+00;
    x0(9) =  0.0000000000000000e+00;


    // SETTING UP THE NMPC CONTROLLER:
    // -------------------------------

	Controller controller( algorithm, reference );

    // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
    // ----------------------------------------------------------
    double simulationStart =  0.0;
    double simulationEnd   =  10.0;

    SimulationEnvironment sim( simulationStart, simulationEnd, process, controller );

    if (sim.init( x0 ) != SUCCESSFUL_RETURN)
    	exit( EXIT_FAILURE );
    if (sim.run( ) != SUCCESSFUL_RETURN)
    	exit( EXIT_FAILURE );

    // ...AND PLOT THE RESULTS
    // ----------------------------------------------------------

	VariablesGrid diffStates;
	sim.getProcessDifferentialStates( diffStates );
	diffStates.print( "diffStates.txt" );
	diffStates.print( "diffStates.m","DIFFSTATES",PS_MATLAB );

	VariablesGrid interStates;
	sim.getProcessIntermediateStates( interStates );
	interStates.print( "interStates.txt" );
	interStates.print( "interStates.m","INTERSTATES",PS_MATLAB );

    VariablesGrid sampledProcessOutput;
    sim.getSampledProcessOutput( sampledProcessOutput );
    sampledProcessOutput.print( "sampledOut.txt" );
    sampledProcessOutput.print( "sampledOut.m","OUT",PS_MATLAB );

    VariablesGrid feedbackControl;
    sim.getFeedbackControl( feedbackControl );
	feedbackControl.print( "controls.txt" );
	feedbackControl.print( "controls.m","CONTROL",PS_MATLAB );

    GnuplotWindow window;
		window.addSubplot( sampledProcessOutput(0), "DIFFERENTIAL STATE: r" );
		window.addSubplot( sampledProcessOutput(1), "DIFFERENTIAL STATE: phi" );
		window.addSubplot( sampledProcessOutput(2), "DIFFERENTIAL STATE: theta" );
		window.addSubplot( sampledProcessOutput(3), "DIFFERENTIAL STATE: dr" );
		window.addSubplot( sampledProcessOutput(4), "DIFFERENTIAL STATE: dphi" );
		window.addSubplot( sampledProcessOutput(5), "DIFFERENTIAL STATE: dtheta" );
		window.addSubplot( sampledProcessOutput(7), "DIFFERENTIAL STATE: Psi" );
		window.addSubplot( sampledProcessOutput(8), "DIFFERENTIAL STATE: CL" );
		window.addSubplot( sampledProcessOutput(9), "DIFFERENTIAL STATE: W" );
	
		window.addSubplot( feedbackControl(0), "CONTROL 1 DDR0" );
		window.addSubplot( feedbackControl(1), "CONTROL 1 DPSI" );
		window.addSubplot( feedbackControl(2), "CONTROL 1 DCL" );
    window.plot( );
	
	GnuplotWindow window2;
	window2.addSubplot( interStates(1) );
	window2.plot();
	
	return 0;
}

/* >>> start tutorial code >>> */
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // ----------------------------
    DifferentialState    x("", 10, 1);    // a differential state vector with dimension 10. (vector)
    DifferentialState    y    ;    // another differential state y                   (scalar)
    Control              u("", 2, 1);    // a control input with dimension 2.              (vector)
    Parameter            p    ;    // a parameter (here a scalar).                   (scalar)

    DifferentialEquation f    ;    // the differential equation

    const double t_start =  0.0;
    const double t_end   =  1.0;


    // READ A MATRIX "A" FROM A FILE:
    // ------------------------------
    DMatrix A; A.read( "matrix_vector_ocp_A.txt" );
    DMatrix B; B.read( "matrix_vector_ocp_B.txt" );


    // READ A VECTOR "x0" FROM A FILE:
    // -------------------------------
    DVector x0; x0.read( "matrix_vector_ocp_x0.txt" );


    // DEFINE A DIFFERENTIAL EQUATION:
    // -------------------------------
    f << dot(x) == -(A*x) + B*u;                           // matrix vector notation for a linear equation
    f << dot(y) == x.transpose()*x + 2.0*u.transpose()*u;  // matrix vector notation:  x^x  = scalar product = ||x||_2^2
                                                           //                          u^u  = scalar product = ||u||_2^2


    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    OCP ocp( t_start, t_end, 20 );
    ocp.minimizeMayerTerm( y );
    ocp.subjectTo( f );

    ocp.subjectTo( AT_START, x == x0  );
    ocp.subjectTo( AT_START, y == 0.0 );


    GnuplotWindow window;
        window.addSubplot( x(0),"x0" );
        window.addSubplot( x(6),"x6" );
        window.addSubplot( u(0),"u0" );
        window.addSubplot( u(1),"u1" );


    // DEFINE AN OPTIMIZATION ALGORITHM AND SOLVE THE OCP:
    // ---------------------------------------------------
    OptimizationAlgorithm algorithm(ocp);

    algorithm.set( MAX_NUM_ITERATIONS, 20 );
    algorithm.set( KKT_TOLERANCE, 1e-10 );

    algorithm << window;
    algorithm.solve();

    return 0;
}