%
%------- Basic system model
%
clear;
T=0.1;
Phi=[1 T;0 1];
Gam=[T^2/2;T];
C=[1 0];
n=size(Phi,1);
m=size(Gam,2);
%
%------- Parameters --------
%
q=5;
qf=10;
r=1;
N=10;
z0=[0.5;1];

%
%------- Define matrices for the QP --------
%

%
%----- For problem 2 with inequalities ---------
%
A=[];
b=[];


%
%----- Cost ------------
%


%
%------- MPC algorithm ------
%

M=100;   %time horizon

zt=z0;
yvec=[];
uvec=[];
options=optimset('largescale','off');
for flcnt1=1:M
    beq=AA*zt; % Matrisen AA definierar hur det senast mätta tillståndet zt
               % bestämmer högerledet i likhetsbivillkoret.
    x=quadprog(H,f,A,b,Aeq,beq,[],[],[],options);
    ut=x(n*N+1);
    zt=Phi*zt+Gam*ut;
    yvec=[yvec;C*zt];
    uvec=[uvec;ut];
end    
tvec=T*(1:1:M);
subplot(3,1,3)  %For the other two sets of parameters you should change the third index to 2 and 3, respectively.
plot(tvec,yvec,'-',tvec,uvec,'--')
grid


