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

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

    USING_NAMESPACE_ACADO


    // DEFINE A VARIABLES GRID:
    // ------------------------
    Grid dataGrid( 0.0, 5.0, 6 );

    VariablesGrid data;
    data.init( 2, dataGrid );

    data( 0, 0 ) = 0.0;  data( 0, 1 ) = 1.0  ;
    data( 1, 0 ) = 0.2;  data( 1, 1 ) = 0.8  ;
    data( 2, 0 ) = 0.4;  data( 2, 1 ) = 0.7  ;
    data( 3, 0 ) = 0.6;  data( 3, 1 ) = 0.65 ;
    data( 4, 0 ) = 0.8;  data( 4, 1 ) = 0.625;
    data( 5, 0 ) = 1.0;  data( 5, 1 ) = 0.613;

    // CONSTRUCT A CURVE INTERPOLATING THE DATA:
    // -----------------------------------------

    Curve c1, c2;

    c1.add( data, IM_CONSTANT );
    c2.add( data, IM_LINEAR   );


    // PLOT CURVES ON GIVEN GRID:
    // --------------------------
    GnuplotWindow window;
         window.addSubplot( c1, 0.0,5.0, "Constant data Interpolation"   );
         window.addSubplot( c2, 0.0,5.0, "Linear data Interpolation"   );
    window.plot();



    return 0;
}

int main( )
{
    USING_NAMESPACE_ACADO

    // DEFINE THE VARIABLES:
    // ----------------------------------------------------------
    DifferentialState   p    ;  // the trolley position
    DifferentialState   v    ;  // the trolley velocity
    DifferentialState   phi  ;  // the excitation angle
    DifferentialState   omega;  // the angular velocity
    Control             a    ;  // the acc. of the trolley

    const double     g = 9.81;  // the gravitational constant
    const double     b = 0.20;  // the friction coefficient
    // ----------------------------------------------------------


    // DEFINE THE MODEL EQUATIONS:
    // ----------------------------------------------------------
    DifferentialEquation f;

    f << dot( p     )  ==  v                                ;
    f << dot( v     )  ==  a                                ;
    f << dot( phi   )  ==  omega                            ;
    f << dot( omega )  == -g*sin(phi) - a*cos(phi) - b*omega;
    // ----------------------------------------------------------


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


    // SETTING UP THE MPC CONTROLLER:
    // ------------------------------
    ExportedRTIscheme rtiScheme( 4,1, 10, 0.3 );
#ifdef USE_CVXGEN
    set_defaults( );
#endif

    Vector xuRef(5);
    xuRef.setZero( );

    VariablesGrid reference;
    reference.addVector( xuRef,  0.0 );
    reference.addVector( xuRef, 10.0 );

    StaticReferenceTrajectory referenceTrajectory( reference );

    Controller controller( rtiScheme,referenceTrajectory );
    controller.set( USE_REFERENCE_PREDICTION,NO );


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

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

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


    // ... AND PLOT THE RESULTS
    // ------------------------
    VariablesGrid diffStates;
    sim.getProcessDifferentialStates( diffStates );

    VariablesGrid feedbackControl;
    sim.getFeedbackControl( feedbackControl );

    GnuplotWindow window;
    window.addSubplot( diffStates(0), "p" );
    window.addSubplot( diffStates(1), "v" );
    window.addSubplot( diffStates(2), "phi" );
    window.addSubplot( diffStates(3), "omega" );
    window.addSubplot( feedbackControl(0), "a" );
    window.plot( );

    return 0;
}

int main( ){

    USING_NAMESPACE_ACADO


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

    DifferentialState   x, y;

    DifferentialEquation f;

    f << dot(x) ==  y;
    f << dot(y) == -x;


    // Define an integrator:
    // ---------------------

    IntegratorRK45 integrator( f );
    integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
	integrator.set( PRINT_INTEGRATOR_PROFILE, YES );
	
    // Define an initial value:
    // ------------------------
    double x_start[2] = { 0.0, 1.0 };
    Grid timeInterval( 0.0, 2.0*M_PI, 100 );

    integrator.freezeAll();
    integrator.integrate( timeInterval, x_start );


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

    VariablesGrid differentialStates;
    integrator.getX( differentialStates );

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

    window.plot();


//     Vector seed(2);
// 
//     seed( 0 ) = 1.0;
//     seed( 1 ) = 0.0;
// 
//     integrator.setForwardSeed( 1, seed );
//     integrator.integrateSensitivities();
// 
//     VariablesGrid sens;
//     integrator.getForwardSensitivities( sens, 1 );
// 
//     GnuplotWindow window2;
//         window2.addSubplot( sens(0) );
//         window2.addSubplot( sens(1) );
//     window2.plot();


    return 0;
}

