% main_exercise8_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);
%A=[A zeros(2,1);D 0]; B=[B;0]; D=[0 0 1];

% Weighting matrices
Q=2.8; R=0.1;
L=40;

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

% 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)];

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=xs(2);
%%ys=1;
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);

% Declare some errays to be used
nt=10;
Delay=ones(nt+1,1); eed=Delay;   % arrays to make time delay, hold (nt) samples

% Initial values for the simulation
x=xs
%x=[xs;1];
%x=xs;
u=us; u_old=u;
y_old=ys; x_old=x;
ym=ys; y=ys;
x
for k=1:N-L
    ym=D*x;
    %y=ym;
    for id=nt+1:-1:2;
        Delay(id)=Delay(id-1);
    end
    Delay(1)=ym;
    y=Delay(nt+1);
    Ym(k,1)=ym;
    
    %y=ym;
    % 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;    
 
    % Storing actual variables for plotting purpose
    X(k,:)=x';
    Y(k,1)=y;
    U(k,1)=u;
    
    % Feed control to the actual process
    f1=-k1*x(1)-k3*x(1)^2+(vs-x(1))*u;
    f2= k1*x(1)-k2*x(2)-x(2)*u;
    f=[f1;f2];
    x(1:2)=x(1:2)+h*f;
    %x(3)=D(1:2)*x(1:2);
    % programming of time delay for process model, 
    % hold (y) in array of length (nt) samples
    %for id=nt:-1:2;
    %    Delay(id)=Delay(id-1);
    %end
    %Delay(1)=y;
    %ym=Delay(nt);                  % the delayed process output Y=y(i-nt)
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')

