Simulate aircraft MPC system

% nbr of time steps
nbr_steps = 100;

% create reference trajectory
r_y1 = 0.25;
r_y2 = 10;
g1 = repmat(MPC.Q*[0;-r_y2;0;-r_y2],10,1);
g1 = [g1;zeros(20,1)];
g2 = zeros(60,1);

% time instance when reference is changed
change_ref = 50;

% initial condition
X0 = zeros(4,1);

% nbr of iters and execution time
nbr_iters = [];
exec_time = [];

% store state, output, and input data
X = X0;
Y = [X0(2);X0(4)];
U = [];

% simulate system
for kk = 1:nbr_steps

    % choose reference
    if kk <= change_ref
        g = g1;
    else
        g = g2;
    end

    % solve MPC optimization problem
    tic;
    [y,iter] = qp_mex(g,X0);
    tt = toc;

    % store nbr of iterations
    nbr_iters = [nbr_iters;iter];
    exec_time = [exec_time;tt];

    % extract control
    u0 = y(41:42);

    % simulate system
    X0 = MPC.Adyn*X0+MPC.Bdyn*u0;

    % store closed loop trajectory data
    X = [X X0];
    Y = [Y [X0(2);X0(4)]];
    U = [U u0];

end

% average number of iterations
fprintf('Average number of iterations: %g\n',mean(nbr_iters));
% max number of iterations
fprintf('Max number of iterations: %d\n',max(nbr_iters));
% average number of iterations
fprintf('Average execution time (ms): %g\n',1e3*mean(exec_time));
% max number of iterations
fprintf('Max execution time (ms): %g\n',1e3*max(exec_time));

% bounds
u_min = -25*ones(2,1);
u_max =  25*ones(2,1);
y_min = [-0.5;-100];
y_max = [0.5;100];


% plot closed loop trajectory
figure;
subplot(2,2,1)
stairs([0:nbr_steps]*5e-3,Y(1,1:end))
hold on;
title('y1')
plot([0 nbr_steps]*5e-3,[y_min(1) y_min(1)],'k--')
plot([0 nbr_steps]*5e-3,[y_max(1) y_max(1)],'k--')
axis([0 0.5 -1 1]);
xlabel('time (ms)')
ylabel('angle of attack')

subplot(2,2,3)
stairs([0:nbr_steps]*5e-3,[r_y2*ones(change_ref+1,1);0*ones(nbr_steps-change_ref,1)],'r-.');
hold on;
stairs([0:nbr_steps]*5e-3,Y(2,1:end))
title('y2')
axis([0 0.5 -0.5 10.5]);
xlabel('time (ms)')
ylabel('pitch angle')


subplot(2,2,2)
stairs([0:100]*5e-3,[U(1,:) U(1,end)])
hold on;
title('u1')
stairs([0 100]*5e-3,[u_min(1) u_min(1)],'k--')
stairs([0 100]*5e-3,[u_max(1) u_max(1)],'k--')
axis([0 0.5 -40 40]);
xlabel('time (ms)')
ylabel('elevator angle')

subplot(2,2,4)
stairs([0:100]*5e-3,[U(2,:) U(2,end)])
hold on;
title('u2')
stairs([0 100]*5e-3,[u_min(2) u_min(2)],'k--')
stairs([0 100]*5e-3,[u_max(2) u_max(2)],'k--')
axis([0 0.5 -40 40]);
xlabel('time (ms)')
ylabel('flaperon angle')

Average number of iterations: 36.1
Max number of iterations: 110
Average execution time (ms): 0.09843
Max execution time (ms): 0.266