% MPC course
% Solution proposal for Exercise 1 step b
%
a=0.7; b=0.8; x0=0;

q=1; p=1;

N=50;
r=ones(N+1,1);

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

g=-q*b/(p+q*b^2);

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