% main_ex46.m
% Matrix QP implementation of example 4.6.
% DDIR, 15.03.2001
clear all

A=1; B=1; D=1; q=1; p=1;         % process model matrices and weighting parameters.

F=[D*B 0;D*A*B D*B];             % EMPC matrices.
O_L=[D;D*A];
Q=eye(2)*q; P=eye(2)*p;
H=F'*Q*F+P;

Ac=[1,0;-1,0]; bc=[0.2;0];       % Describing the inequality constraint, u_k <= 0.2

t=0:1:10; N=length(t);           % Time intervall.
x=0.5;                           % Initial value for the state.
for i=1:N
   if i < 4                      % Specified reference.
      r=[0.5;0.5];
   elseif i>=4; 
      r=[1;1]; 
   end
   y=x;                          % Process measurement.
   
   p=O_L*A*x;
   f=F'*Q*(p-r);
   uf=quadprog(H,f,Ac,bc);       % Equivalent unconstrained controls, uf=-inv(H)*f; 
   u=uf(1);                      % Only 1st variable is used for control.
   U(i,:)=uf'; Y(i,1)=y;         % Store y and u.
   x=x+u;                        % Update state equation (put u to the process).
end
subplot(211), plot(t,U(:,1),'-',t,U(:,1),'bo'),grid, ylabel('u_k'),axis([0,10,-0.1,0.4])
title('Simulation of the MPC system in Example 4.6')
subplot(212), plot(t,Y,'-',t,Y,'bo'), grid, ylabel('y_k'), axis([0,10,0.4,1.1])
xlabel('Discrete time')
