% r_VDV.m
% driving file for MPC simulations using the VDV reactor
% requires the following files:  
%       QSSmpcNLPlant.m    main simulation file
%       qSSmpccalc.m       calculates the control move (solves
%                          either the unconsrained or constrained problem)
%       KmatQSS.m          generates matrices
%       linVDVode.m        linear continuous plant odes
% 22 July 2011 -- presentation at PASI Workshop
% 20 November 2011 -- revised with more comments for clarity and use
%                     in Fall 2011 MPC course
% linear van de vuuse reactor model (Module 5 from Bequette, 2003)
% run 1: p = 10, m = 3, wu = 0, unconstrained
% run 2: p = 25, m = 1, wu = 0, unconstrained
% run 3: p =  8, m = 1, wu = 0, unconstrained
% run 4: p =  7, m = 1, wu = 0, unconstrained (unstable)
% run 5: p =  7, m = 1, wu = 0, constrained (unstable, but attains a
%                               steady-state at the input constraint)
% ----------------------------------------------------------------
% simulation parameters
% first, setpoint change only
  ysp = [1];      % setpoint change (from 0 vector); dimension ny
  timesp = 1;   % time of setpoint change
  dist = [0];   % magnitude of input disturbance; dimension nd
  timedis = 6;  % time of input disturbance
  delt = 0.1;   % sample time
  tfinal = 6;   % final simulation time
% model parameters (continuous time, state space Van de Vusse)
  am = [-2.4048 0;0.8333 -2.2381];
  bm = [7 7; -1.117 -1.117]; % 1 manip input, 1 dist
  cm = [0 1];
  dm = [0 0];
  ninputs = size(bm,2);       % includes disturbance input
  noutputs = size(cm,1);
  nstates = size(am,1);       % number of model states
  sysc_mod = ss(am,bm,cm,dm); % continuous model object
%
% discretize the model
  sysd_mod = c2d(sysc_mod,delt);  % forms discrete state space model
%
%   the next function extracts the discrete state space model matrices
  [phi_mod,gamma_stuff,cd_mod,dd_stuff] = ssdata(sysd_mod)
%
  gamma_mod  = gamma_stuff(:,1);   % first input is manipulated
  gammad_mod = gamma_stuff(:,2);   % second input is disturbance
% ----------------------------------------------------------------------
% ------------ plant parameters ----------------------------------------
%              the plant equations are in the linVDVode.m function file
%
  planteqns = 'linVDVode'
  cplant = [0 1];   % state 2 is the output
  nstates_p = 2;    % two plant states
  parvec = []; % parameters are embedded in the ODE file
% -----------------------------------------------------------------------
% controller parameters (p = 10, m = 3 for first simulation run)
  p = 10;       % prediction horizon
  m = 3;        % control horizon
  ny = 1;       % number of measured outputs
  nu = 1;       % number of manipulated inputs
  nd = 1;       % number of actual disturbances
  nd_est = 1;   % number of estimated disturbances
  weightu = [0]; % weighting matrix for control action
  weighty = [1];      % weighting matrix for outputs
% constraints
  umin = [-1000];  % minimum input constraint (effectively unconstrained)
  umax = [1000];   % maximum input constraint (effectively unconstrained)
  dumin = [-1000]; % mimimum input velocity constraint
  dumax = [1000];  % maximum input velocity constraint
% Kalman Filter matrices
  Q  = eye(nd_est,nd_est);     % state covariance - need to generalize!
  R  = eye(ny);
%  
  isim = 1;               % additive output disturbance (DMC) assumption
  iqp = 1;                % unconstrained solution
  noisemag = zeros(ny,1); % no noise
% -----------------------------------------------------------------------
% ------- all model, plant and simulation parameters have now been set --
% ------- run simulation for unconstrained, p = 10, m = 3, no input weight
  QSSmpcNLPlant
