% Solution proposal, Exercise 3, using functions: scmat, ss2h, etc.

% Model parameters
a=0.7; b=0.8; d=1;

% MPC parameters
L=4;  % Prediction horizon
r=1;  % Reference
Ri=1; % Weight for rate of change variables
q=1 % Weight for deviations, y_k+i-r_k+i

N=50; % Simulation horizon

% Define some matrices in MPC problem
[S,c]=scmat(1,L);
[HLd,OL,OLB]=ss2h(a,b,d,0,L,0);
Q=q2qt(q,L);
R=q2qt(Ri,L);

% Compute constant matrix in MPC unconstrained solution, Eq. (2.124)
% in lecture notes.
FL=[OLB HLd]; 
FLd=FL*S;

G=inv(FLd'*Q*FLd+R)*FLd'*Q;

x=0; % Initial state
u_old=0; % Initialize, u_k-1=u_old
u=0;
for k=1:N
    % take measurement
    y=d*x;
           
    % Compute MPC controls
    rf=ones(L,1)*r;
    pl=OL*a*x;
    pld=pl+FL*c*u_old;
    
    duf = G*(rf-pld);        % Compute, Delta u_(k | L)
    du  = duf(1);            % MPC optimal control deviation
    u = u + du;              % Actual MPC control
    u_old = u;               % Save u_k for use in u_(k-1)
    
    % save variables for plotting purposes
    Y(k,1)=y; U(k,1)=u;
    
    % Feed control to process
    x = a*x+b*u;
end

figure(1)
subplot(211), plot(U)
subplot(212), plot([ones(N,1)*r Y])