% MPC course
% Solution proposal for Exercise 1, Task 2 step b
% This MPC controller gives integral action
a=0.7; b=0.8; x0=0;

q=3; p=1;

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

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

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

x=x0; u_old=0; y_old=x;     % u_old=u_(k-1) and y_old=y_(k-1)
for k=1:N
    y=x;                    % Process measurements
    
%--------------------------    
    dy=y-y_old; y_old=y;
    p1=y+a*dy;
    du=g*(p1-r(k+1))        % Computing MPC rate of change, i.e. du=u_k-u_(k-1)
    
    u = u_old+du;
    u_old=u;
%-------------------------- % 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')