% main_exercise7_mpc.m
% m-file script to demonstrate MPC of MIMO system.

A=[1.5 1.0 0.10;
  -0.7 0.0 0.10
   0.0 0.0 0.85];
B=[0 0;
   0 1
   1 0];
D=[3 0 -0.6
   0 1    1];

Q=0.03*eye(2); R=eye(2);

%%%%%
nx=size(A,1); nu=size(B,2); ny=size(D,1);

%%%%% Prediction model matrices
L=10;
[HdL,OL,OLB]=ss2h(A,B,D,zeros(ny,nu),L,0);
F_L=[OLB HdL];
[S,c] = scmat(nu,L);
F_Ld=F_L*S;
Qt=q2qt(Q,L);
Rt=q2qt(R,L);

H=F_Ld'*Qt*F_Ld + Rt;

% references
r=[1;0.5];
N=250;                                      % Simulation horizon.
rand('seed',0), randn('seed',0)
ref=[0.9*ones(N,1)+0.1*prbs1(N,30,70) ...
     0.4*ones(N,1)+0.1*prbs1(N,30,70)];   % Reference r_k.


Hd=D*inv(eye(3)-A)*B;
us=inv(Hd)*r
xs=inv(eye(3)-A)*B*us;

x=xs;
u=us; 
u_old=u;
for k=1:N-L
    y=D*x;
        
    %%%% Make the extended reference vector, r_(k,L)
    rf =ref(k+1:k+L,:); 
    r1L=rf(1,:)';
    for i=2:L
       r1L=[r1L;rf(i,:)'];
    end
    
    % Compute unconstrained MPC control
    pl=OL*A*x;
    pld=pl+F_L*c*u_old;
    f=F_Ld'*Qt*(pld-r1L);
    duf=-inv(H)*f;
    u=u+duf(1:nu); u_old=u;
    
    % Sore variables
    Y(k,:)=y'; U(k,:)=u';
    
    % Feed control to the process
    x=A*x+B*u;
end

%% Plot results
t=1:N-L; 
figure(1)
subplot(211), plot(t,[Y(:,1) ref(1:N-L,1)]), grid, ylabel('y1_k and r1_k')
title('Output y_k and reference r_k')
subplot(212), plot(t,[Y(:,2) ref(1:N-L,2)]), grid, ylabel('y2_k and r2_k')
xlabel('Discrete time')

figure(2)
subplot(211), plot(t,U(:,1)), grid, ylabel('u1_k')
title('Actual control input, u_k')
subplot(212), plot(t,U(:,2)), grid, ylabel('u2_k')
xlabel('Discrete time')