% main_exercise6_mpc.m
% MPC control of non-linear reactor model as in Exercise 6
% Written: DDiR, November 15th 2007

clear all

% Parameters in non-linear model
k1=50; k2=100; k3=10;

% Continuous time linearized model
Ac=[-125 0;50 -125];
Bc=[7.5;-1];
Dc=[0 1];

% Discrete time model
h=0.001;
[A,B,D]=c2dm(Ac,Bc,Dc,zeros(1),h);

% Weighting matrices Q and R
Q=2.8; R=0.1;
Q=Q/R; R=1;
L=40;

%Q=100; L=10;
%Q=500; L=5;

%define constraint matrices, vectors
[S,c]=scmat(1,L);
Acal=[eye(L);-eye(L);S;-S];
Du_max=ones(L,1)*10;
Du_min=ones(L,1)*(-10);
u_max=ones(L,1)*50;
u_min=ones(L,1)*10;

% Dimensions and extended state space model in terms of deviation variables
nx=size(A,1); nu=size(B,2); ny=size(D,1);

At=[A zeros(nx,ny);D eye(ny,ny)];
Bt=[B;zeros(ny,nu)];
Dt=[D eye(ny,ny)];

% LQ optimal feedback for comparison purpose
Qt=Dt'*Q*Dt;

G=dlqr(At,Bt,Qt,R); G=-G;
G1=G(:,1:nx); G2=G(:,nx+1:nx+ny);

%%%%% Prediction model matrices
[HdL,OL,OLB]=ss2h(At,Bt,Dt,zeros(ny,nu),L,0);
F_L=[OLB HdL];
Qt=q2qt(Q,L);
Rt=q2qt(R,L);

H=F_L'*Qt*F_L + Rt;

% Steady state nominal values
xs=[2.5;1.0]; ys=D*xs;
us=25;
vs=10;

% Simulation horizon
N=1/h;

% references
Ref=zeros(N,1);
rand('seed',0);
Ref=0.95*ones(N,1)+0.05*prbs1(N,170,320);

% MPC flag: IMPC=1 (unconstrained), IMPC=2 (QP problem with constraints)
impc=2;

% Initial values for the simulation
x=xs;
u=us; u_old=u;
y_old=ys; x_old=x; 
for k=1:N-L
    y=D*x;
    
    if impc == 1
    % Compute unconstrained MPC control
       r1L=Ref(k+1:k+L);
       xt=[x-x_old;y_old];     
       y_old=y; 
       x_old=x;
    
       pl=OL*At*xt;
       f=F_L'*Qt*(pl-r1L);
       duf=-inv(H)*f;
       u=u_old+duf(1:nu); u_old=u;    
 
    elseif impc == 2
       r1L=Ref(k+1:k+L);
       xt=[x-x_old;y_old];     
       y_old=y; 
       x_old=x;
    
       pl=OL*At*xt;
       f=F_L'*Qt*(pl-r1L);
       
       bk=[Du_max;-Du_min;u_max-c*u_old;-u_min+c*u_old];
       duf=quadprog(H,f,Acal,bk);
       u=u_old+duf(1:nu); u_old=u;  
       
    end
        
        
    % Storing actual variables for plotting purpose
    X(k,:)=x';
    Y(k,1)=y;
    U(k,1)=u;
    
    % Feed control to the actual process (non-linear model)
    f1=-k1*x(1)-k3*x(1)^2+(vs-x(1))*u;
    f2= k1*x(1)-k2*x(2)-x(2)*u;
    fx=[f1;f2];
    x=x+h*fx;
end

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

