VanDerPol.m

Dateiinhalt

clc;
clear all;
close all;

EXPORT = 1;
COMPILE = 1;

T = 10;
N = 20;
Ts = T/N;

DifferentialState x1 x2;
Control u;

%% Differential Equation
f = [   dot(x1) == (1-x2^2)*x1 - x2 + u; ...
        dot(x2) == x1 ];

%% SIMexport
acadoSet('problemname', 'sim');

sim = acado.SIMexport( Ts );
sim.setModel(f);
sim.set( 'INTEGRATOR_TYPE',             'INT_IRK_GL4'   );
sim.set( 'NUM_INTEGRATOR_STEPS',        2               );

if EXPORT
    sim.exportCode('export_SIM');
end
if COMPILE 
    cd export_SIM
    make_acado_integrator('../integrate_vdp')
    cd ..
end

%% MPCexport
acadoSet('problemname', 'mpc');

ocp = acado.OCP( 0.0, T, N );

ocp.minimizeLSQ( eye(3), [x1; x2; u] );
ocp.minimizeLSQEndTerm( eye(2), [x1; x2] );

ocp.subjectTo( -1 <= u <= 1 );
ocp.subjectTo( 'AT_END', x1 == 0 );
ocp.subjectTo( 'AT_END', x2 == 0 );
ocp.setModel(f);

mpc = acado.OCPexport( ocp );
mpc.set( 'HESSIAN_APPROXIMATION',       'GAUSS_NEWTON'      );
mpc.set( 'DISCRETIZATION_TYPE',         'MULTIPLE_SHOOTING' );
mpc.set( 'SPARSE_QP_SOLUTION',          'FULL_CONDENSING_N2');
mpc.set( 'INTEGRATOR_TYPE',             'INT_RK4'           );
mpc.set( 'NUM_INTEGRATOR_STEPS',        N                   );
mpc.set( 'QP_SOLVER',                   'QP_QPOASES'    	);

if EXPORT
    mpc.exportCode( 'export_MPC' );
end
if COMPILE
    cd export_MPC
    make_acado_solver('../acado_MPCstep')
    cd ..
end

%% PARAMETERS SIMULATION 
Te = 10; Tf = 20;
X0 = [0 1];
input.x = zeros(N+1,2);
input.u = zeros(N,1);

input.y = zeros(N,3);
input.yN = zeros(2,1);

%% SIMULATION LOOP
display('------------------------------------------------------------------')
display('               Simulation Loop'                                    )
display('------------------------------------------------------------------')

iter = 0; 
time = 0; 
controls_MPC = [];  
state_sim = X0;

while time(end) < Tf
    
    % Solve NMPC OCP
    input.x0 = state_sim(end,:).';
    output = acado_MPCstep(input);
    
    % Save the MPC step
    controls_MPC = [controls_MPC; output.u(1,:)];
    
    % Shift the trajectories:
    input.x = [output.x(2:end,:); output.x(end,:)];
    input.u = [output.u(2:end,:); output.u(end,:)];
    
    % Simulate system
    sim_input.x = state_sim(end,:).';
    sim_input.u = output.u(1,:).';
    states = integrate_vdp(sim_input);
    state_sim = [state_sim; states.value'];
    
    % Time step
    iter = iter+1;
    nextTime = iter*Ts; 
    disp(['current time: ' num2str(nextTime) '   ' char(9) ' (RTI step: ' num2str(round(output.info.cpuTime*1e6)) ' µs)'])
    time = [time nextTime];
    
    visualize; pause
end