% r_quadlin.m
%  1 Dec 03 - quadruple tank problem -- linear plant and linear model
%             see Bequette (2003), chapter 14
%  8 Dec 06 - additional comments
% 18 Nov 09 - revised for plant equation structure
% 21 Nov 11 - more documentation for MPC 2011 course
%
%             The first simulations are for the non-minimum phase (NMP)
%             operating point, which has performance limitations
%             Run 1: Setpoint change
%             Run 2: Disturbance in input 1, using DMC (additive output)
%             Run 3: Disturbance in input 1, using a KF to estimate
%                        disturbance
%
%             Then, simulations are performed for the minimum phase (MP)
%             operating point, which is much easier to control
%             Run 4: Setpoint change
%             Run 5: Disturbance in input 1, using DMC (additive output)
%
% -------------------------------------------------------------------
% first, setpoint change only
  ysp = [1;-1];   % setpoint change (from 0 vector); dimension ny
  timesp = 20;    % time of setpoint change
  dist = [0;0];   % magnitude of input disturbance; dimension nd
  timedis = 20;   % time of input disturbance
  delt = 2;       % sample time
  tfinal = 600;
% model parameters (continuous time, state space quadruple tank)
% parameter values
  A1 = 28;    A3 = A1; % cross-sectional area
  A2 = 32;    A4 = A2; % cross-sectional area
  a1 = 0.071; a3 = a1;
  a2 = 0.057; a4 = a2;
  beta = 0.5;
  grav = 981;
% parameters for operating point 1 (minimum phase)
  k1 = 3.33;  k2 = 3.35;
  gamma1 = 0.7;  gamma2 = 0.6;
  h1s = 12.4; h2s = 12.7;
  h3s = 1.8;  h4s = 1.4;
  v1s = 3;    v2s = 3;
% operating point 2 (nonminimum phase) -- these are the first simulations
  k1 = 3.14;  k2 = 3.29;
  gamma1 = 0.43;  gamma2 = 0.34;
  h1s = 12.6; h2s = 13.0;
  h3s = 4.8;  h4s = 4.9;
  v1s = 3.15;    v2s = 3.15;
% time constants
  tau1 = (A1/a1)*sqrt(2*h1s/grav);
  tau2 = (A2/a2)*sqrt(2*h2s/grav);
  tau3 = (A3/a3)*sqrt(2*h3s/grav);
  tau4 = (A4/a4)*sqrt(2*h4s/grav);
% continuous state space model
  am = [-1/tau1,0,A3/(A1*tau3),0;0,-1/tau2,0,A4/(A2*tau4);0,0,-1/tau3,0;0,0,0,-1/tau4]
  bm1 = [gamma1*k1/A1,0;0,gamma2*k2/A2;0,(1-gamma2)*k2/A3;(1-gamma1)*k1/A4,0]
  cm = [beta,0,0,0;0,beta,0,0]
  dm1 = zeros(2,2)
  bm  = [bm1 bm1]            % indicates that the disturbance inputs have the same effect as manipulated inputs
  dm  = [dm1 dm1]
% inputs 1 and 2 are manipulated, inputs 3 and 4 are disturbances
  ninputs = size(bm,2);         % includes disturbance inputs
  noutputs = size(cm,1);
  nstates = size(am,1);         % number of model states
  sysc_mod = ss(am,bm,cm,dm);   % continuous state space model
% discretize the model
  sysd_mod = c2d(sysc_mod,delt);                             % discrete state space model
  [phi_mod,gamma_stuff,cd_mod,dd_stuff] = ssdata(sysd_mod)   % extracts discrete model matrices
%
  gamma_mod  = gamma_stuff(:,1:2);   % first 2 inputs are manipulated
  gammad_mod = gamma_stuff(:,3:4);   % third and 4th inputs are disturbances
% ------ plant parameters -------------------------------------
  planteqns = 'odequadtanklin'
  nstates_p = 4;       % number of plant states
  parvec(1) = 2;       % NMP operating point
  cplant = [beta,0,0,0;0,beta,0,0]
%
% ---------------------------------------------------------------
% controller parameters
  p = 100;      % prediction horizon
  m = 3;        % control horizon
  ny = 2;       % number of measured outputs
  nu = 2;       % number of manipulated inputs
  nd = 2;       % number of actual disturbances
  nd_est = 2;   % number of estimated disturbances
