% MPC course
% Solution proposal for Exercise 2 step b
% modified to only compute u_k (Reducing the number of unknown inputs).
%
a=0.7; b=0.8; d=1; x0=0;

q=3; p=1;
% Exercise 2a
% Solution proposal for simple scalar system controlled by a MPC controller
% with varying horizon, L=1, L=4

N=50;
r=ones(N+1,1); r(26:50)=ones(25,1)*0.5;

Y=zeros(N,1); U=zeros(N,1);

hl=4; 
iu=1;   % Gives standard MPC solution
iu=2; % Gives reduced MPC solution, only computing actual, u_k.

if hl==1
    g=-q*b/(p+q*b^2);
elseif hl==4
    Q=diag([q q q q]); P=diag([p p p p]);
    O4=[d;d*a;d*a^2;d*a^3];
    H4=[0 0 0;d*b,0,0;d*a*b,d*b,0;d*a^2*b,d*a*b,d*b];
    F4=[O4*b H4]
    F4s=[d*b;d*b+d*a*b;d*b+d*a*b+d*a^2*b;d*b+d*a*b+d*a^2*b+d*a^3*b]
    G=-inv(F4'*Q*F4+P)*F4'*Q;
    Gs=-inv(F4s'*Q*F4s+4*p)*F4s'*Q
    O4A=O4*a;
end

x=x0;
for k=1:N-3
    y=x;                    % Process measurements
    
    if hl==1
        u=g*(a*y-r(k+1))        % Computing control input
    elseif hl==4
        if iu==1
           u=G(1,:)*(O4A*x-r(k+1:k+4));
        elseif iu==2
           u=Gs*(O4A*x-r(k+1:k+4));
        end
    end
    
%---------------------------  Storing variables
    U(k)=u;
    Y(k)=y; 
%---------------------------     
    x=a*x+b*u;              % Update the state equation
end
%Y(N)=y;
    
t=1:N;
subplot(211), plot(t,U,'-',t,U,'*'), grid
title('Simulation of Exercise 2')
ylabel('u_k^\ast')
subplot(212), plot(t,[r(1:N,1) Y],'-',t,[r(1:N,1) Y],'*'), grid
ylabel('r_k and y_k')
xlabel('Discrete time   0 \leq k \leq N')