% main_exercise2.m
% Solution proposal Exercise 2. Alternative to the one given in main_ex2b.m
% using m-file functions q2qt.m and ss2h.m
clear all

% System parameters
A=0.7; B=0.8; D=1;   % Model parameters
p=1; q=3;            % LQ objectiwe weighting parameters
L=4;                 % Prediction horizon 
r=1;                 % Constant future reference
N=20;                % Simulation horizon

% Make matrices, F_L and O_L using m-file functions, ss2h
[HLd,OL,OLB]=ss2h(A,B,D,0,L,0);
Q=q2qt(q,L);
P=q2qt(p,L);

% MPC gain matrix, unconstrained problem.
F_L=[OL*B HLd];
G=inv(F_L'*Q*F_L+P)*F_L'*Q;

x=0;
for k=1:N
    y=D*x;           % 1. Measurement from process
    
%%%%%%%%%%%%%%%%%%%%%% 2. Unconstrained MPC controller                    
    rf=r*ones(4,1);  % references over prediction horizon
    pk=OL*A*x;
    uf=G*(rf-pk);
    u=uf(1);            % Use 1st computed value as control action
    
%%%%%%%%%%%%%%%%%%%%%% 3, Store some variables      
    Y(k,1)=y;        
    U(k,1)=u;
    
%%%%%%%%%%%%%%%%%%%%%% 4. feed control action, u_k, to process      
    x=A*x+B*u;
end

% Plot variables
plot(Y)
    
    