% ------- simulation done, setup plots ----------------------------------
% zero-order hold on input and setpoint (for plotting)
  [tt,uu] = stairs(t,u');
  [ttr,rr] = stairs(t,r');
%
%  plot the actual plant output without measurement noise
  figure(101)
  subplot(2,1,1)
  plot(ttr,rr,':',t,y)
  ylabel('y')
  xlabel('time')
  title('DMC: P = 10, M = 3')
  subplot(2,1,2)
  plot(tt,uu)
  ylabel('u')
  xlabel('time')
%       rename vectors for plot comparisons with other simulations
  t1 = t; y1 = y; tt1 = tt; uu1 = uu; ttr1 = ttr; rr1 = rr;
% ----------------------------------------------------------------
% ---------------- unconstrained simulations ---------------------
% ---------------- run 2, p = 25, m = 1
  isim = 1; % DMC
  iqp  = 1 % unconstrained
  p = 25;
  m = 1;

  % run simulation
  QSSmpcNLPlant
% zero-order hold on input and setpoint
  [tt,uu] = stairs(t,u');
  [ttr,rr] = stairs(t,r');
  figure(102)
  subplot(2,1,1)
  plot(t,y,ttr,rr,':')
  ylabel('y')
  xlabel('time')
  title('DMC: P = 25, M = 1')
  subplot(2,1,2)
  plot(tt,uu)
  ylabel('u')
  xlabel('time')
%     rename vectors for comparison plots that follow
  t2 = t; y2 = y; tt2 = tt; uu2 = uu; ttr2 = ttr; rr2 = rr;
%-------------------------------------------------------------
% -----compare run 1 (p = 10, m =3) with run 2 (p = 25, m = 1)
  figure(103)
  subplot(2,1,1)
  plot(t1,y1,t2,y2)
  legend('P = 10, M = 3','P = 25, M = 1')
  ylabel('y')
  xlabel('time')
  title('DMC: P = 25, M = 1 and P = 10, M = 3')
  subplot(2,1,2)
  plot(tt1,uu1,tt2,uu2)
  ylabel('u')
  xlabel('time')
%------------------------------------------------
% run 3, p = 8, m = 1
  isim = 1; % DMC
  iqp  = 1 % unconstrained
  p = 20;
  m = 1;
  % simulation run 3 (p = 20, m = 1)
  QSSmpcNLPlant
% zero-order hold on input and setpoint
  [tt,uu] = stairs(t,u');
  [ttr,rr] = stairs(t,r');
  figure(104)
  subplot(2,1,1)
  plot(t,y,ttr,rr,':')
  ylabel('y')
  xlabel('time')
  title('DMC: P = 8, M = 1')
  subplot(2,1,2)
  plot(tt,uu)
  ylabel('u')
  xlabel('time')
  t3 = t; y3 = y; tt3 = tt; uu3 = uu; ttr3 = ttr; rr3 = rr;
% ----------------------------------------------------------------
% ---- simulation run 4, p = 20, m = 5 --------------------------
  p = 7;    % unstable prediction and control horizons
  m = 1;
  % run simulation
  iqp = 1;  % constrained
  umin = -1000; 
  tfinal = 6;
  QSSmpcNLPlant
% zero-order hold on input and setpoint
  [tt,uu] = stairs(t,u');
  [ttr,rr] = stairs(t,r');
  t4 = t; y4 = y; tt4 = tt; uu4 = uu; ttr4 = ttr; rr4 = rr;
  figure(105)
  subplot(2,1,1)
  plot(t3,y3,t4,y4,'--',ttr4,rr4,':')
  axis([0 6 -1.5 1.5])
  legend('P = 8, M = 1','P = 7, M = 1')
  ylabel('y')
  xlabel('time')
  title('DMC: P = 8, M = 1 and P = 7, M = 1')
  subplot(2,1,2)
  plot(tt3,uu3,tt4,uu4,'--')
  legend('P = 8, M = 1','P = 7, M = 1')
  axis([0 6 -7 3])
  ylabel('u')
  xlabel('time')
%----------------------------------------------------------------------
%------- apply constraints to the unstable problem from simulation run 4
% p = 7, m = 1
%
  % run simulation
  iqp = 2;  % constrained
  umin = -5;
  umax =  5;
  tfinal = 6;
  QSSmpcNLPlant
% zero-order hold on input and setpoint
  [tt,uu] = stairs(t,u');
  [ttr,rr] = stairs(t,r');
  t5 = t; y5 = y; tt5 = tt; uu5 = uu; ttr5 = ttr; rr5 = rr;
  figure(106)
  subplot(2,1,1)
  plot(t4,y4,t5,y5,'--',ttr4,rr4,':')
  axis([0 6 -5 1.5])
  legend('unconstrained','constrained')
  ylabel('y')
  xlabel('time')
  title('P = 7, M = 1, unconstrained and constrained')
  subplot(2,1,2)
  plot(tt4,uu4,tt5,uu5,'--')
  legend('unconstrained','constrained')
  axis([0 6 -10 3])
  ylabel('u')
  xlabel('time')