int main( ){


    // Define a Right-Hand-Side:
    // -------------------------
    DifferentialState x("", 4, 1), P("", 4, 4);
    Control           u("", 2, 1);

    IntermediateState rhs = cstrModel( x, u );

    DMatrix Q = zeros<double>(4,4);

    Q(0,0) = 0.2;
    Q(1,1) = 1.0;
    Q(2,2) = 0.5;
    Q(3,3) = 0.2;


    DMatrix R = zeros<double>(2,2);

    R(0,0) = 0.5;
    R(1,1) = 5e-7;

    DifferentialEquation f;
    f << dot(x) == rhs;
    f << dot(P) == getRiccatiODE( rhs, x, u, P, Q, R );



    // Define an integrator:
    // ---------------------

    IntegratorRK45 integrator( f );
    integrator.set( INTEGRATOR_PRINTLEVEL, MEDIUM );
	integrator.set( PRINT_INTEGRATOR_PROFILE, YES );
	
    // Define an initial value:
    // ------------------------
    //double x_ss[4] = { 2.14, 1.09, 114.2, 112.9 };
	double x_start[20] = { 1.0, 0.5, 100.0, 100.0, 1.0, 0.0, 0.0, 0.0,
                                                   0.0, 1.0, 0.0, 0.0,
                                                   0.0, 0.0, 1.0, 0.0,
                                                   0.0, 0.0, 0.0, 1.0 };

	double u_start[2] = { 14.19, -1113.5 };
//	double u_start[2] = { 10.00, -7000.0 };

	Grid timeInterval( 0.0, 5000.0, 100 );

    integrator.freezeAll();
    integrator.integrate( timeInterval, x_start, 0 ,0, u_start );


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

    VariablesGrid differentialStates;
    integrator.getX( differentialStates );

	DVector PP = differentialStates.getLastVector();
	DMatrix PPP(4,4);
	for( int i=0; i<4; ++i )
		for( int j=0; j<4; ++j )
			PPP(i,j) = PP(4+i*4+j);
	PPP.print( "P1.txt","",PS_PLAIN );
//	PPP.printToFile( "P2.txt","",PS_PLAIN );

    GnuplotWindow window;
        window.addSubplot( differentialStates(0), "cA [mol/l]" );
        window.addSubplot( differentialStates(1), "cB [mol/l]" );
        window.addSubplot( differentialStates(2), "theta [C]" );
        window.addSubplot( differentialStates(3), "thetaK [C]" );

        window.addSubplot( differentialStates(4 ), "P11" );
        window.addSubplot( differentialStates(9 ), "P22" );
        window.addSubplot( differentialStates(14), "P33" );
        window.addSubplot( differentialStates(19), "P44" );
		
    window.plot();

    return 0;
}

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


    USING_NAMESPACE_ACADO

    // DEFINE VALRIABLES:
    // ---------------------------
    DifferentialStateVector       x(3);  // the position of the pendulum  (x,y,alpha)
    DifferentialStateVector       v(3);  // the associated velocities
    AlgebraicStateVector          a(3);  // the associated accelerations
    AlgebraicStateVector     lambda(2);  // the constraint forces


    const double L = 1.00;               // the length of the pendulum
    const double m = 1.00;               // the mass of the pendulum
    const double g = 9.81;               // the gravitational constant

    const double J = m*L*L;              // the inertial of the pendulum

    IntermediateStateVector R(3);
    IntermediateStateVector G(2);

    R.setComponent( 0, m*a(0)       );       // ----------------------------------------
    R.setComponent( 1, m*a(1) + m*g );       // the definition of the force residuum:
    R.setComponent( 2, J*a(2)       );       //       R := m*a - F

    G.setComponent( 0, x(0)-L*sin(x(2)) );   // definition of the constraint manifold G
    G.setComponent( 1, x(1)+L*cos(x(2)) );   // ---------------------------------------



    // AUTOMATIC GENERATION OF AN INDEX 1 DAE SYSTEM BASES ON THE
    // NEWTON EULER FORMALISM:
    // -----------------------------------------------------------
    DifferentialEquation  f;
    NewtonEulerFormalism( f, R, G, x, v, a, lambda );


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

    // Define an initial value:
    // ------------------------
    double x_start[6];
    double z_start[5];

    x_start[0] =  1.9866932270683099e-01;
    x_start[1] = -9.8006654611577315e-01;
    x_start[2] =  2.0000003107582773e-01;
    x_start[3] = -1.4519963562050693e-04;
    x_start[4] =  4.7104175041346282e-04;
    x_start[5] =  4.4177521668741377e-04;

    z_start[0] = -9.5504866367984165e-01;
    z_start[1] = -1.9359778029074531e-01;
    z_start[2] = -9.7447321693831934e-01;
    z_start[3] = -9.5504866367984165e-01;
    z_start[4] =  9.6164022197092560e+00;


    double t_start    =   0.0;
    double t_end      =  10.0;

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

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


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

    GnuplotWindow window;
        window.addSubplot( xres(0), "The x-position of the mass m" );
        window.addSubplot( xres(1), "The y-position of the mass m" );
        window.addSubplot( xres(2), "The excitation angle of the pendulum" );
        window.addSubplot( xres(3), "The velocity in x-direction" );
        window.addSubplot( xres(4), "The velocity in y-direction" );
        window.addSubplot( xres(5), "The angular velocity" );
