% r_FOmpc.m
% 20 July 2011 - B.W. Bequette
%    First-order problem
% Illustrates the use of State Space MPC 
% The plant equations are continuous ODES provided in 'odetnkdev'
%    The plant equations are in deviation variable form
% The controller model is discretized
%
% ---------- model parameters ----------------------------------- 
%       (continous time, first-order problem)
  am = [-1/2]         % single state (time constant = 2)
  bm = [1/2 1/2]      % first column manip, 2nd column disturbance
  cm = [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);
%
% discretize the model
  delt = 0.2;               % sample time = 0.2
  sysd_mod = c2d(sysc_mod,delt);
  [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 disturbances
%
% ------------ plant parameters -------------------------------
  planteqns = 'FOodedev'
  cplant = [1];   % the only state is also the output
  nstates_p = 1;      % one plant state
  parvec(1) = 2; % the parameter is a time constant with value = 2
% ---------------- controller parameters -------------------
  p = 10;        % prediction horizon
  m = 1;         % 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 (used by KF)
  weightu = [0]; % weighting matrix for control action
  weighty = [1]; % weighting matrix for outputs
% -----------------constraints ------------------------------
  umin = [-1000];
  umax = [1000];
  dumin = [-1000];
  dumax = [1000];
% ------ Kalman Filter matrices -----------------------------
  Q  = 100*eye(nd_est,nd_est);  % state covariance
  R  = eye(ny);             % measurement noise covariance
%  
% -------- simulation parameters ------------------------------
% first, setpoint change only (no disturbance)
  ysp = [0.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
  tfinal = 10;
%
  isim = 1; % additive output disturbance
  iqp = 1;  % unconstrained solution
% noisemag = zeros(ny,1); % no noise
  noisemag = 0.0;      % no measurement noise
% ------------------- First Simulation -------------------------------
% ------------------- run simulation (call QSSmpcNLPLant) ------------
  QSSmpcNLPlant
%
  t1 = t; y1 = y; u1 = u; r1 = r;  % save vectors for plotting
  t1plot = tplot; y1plot = yplot; ymeas1 = ymeas;
% zero-order hold on input and setpoint
  [tt1,uu1] = stairs(t1,u1');
  [ttr1,rr1] = stairs(t1,r1');
%
  figure(1)
   subplot(2,1,1)
    plot(t1plot,y1plot,t1,ymeas1,'+',ttr1,rr1,':')
    legend('actual','measured','setpoint')
    ylabel('y')
    xlabel('time')
    title('FO process, P = 10, M = 1')
   subplot(2,1,2)
    plot(tt1,uu1)
    ylabel('u')
    xlabel('time')
% ----------------    Second Simulation    --------------------
% ---------------- another uconstrained simulation ------------
% ---------------- different control horizon ------------------
  isim = 1;
  iqp  = 2 % constrained
  umax = 0.4;
  m = 5;
  % run simulation
  QSSmpcNLPlant
  t2 = t; y2 = y; u2 = u; r2 = r;
  t2plot = tplot; y2plot = yplot; ymeas2 = ymeas;
% zero-order hold on input and setpoint
  [tt2,uu2] = stairs(t2,u2');
  [ttr2,rr2] = stairs(t2,r2');
%
  figure(2)
   subplot(2,1,1)
    plot(t2plot,y2plot,t2,ymeas2,'+',ttr2,rr2,':')
    legend('actual','measured','setpoint')
    ylabel('y')
    xlabel('time')
    title('FO process, P = 10, M = 5')
   subplot(2,1,2)
    plot(tt2,uu2)
    ylabel('u')
    xlabel('time')
% ------------------------- compare simulations 1 and 2 --------
%         compare M = 1 and 5 on same plot
  figure(3)
   subplot(2,1,1)
    plot(t1plot,y1plot,'b',t2plot,y2plot,'g',ttr2,rr2(:,1),'r:')
    legend('M = 1','M = 5')
    ylabel('y')
    xlabel('time')
    title('P = 10, M = 1 and 5')
   subplot(2,1,2)
   plot(tt1,uu1,'b',tt2,uu2,'g')
   legend('M = 1','M = 5')
   ylabel('u')
   xlabel('time')
%-------------------------------------------------------------------
%-----------------   Simulation 3 ----------------------------------
% ----------------   disturbance rejection -------------------------
%    P = 10, M = 1
  m = 1;  
  ysp = [0];    % setpoint change (from 0 vector); dimension ny
  timesp = 1;   % time of setpoint change
  dist = [1];   % magnitude of input disturbance; dimension nd
  timedis = 1;  % time of input disturbance
  tfinal = 10;
%
  isim = 1; % additive output disturbance
  iqp = 1;  % unconstrained solution
  noisemag = zeros(ny,1); % no noise
% ------------------- run simulation (call QSSmpcNLPLant) ------------
  QSSmpcNLPlant
%
  t3 = t; y3 = y; u3 = u; r3 = r;  % save vectors for plotting
  t3plot = tplot; y3plot = yplot;
% zero-order hold on input and setpoint
  [tt3,uu3] = stairs(t3,u3');
  [ttr3,rr3] = stairs(t3,r3');
%
  figure(4)
   subplot(2,1,1)
    plot(t3plot,y3plot,ttr3,rr3,':')
    ylabel('y')
    xlabel('time')
    title('Disturbance Rejection, additive output, First-order, P = 10, M = 1')
   subplot(2,1,2)
    plot(tt3,uu3)
    ylabel('u')
    xlabel('time')
 %-----------------------------------------------------------------------
 %-----------------   Simulation 4 ----------------------------------
% ----------------   disturbance rejection using KF ---------------------
%   keeping P = 10, M = 1
  ysp = [0];    % setpoint change (from 0 vector); dimension ny
  timesp = 1;   % time of setpoint change
  dist = [1];   % magnitude of input disturbance; dimension nd
  timedis = 1;  % time of input disturbance
  tfinal = 10;
%
  isim = 2; % KF for disturbance estimation
  iqp = 1;  % unconstrained solution
  noisemag = zeros(ny,1); % no noise
  Q  = 100*eye(nd_est,nd_est);  % state covariance
  R  = eye(ny);             % measurement noise covariance
% ------------------- run simulation (call QSSmpcNLPLant) ------------
  QSSmpcNLPlant
%
  t4 = t; y4 = y; u4 = u; r4 = r;  % save vectors for plotting
  t4plot = tplot; y4plot = yplot;
% zero-order hold on input and setpoint
  [tt4,uu4] = stairs(t4,u4');
  [ttr4,rr4] = stairs(t4,r4');
%
  figure(5)
   subplot(2,1,1)
    plot(t4plot,y4plot,ttr4,rr4,':')
    ylabel('y')
    xlabel('time')
    title('Disturbance Rejection, KF dist est, FO process, P = 10, M = 1')
   subplot(2,1,2)
    plot(tt4,uu4)
    ylabel('u')
    xlabel('time')
 % -------------------- compare additive disturbance and KF ----------
   figure(6)
   subplot(2,1,1)
    plot(t3plot,y3plot,t4plot,y4plot,ttr4,rr4,':')
    legend('DMC','KF','setpoint')
    ylabel('y')
    xlabel('time')
    title('Disturbance Rejection, DMC & KF, First-order, P = 10, M = 1')
   subplot(2,1,2)
    plot(tt3,uu3,tt4,uu4)
    ylabel('u')
    xlabel('time')
 % ---------------------------------------------------------------------
 % ------------ another disturbance rejection problem, but with noise
 % ------------  --------------
  isim = 1; % additive disturbance (DMC)
  ysp = [0];    % no setpoint change (from 0 vector); dimension ny
  timesp = 1;   % time of setpoint change
  dist = [1];   % magnitude of input disturbance; dimension nd
  iqp  = 1; % unconstrained
  isim = 1; % additive output disturbance
  noisemag = 0.1; 
  p=10;
  m = 1;
  % run simulation
  QSSmpcNLPlant
  t5 = t; y5 = y; u5 = u; r5 = r;
  t5plot = tplot; y5plot = yplot;
% zero-order hold on input and setpoint
  [tt5,uu5] = stairs(t5,u5');
  [ttr5,rr5] = stairs(t5,r5');
%
  figure(7)
   subplot(2,1,1)
    plot(t5plot,y5plot,tt5,rr5,':')
    ylabel('y')
    xlabel('time')
    title('FO process with noise, DMC, P = 10, M = 5')
   subplot(2,1,2)
    plot(tt5,uu5)
    ylabel('u')
    xlabel('time')
 %----------------- now, KF
    isim = 2;  % KF simulation
    Q  = eye(nd_est,nd_est);  % state covariance
    R  = eye(ny);             % measurement noise covariance
    QSSmpcNLPlant
  t6 = t; y6 = y; u6 = u; r6 = r;
  t6plot = tplot; y6plot = yplot;
% zero-order hold on input and setpoint
  [tt6,uu6] = stairs(t6,u6');
  [ttr6,rr6] = stairs(t6,r6');
    figure(8)
   subplot(2,1,1)
    plot(t6plot,y6plot,tt6,rr6,':')
    ylabel('y')
    xlabel('time')
    title('FO process, KF, P = 10, M = 1')
   subplot(2,1,2)
    plot(tt6,uu6)
    ylabel('u')
    xlabel('time')
    
  % ------------------- compare ---------------
    figure(9)
   subplot(2,1,1)
    plot(t5plot,y5plot,t6plot,y6plot,tt6,rr6,':')
    legend('DMC','KF')
    ylabel('y')
    xlabel('time')
    title('FO process with noise, DMC vs. KF, P = 10, M = 1')
   subplot(2,1,2)
    plot(tt5,uu5,tt6,uu6)
     legend('DMC','KF')
    ylabel('u')
    xlabel('time')