%
  weightu = [0.1 0;0 0.1]; % weighting matrix for control action
%
  weighty = [1 0;0 1];      % weighting matrix for outputs
  umin   = [-1000;-1000];
  umax   = [1000;1000];
  dumin = umin;
  dumax = umax;
  Q  = eye(nd_est,nd_est);       % state covariance
  R  = eye(ny);
%  
  isim = 1; % DMC
  iqp  = 1; % unconstrained
  noisemag = zeros(ny,1); % no noise
% ---------------------------------------------------
% ----- run the first simulation --------------------
  QSSmpcNLPlant
% zero-order hold on input and setpoint
  [tt,uu] = stairs(t,u');
  [ttr,rr] = stairs(t,r');
%          plot
  figure(10)
  subplot(2,2,1),plot(t,y(1,:),ttr,rr(:,1),':')
  title('Quad Tank - NMP, unconstrained DMC')
  ylabel('y1')
  xlabel('time')
  subplot(2,2,2),plot(t,y(2,:),ttr,rr(:,2),':')
  title('Quad Tank - NMP, unconstrained DMC')
  ylabel('y2')
  xlabel('time')
  subplot(2,2,3),plot(tt,uu(:,1))
  xlabel('time')
  ylabel('u1')
  subplot(2,2,4),plot(tt,uu(:,2))
  xlabel('time')
  ylabel('u2')
%  save vectors for plot comparison later
  tt1 = tt; ttr1 = ttr; rr1 = rr; uu1 = uu; t1 = t; y1 = y;
% ---------------- Simulation Run 2 --------------------------
% ---------------- disturbance simulation, using DMC ---------
% -- perturbation to input 1 ---------------------------------
  ysp = [0;0];      % setpoint change (from 0 vector); dimension ny
  timesp = 20;      % time of setpoint change
  dist = [1;0];     % magnitude of input disturbance; dimension nd
  timedis = 20;     % time of input disturbance
    QSSmpcNLPlant   % run simulation
    tDMC = t; yDMC = y;                  % save vectors for plotting
    uDMC = u; rDMC = r;                  % save vectors for plotting
  [ttDMC,uuDMC] = stairs(tDMC,uDMC');    % zero-order hold for plots
  [ttrDMC,rrDMC] = stairs(tDMC,rDMC');   % zero-order hold for plots
%
%  plot
  figure(12)
  subplot(2,2,1),plot(tDMC,yDMC(1,:),ttrDMC,rrDMC(:,1),':')
  title('Quad Tank - NMP w/disturb, unconstr DMC')
  ylabel('y1')
  xlabel('time')
  subplot(2,2,2),plot(tDMC,yDMC(2,:),ttrDMC,rrDMC(:,2),':')
  title('Quad Tank - NMP w/disturb, unconstr DMC')
  ylabel('y2')
  xlabel('time')
  subplot(2,2,3),plot(ttDMC,uuDMC(:,1))
  xlabel('time')
  ylabel('u1')
  subplot(2,2,4),plot(ttDMC,uuDMC(:,2))
  xlabel('time')
  ylabel('u2')
% -------------- Simulation Run 3 (still NMP) ------------------------------
% -------------- disturbance using KF to estimate disturbance --------------
  isim = 2;   % KF estimation of disturbance
    QSSmpcNLPlant                       % run simulation
    tKF = t; yKF = y;                   % save vectors for plotting
    uKF = u; rKF = r;                   % save vectors for plotting
% zero-order hold on input and setpoint
  [ttKF,uuKF] = stairs(tKF,uKF');
  [ttrKF,rrKF] = stairs(tKF,rKF');
%
%  plot
  figure(13)
  subplot(2,2,1),plot(tKF,yKF(1,:),ttrKF,rrKF(:,1),':')
  title('Quad Tank - NMP w/disturb, unconstr KF-MPC')
  ylabel('y1')
  xlabel('time')
  subplot(2,2,2),plot(tKF,yKF(2,:),ttrKF,rrKF(:,2),':')
  title('Quad Tank - NMP w/disturb, unconstr KF-MPC')
  ylabel('y2')
  xlabel('time')
  subplot(2,2,3),plot(ttKF,uuKF(:,1))
  xlabel('time')
  ylabel('u1')
  subplot(2,2,4),plot(ttKF,uuKF(:,2))
  xlabel('time')
  ylabel('u2')
% ------ compare KF (run 3) and DMC (run 2) disturbance rejection -------
% ------ still the NMP operating point ----------------------------------
  figure(14)
  subplot(2,2,1),plot(tKF,yKF(1,:),tDMC,yDMC(1,:),'--',ttrKF,rrKF(:,1),':')
  title('Quad Tank - NMP w/disturb')
  legend('KF','DMC')
  ylabel('y1')
  xlabel('time')
  subplot(2,2,2),plot(tKF,yKF(2,:),tDMC,yDMC(2,:),'--',ttrKF,rrKF(:,2),':')
  title('Quad Tank - NMP w/disturb')
  legend('KF','DMC')
  ylabel('y2')
  xlabel('time')
  subplot(2,2,3),plot(ttKF,uuKF(:,1),ttDMC,uuDMC(:,1),'--')
  xlabel('time')
  ylabel('u1')
  legend('KF','DMC')
  subplot(2,2,4),plot(ttKF,uuKF(:,2),ttDMC,uuDMC(:,2),'--')
  xlabel('time')
  ylabel('u2')
  legend('KF','DMC')
% -------------------------------------------------------------------------
% ------ Now, simulate the minimum phase (MP) operating point -------------
% ------
% parameters for operating point 1 (minimum phase)
  k1 = 3.33;  k2 = 3.35;
  gamma1 = 0.7;  gamma2 = 0.6;
  h1s = 12.4; h2s = 12.7;
  h3s = 1.8;  h4s = 1.4;
  v1s = 3;    v2s = 3;
 %
 % time constants
  tau1 = (A1/a1)*sqrt(2*h1s/grav);
  tau2 = (A2/a2)*sqrt(2*h2s/grav);
  tau3 = (A3/a3)*sqrt(2*h3s/grav);
  tau4 = (A4/a4)*sqrt(2*h4s/grav);
% continuous state space model
  am = [-1/tau1,0,A3/(A1*tau3),0;0,-1/tau2,0,A4/(A2*tau4);0,0,-1/tau3,0;0,0,0,-1/tau4]
  bm1 = [gamma1*k1/A1,0;0,gamma2*k2/A2;0,(1-gamma2)*k2/A3;(1-gamma1)*k1/A4,0]
  cm = [beta,0,0,0;0,beta,0,0]
  dm1 = zeros(2,2)
  bm  = [bm1 bm1]            % indicates that the disturbance inputs have the same effect as manipulated inputs
  dm  = [dm1 dm1]
% inputs 1 and 2 are manipulated, inputs 3 and 4 are disturbances
  ninputs = size(bm,2);         % includes disturbance inputs
  noutputs = size(cm,1);
  nstates = size(am,1);         % number of model states
  sysc_mod = ss(am,bm,cm,dm);   % continuous state space model
% discretize the model
  sysd_mod = c2d(sysc_mod,delt);                             % discrete state space model
  [phi_mod,gamma_stuff,cd_mod,dd_stuff] = ssdata(sysd_mod)   % extracts discrete model matrices
%
  gamma_mod  = gamma_stuff(:,1:2);   % first 2 inputs are manipulated
  gammad_mod = gamma_stuff(:,3:4);   % third and 4th inputs are disturbances
% ------ plant parameters -------------------------------------
  planteqns = 'odequadtanklin'
  nstates_p = 4;       % number of plant states
  parvec(1) = 1;       % MP operating point
  cplant = [beta,0,0,0;0,beta,0,0]
%
% first, setpoint change only
  ysp = [1;-1];   % setpoint change (from 0 vector); dimension ny
  timesp = 20;    % time of setpoint change
  dist = [0;0];   % magnitude of input disturbance; dimension nd
  timedis = 20;   % time of input disturbance
  delt = 2;       % sample time
  tfinal = 100;
%
% ---------------------------------------------------------------
% controller parameters
  p = 100;      % prediction horizon
  m = 3;        % control horizon
  ny = 2;       % number of measured outputs
  nu = 2;       % number of manipulated inputs
  nd = 2;       % number of actual disturbances
  nd_est = 2;   % number of estimated disturbances
%
  weightu = [0.1 0;0 0.1]; % weighting matrix for control action
%
  weighty = [1 0;0 1];      % weighting matrix for outputs
  umin   = [-1000;-1000];
  umax   = [1000;1000];
  dumin = umin;
  dumax = umax;
  Q  = eye(nd_est,nd_est);       % state covariance
  R  = eye(ny);
%  
  isim = 1; % DMC
  iqp  = 1; % unconstrained
  noisemag = zeros(ny,1); % no noise
% ---------------------------------------------------
% ----- run the fourth simulation --------------------
  QSSmpcNLPlant
% zero-order hold on input and setpoint
  [tt,uu] = stairs(t,u');
  [ttr,rr] = stairs(t,r');
%          plot
  figure(20)
  subplot(2,2,1),plot(t,y(1,:),ttr,rr(:,1),':')
  title('Quad Tank - MP, unconstrained DMC')
  ylabel('y1')
  xlabel('time')
  subplot(2,2,2),plot(t,y(2,:),ttr,rr(:,2),':')
  title('Quad Tank - MP, unconstrained DMC')
  ylabel('y2')
  xlabel('time')
  subplot(2,2,3),plot(tt,uu(:,1))
  xlabel('time')
  ylabel('u1')
  subplot(2,2,4),plot(tt,uu(:,2))
  xlabel('time')
  ylabel('u2')
%  save vectors for plot comparison later
  tt2 = tt; ttr2 = ttr; rr2 = rr; uu2 = uu; t2 = t; y2 = y;
%  comparison of NMP with MP for setpoint change
  figure(21)
  subplot(2,2,1),plot(t1,y1(1,:),t2,y2(1,:),ttr1,rr1(:,1),':')
  title('Quad Tank - NMP & MP, unconstrained DMC')
  ylabel('y1')
  xlabel('time')
  legend('NMP',' MP')
  subplot(2,2,2),plot(t1,y1(2,:),t2,y2(2,:),ttr1,rr1(:,2),':')
  title('Quad Tank - NMP & MP, unconstrained DMC')
  ylabel('y2')
  xlabel('time')
  subplot(2,2,3),plot(tt1,uu1(:,1),tt2,uu2(:,1))
  xlabel('time')
  ylabel('u1')
  subplot(2,2,4),plot(tt1,uu1(:,2),tt2,uu2(:,2))
  xlabel('time')
  ylabel('u2')
% ---------------- Simulation Run 5 --------------------------
% ---------------- disturbance simulation, using DMC ---------
% -- perturbation to input 1 ---------------------------------
  ysp = [0;0];      % setpoint change (from 0 vector); dimension ny
  timesp = 20;      % time of setpoint change
  dist = [1;0];     % magnitude of input disturbance; dimension nd
  timedis = 20;     % time of input disturbance
  tfinal = 300;
    QSSmpcNLPlant   % run simulation
    tDMC2 = t; yDMC2 = y;                  % save vectors for plotting
    uDMC2 = u; rDMC2 = r;                  % save vectors for plotting
  [ttDMC2,uuDMC2] = stairs(tDMC2,uDMC2');    % zero-order hold for plots
  [ttrDMC2,rrDMC2] = stairs(tDMC2,rDMC2');   % zero-order hold for plots
%
%  plot
  figure(22)
  subplot(2,2,1),plot(tDMC2,yDMC2(1,:),ttrDMC2,rrDMC2(:,1),':')
  title('Quad Tank - MP w/disturb, unconstr DMC')
  ylabel('y1')
  xlabel('time')
  subplot(2,2,2),plot(tDMC2,yDMC2(2,:),ttrDMC2,rrDMC2(:,2),':')
  title('Quad Tank - MP w/disturb, unconstr DMC')
  ylabel('y2')
  xlabel('time')
  subplot(2,2,3),plot(ttDMC2,uuDMC2(:,1))
  xlabel('time')
  ylabel('u1')
  subplot(2,2,4),plot(ttDMC2,uuDMC2(:,2))
  xlabel('time')
  ylabel('u2')
% ----------------------------------------------------------
% compare setpoint responses for MP and NMP
%