//         window.addSubplot( zres(0), "The acceleration in x-direction" );
//         window.addSubplot( zres(1), "The acceleration in y-direction" );
//         window.addSubplot( zres(2), "The angular acceleration" );
        window.addSubplot( zres(3), "The constraint force in x-direction" );
        window.addSubplot( zres(4), "The constraint force in y-direction" );
    window.plot();


    return 0;
}

//.........这里部分代码省略.........
    OCP ocp        ( t_start, t_end, 14 );
    ocp.minimizeLSQ( Q, h, r );
    ocp.subjectTo  ( f );

    ocp.subjectTo( -1.0 <= u <= 2.0 );
    //ocp.subjectTo(  w == 0.0 );


    // SETTING UP THE (SIMULATED) PROCESS:
    // -----------------------------------
    OutputFcn identity;
    DynamicSystem dynamicSystem( f2,identity );
    Process process( dynamicSystem,INT_RK45 );


	VariablesGrid disturbance = fopen( "my_disturbance.txt", "r" );

// 	GnuplotWindow window2;
// 		window2.addSubplot( disturbance, "my disturbance"   );
// 	window2.plot();

	process.setProcessDisturbance( disturbance );



    // SETUP OF THE ALGORITHM AND THE TUNING OPTIONS:
    // ----------------------------------------------
	double samplingTime = 0.5;
    RealTimeAlgorithm  algorithm( ocp,samplingTime );

// //  algorithm.set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE );
     algorithm.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
// 
// //     algorithm.set( ABSOLUTE_TOLERANCE  , 1e-7 );
// //     algorithm.set( INTEGRATOR_TOLERANCE, 1e-9 );
// 
//     algorithm.set( KKT_TOLERANCE, 1e-4 );

	algorithm.set( MAX_NUM_ITERATIONS,1 );
	algorithm.set( USE_REALTIME_SHIFTS, YES );
// 	algorithm.set( USE_REALTIME_ITERATIONS,NO );
// 	algorithm.set( TERMINATE_AT_CONVERGENCE,YES );

// 	algorithm.set( PRINTLEVEL,HIGH );


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

// // 	algorithm.solve( x0 );
// 
//     GnuplotWindow window1;
//         window1.addSubplot( x, "DIFFERENTIAL STATE: x" );
//         window1.addSubplot( u, "CONTROL: u" );
//     window1.plot();
// 
// 	return 0;


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

    VariablesGrid myReference = fopen( "my_reference.txt", "r" );
    PeriodicReferenceTrajectory reference( myReference );

// 	GnuplotWindow window3;
// 		window3.addSubplot( myReference(1), "my reference"   );
// 	window3.plot();
	
    Controller controller( algorithm,reference );



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

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

    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), "DIFFERENTIAL STATE: x" );
        window.addSubplot( feedbackControl(0),      "CONTROL: u" );
    window.plot();


    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;
}

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;
}

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;
}

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

    // 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;
}

//.........这里部分代码省略.........
						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;
}

//.........这里部分代码省略.........
    // 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;
}

//.........这里部分代码省略.........
    // 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;


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

	Process process( dynamicSystem,INT_RK45 );

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

    // DEFINE AN OPTIMAL CONTROL PROBLEM:
    // ----------------------------------
    Function h;

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

    Matrix Q = zeros(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;

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


    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 MPC CONTROLLER:
    // ------------------------------
	RealTimeAlgorithm alg( ocp,0.05 );
	alg.set( INTEGRATOR_TYPE, INT_RK78 );
	alg.set( DYNAMIC_SENSITIVITY,FORWARD_SENSITIVITY );
// 	alg.set( MAX_NUM_ITERATIONS, 2 );
//  	alg.set( USE_IMMEDIATE_FEEDBACK,YES );

	StaticReferenceTrajectory zeroReference;

	Controller controller( alg,zeroReference );


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

	Vector x0(4);
	x0.setZero();

	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 );

 	VariablesGrid feedbackControl;
 	sim.getFeedbackControl( feedbackControl );

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

    return EXIT_SUCCESS;
}