% r_hw3_db.m -- IMC Workshop Solution
% MPC Workshop, Trondheim (January 2015)
%  deadbeat final example
%
% Homework 3 Solution -- MPC 2013
% B.W. Bequette, 18 September 2013
%   Implements discrete IMC strategies, based on approach by Zarifiou &
%   Morari
%
%  function files:
%    dimc    -- IMC discrete controller q(z) -- calculates u(k)
%    dimcmod -- internal model -- calculates y(k)
%
% first, continuous state space model
  a = [-1 0 0;1 -1 0;0 1 -1]
  b = [1;0;0]
  c = [0 0 1]
  d = 0
%
  lintank = ss(a,b,c,d)              % defines continuous state space model
% --------------------------------------------------------------------
% 
% discrete time model
  delt = 0.5;                        % sample time of 0.5 minutes
  tankssz = c2d(lintank,delt,'zoh')  % create discrete state space model from continuous
% discrete process transfer function
  tanktfz = tf(tankssz)              % create discrete t.f. from discrete state space model
  [nump,denp,tsample] = tfdata(tanktfz,'v')  % get the numerator and denominator polynomials
  roots(nump)                        % finds zeros of discrete transfer function
  roots(denp)                        % finds poles of discrete transfer function
  tanktzpk = zpk(tankssz)            % zero-pole-gain form (consistency check)
%--------------------------------------------------------------------------
%
%------  Discrete IMC formulation
%------
  lambda = 1    % minute
  alpha = exp(-delt/lambda)    % discrete filter parameter
  qnum = 16.414*(1-alpha).*[1 -1.8195 1.1035  -0.2231];  % IMC numerator
  qden = [1 -alpha]                       % IMC denominator
  modnum = [0.01439  0.03973 0.006794]            % IM numerator
  modden = [1  -1.82  1.104  -0.2231]        % IM denominator
%
% now, set up a for loop and integrate over 0.5 minute time steps
% currently set up for single input - single output
%
  tplot = [];
  yplot = [];  % save more points for plotting in between samples
  tbeg = 0;    % simulation start time
  tend = 5;   % simulation end time
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(1,1);ones(length(time)-1,1)]; % setpoint change at second step
  x0  = zeros(3,1);
  y0  = 0;
  u0  = 0;
  xdis(:,1) = x0;              % discrete plant state
  ydis(1)   = y0;              % discrete plant output (measurement) at first step
  ymod0     = y0;              % initially, model = plant output
  ymod(1)   = ymod0;
  for k = 1:length(time)-1;
    if k == 1;
        rmodvec = [0;0;0;rsp(1)];
        uvec = [u0];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
        umodvec = [0;u0;u(k)];
        ymodvec = [0;0;y0];
        ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 2;
        rmodvec = [0;0;rsp(1);rsp(2)-distmod(2)];
        uvec = [u(k-1)];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u0;u(k-1);u(k)];
         ymodvec = [y0;y0;ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 3;
         rmodvec = [0;rsp(k-2:k)-distmod(k-2:k)'];
         uvec =  [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [y0;ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    else
         rmodvec = rsp(k-3:k)-distmod(k-3:k)';
         uvec = [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [ymod(k-2);ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:);   % plant state
      ydis(k+1) = c*xdis(:,k+1);      % plant output (measured)
      distmod(k+1) = ydis(k+1) - ymod(k+1);
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(211)
  subplot(2,1,1)
  plot(tplot,yplot,ttrsp,rrsp,'--')
  legend('y','sp')
  ylabel('y')
  title('three tank process, IMC implementation')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%  save vectors for plotting
  tplot1 = tplot; yplot1 = yplot; tt1 = tt; uu1 = uu; rrsp1 = rrsp; ttrsp1 = ttrsp;
% ------------------------------------------------------------
  lambda = 0.5
  alpha = exp(-delt/lambda)
  qnum = 16.414*(1-alpha).*[1 -1.8195 1.1035  -0.2231];  % IMC numerator
  qden = [1 -alpha]                       % IMC denominator
  modnum = [0.01439  0.03973 0.006794]            % IM numerator
  modden = [1  -1.82  1.104  -0.2231]        % IM denominator
%
% now, set up a for loop and integrate over 0.5 minute time steps
% currently set up for single input - single output
%
  tplot = [];
  yplot = [];  % save more points for plotting in between samples
  tbeg = 0;    % simulation start time
  tend = 5;   % simulation end time
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(1,1);ones(length(time)-1,1)]; % setpoint change at second step
  x0  = zeros(3,1);
  y0  = 0;
  u0  = 0;
  xdis(:,1) = x0;              % discrete plant state
  ydis(1)   = y0;              % discrete plant output (measurement) at first step
  ymod0     = y0;              % initially, model = plant output
  ymod(1)   = ymod0;
  for k = 1:length(time)-1;
    if k == 1;
        rmodvec = [0;0;0;rsp(1)];
        uvec = [u0];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
        umodvec = [0;u0;u(k)];
        ymodvec = [0;0;y0];
        ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 2;
        rmodvec = [0;0;rsp(1);rsp(2)-distmod(2)];
        uvec = [u(k-1)];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u0;u(k-1);u(k)];
         ymodvec = [y0;y0;ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 3;
         rmodvec = [0;rsp(k-2:k)-distmod(k-2:k)'];
         uvec =  [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [y0;ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    else
         rmodvec = rsp(k-3:k)-distmod(k-3:k)';
         uvec = [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [ymod(k-2);ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:);   % plant state
      ydis(k+1) = c*xdis(:,k+1);      % plant output (measured)
      distmod(k+1) = ydis(k+1) - ymod(k+1);
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(212)
  subplot(2,1,1)
  plot(tplot,yplot,ttrsp,rrsp,'--')
  legend('y','sp')
  ylabel('y')
  title('three tank process, IMC implementation')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%  save vectors for plotting
  tplot2 = tplot; yplot2 = yplot; tt2 = tt; uu2 = uu; rrsp2 = rrsp; ttrsp2 = ttrsp;
% -------------------------------------------------------------------------
%  another value of lambda
  lambda = 0.25   % minutes
  alpha = exp(-delt/lambda)
  qnum = 16.414*(1-alpha).*[1 -1.8195 1.1035  -0.2231];  % IMC numerator
  qden = [1 -alpha]                       % IMC denominator
  modnum = [0.01439  0.03973 0.006794]            % IM numerator
  modden = [1  -1.82  1.104  -0.2231]        % IM denominator
%
% now, set up a for loop and integrate over 0.5 minute time steps
% currently set up for single input - single output
%
  tplot = [];
  yplot = [];  % save more points for plotting in between samples
  tbeg = 0;    % simulation start time
  tend = 5;   % simulation end time
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(1,1);ones(length(time)-1,1)]; % setpoint change at second step
  x0  = zeros(3,1);
  y0  = 0;
  u0  = 0;
  xdis(:,1) = x0;              % discrete plant state
  ydis(1)   = y0;              % discrete plant output (measurement) at first step
  ymod0     = y0;              % initially, model = plant output
  ymod(1)   = ymod0;
  for k = 1:length(time)-1;
    if k == 1;
        rmodvec = [0;0;0;rsp(1)];
        uvec = [u0];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
        umodvec = [0;u0;u(k)];
        ymodvec = [0;0;y0];
        ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 2;
        rmodvec = [0;0;rsp(1);rsp(2)-distmod(2)];
        uvec = [u(k-1)];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u0;u(k-1);u(k)];
         ymodvec = [y0;y0;ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 3;
         rmodvec = [0;rsp(k-2:k)-distmod(k-2:k)'];
         uvec =  [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [y0;ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    else
         rmodvec = rsp(k-3:k)-distmod(k-3:k)';
         uvec = [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [ymod(k-2);ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:);   % plant state
      ydis(k+1) = c*xdis(:,k+1);      % plant output (measured)
      distmod(k+1) = ydis(k+1) - ymod(k+1);
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(213)
  subplot(2,1,1)
  plot(tplot,yplot,ttrsp,rrsp,'--')
  legend('y','sp')
  ylabel('y')
  title('three tank process, IMC implementation')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%  save vectors for plotting
  tplot3 = tplot; yplot3 = yplot; tt3 = tt; uu3 = uu; rrsp3 = rrsp; ttrsp3 = ttrsp;
% -------------------------------------------------------------------------
%  compare response for different values of lambda
  figure(214)
  subplot(2,1,1)
  plot(tplot3,yplot3,tplot2,yplot2,tplot1,yplot1,ttrsp,rrsp,'--')
  legend('lambda = 0.25','lambda = 0.5','lambda = 1','sp')
  ylabel('y')
  title('three tank process, IMC implementation')
  subplot(2,1,2)
  plot(tt3,uu3,tt2,uu2,tt1,uu1)
  xlabel('t, min')
  ylabel('u')
%----------------------------------------------------------------------
% reduce lambda more -- state deadbeat
%
  lambda = 0.000     % state deadbeat when lamba = 0
    alpha = exp(-delt/lambda)
  qnum = 16.414*(1-alpha).*[1 -1.8195 1.1035  -0.2231];  % IMC numerator
  qden = [1 -alpha]                       % IMC denominator
  modnum = [0.01439  0.03973 0.006794]            % IM numerator
  modden = [1  -1.82  1.104  -0.2231]        % IM denominator
%
% now, set up a for loop and integrate over 0.5 minute time steps
% currently set up for single input - single output
%
  tplot = [];
  yplot = [];  % save more points for plotting in between samples
  tbeg = 0;    % simulation start time
  tend = 5;   % simulation end time
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(1,1);ones(length(time)-1,1)]; % setpoint change at second step
  x0  = zeros(3,1);
  y0  = 0;
  u0  = 0;
  xdis(:,1) = x0;              % discrete plant state
  ydis(1)   = y0;              % discrete plant output (measurement) at first step
  ymod0     = y0;              % initially, model = plant output
  ymod(1)   = ymod0;
  for k = 1:length(time)-1;
    if k == 1;
        rmodvec = [0;0;0;rsp(1)];
        uvec = [u0];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
        umodvec = [0;u0;u(k)];
        ymodvec = [0;0;y0];
        ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 2;
        rmodvec = [0;0;rsp(1);rsp(2)-distmod(2)];
        uvec = [u(k-1)];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u0;u(k-1);u(k)];
         ymodvec = [y0;y0;ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 3;
         rmodvec = [0;rsp(k-2:k)-distmod(k-2:k)'];
         uvec =  [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [y0;ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    else
         rmodvec = rsp(k-3:k)-distmod(k-3:k)';
         uvec = [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [ymod(k-2);ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:);   % plant state
      ydis(k+1) = c*xdis(:,k+1);      % plant output (measured)
      distmod(k+1) = ydis(k+1) - ymod(k+1);
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(215)
  subplot(2,1,1)
  plot(tplot,yplot,time,ydis,'b+',ttrsp,rrsp,'--')
  legend('y','ymeas','sp')
  ylabel('y')
  title('three tank process, state deadbeat control')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%  save vectors for plotting
  tplot4 = tplot; yplot4 = yplot; tt4 = tt; uu4 = uu; rrsp4 = rrsp; ttrsp4 = ttrsp;
% -------------------------------------------------------------------------
% ---  model error
% ---  keep model for controller design the same, change plant
%  new plant equations
  ap = [-1 0 0;1 -1 0;0 1 -1]
  bp = [1.3;0;0]                 %   increase in plant gain
  cp = [0 0 1]
  dp = 0
%
  lambda = 0.25
  alpha = exp(-delt/lambda)
  qnum = 16.414*(1-alpha).*[1 -1.8195 1.1035  -0.2231];  % IMC numerator
  qden = [1 -alpha]                       % IMC denominator
  modnum = [0.01439  0.03973 0.006794]            % IM numerator
  modden = [1  -1.82  1.104  -0.2231]        % IM denominator
%
% now, set up a for loop and integrate over 1 minute time steps
% currently set up for single input - single output
%
  tplot = [];
  yplot = [];  % save more points for plotting in between samples
  tbeg = 0;    % simulation start time
  tend = 5;   % simulation end time
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(1,1);ones(length(time)-1,1)]; % setpoint change at second step
  x0  = zeros(3,1);
  y0  = 0;
  u0  = 0;
  xdis(:,1) = x0;              % discrete plant state
  ydis(1)   = y0;              % discrete plant output (measurement) at first step
  ymod0     = y0;              % initially, model = plant output
  ymod(1)   = ymod0;
  for k = 1:length(time)-1;
    if k == 1;
        rmodvec = [0;0;0;rsp(1)];
        uvec = [u0];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
        umodvec = [0;u0;u(k)];
        ymodvec = [0;0;y0];
        ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 2;
        rmodvec = [0;0;rsp(1);rsp(2)-distmod(2)];
        uvec = [u(k-1)];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u0;u(k-1);u(k)];
         ymodvec = [y0;y0;ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 3;
         rmodvec = [0;rsp(k-2:k)-distmod(k-2:k)'];
         uvec =  [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [y0;ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    else
         rmodvec = rsp(k-3:k)-distmod(k-3:k)';
         uvec = [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [ymod(k-2);ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],ap,bp,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:);   % plant state
      ydis(k+1) = cp*xdis(:,k+1);      % plant output (measured)
      distmod(k+1) = ydis(k+1) - ymod(k+1);
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(216)
  subplot(2,1,1)
  plot(tplot,yplot,ttrsp,rrsp,'--')
  legend('y','sp')
  ylabel('y')
  title('three tank process, IMC implementation, kp = 1.3')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%  plot disturbance term
  figure(217)
  plot(time,distmod)
  xlabel('t, min')
  ylabel('dest')
  title('disturbance estimate')
%  save vectors for plotting
  tplot5 = tplot; yplot5 = yplot; tt5 = tt; uu5 = uu; rrsp5 = rrsp; ttrsp5 = ttrsp;
  time5 = time; distmod5 = distmod;
%--------------------------------------------------------------------------
% gain uncertainty, kp = 0.7
%  new plant equations
  ap = [-1 0 0;1 -1 0;0 1 -1]
  bp = [0.7;0;0]                 %   increase in plant gain
  cp = [0 0 1]
  dp = 0
%
  lambda = 0.25
  alpha = exp(-delt/lambda)
  qnum = 16.414*(1-alpha).*[1 -1.8195 1.1035  -0.2231];  % IMC numerator
  qden = [1 -alpha]                       % IMC denominator
  modnum = [0.01439  0.03973 0.006794]            % IM numerator
  modden = [1  -1.82  1.104  -0.2231]        % IM denominator
%
% now, set up a for loop and integrate over 0.5 minute time steps
% currently set up for single input - single output
%
  tplot = [];
  yplot = [];  % save more points for plotting in between samples
  tbeg = 0;    % simulation start time
  tend = 5;   % simulation end time
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(1,1);ones(length(time)-1,1)]; % setpoint change at second step
  x0  = zeros(3,1);
  y0  = 0;
  u0  = 0;
  xdis(:,1) = x0;              % discrete plant state
  ydis(1)   = y0;              % discrete plant output (measurement) at first step
  ymod0     = y0;              % initially, model = plant output
  ymod(1)   = ymod0;
  for k = 1:length(time)-1;
    if k == 1;
        rmodvec = [0;0;0;rsp(1)];
        uvec = [u0];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
        umodvec = [0;u0;u(k)];
        ymodvec = [0;0;y0];
        ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 2;
        rmodvec = [0;0;rsp(1);rsp(2)-distmod(2)];
        uvec = [u(k-1)];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u0;u(k-1);u(k)];
         ymodvec = [y0;y0;ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 3;
         rmodvec = [0;rsp(k-2:k)-distmod(k-2:k)'];
         uvec =  [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [y0;ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    else
         rmodvec = rsp(k-3:k)-distmod(k-3:k)';
         uvec = [u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [ymod(k-2);ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],ap,bp,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:);   % plant state
      ydis(k+1) = cp*xdis(:,k+1);      % plant output (measured)
      distmod(k+1) = ydis(k+1) - ymod(k+1);
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(218)
  subplot(2,1,1)
  plot(tplot,yplot,ttrsp,rrsp,'--')
  legend('y','sp')
  ylabel('y')
  title('three tank process, IMC implementation')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%  plot disturbance term
  figure(219)
  plot(time,distmod)
  xlabel('t, min')
  ylabel('dest')
  title('disturbance estimate')
%  save vectors for plotting
  tplot6 = tplot; yplot6 = yplot; tt6 = tt; uu6 = uu; rrsp6 = rrsp; ttrsp6 = ttrsp;
  time6 = time; distmod6 = distmod;
%--------------------------------------------------------------------------
%
%  compare closed-loop responses for high (1.3) and low (0.7) gain plants
%
  figure(220)
  subplot(2,1,1)
  plot(tplot5,yplot5,tplot6,yplot6,ttrsp,rrsp,'--')
  legend('kp = 1.3','kp = 0.7','sp')
  ylabel('y')
  title('three tank process, IMC implementation, lambda = 0.25 min')
  subplot(2,1,2)
  plot(tt5,uu5,tt6,uu6)
  xlabel('t, min')
  ylabel('u')
%
  figure(221)
  plot(time5,distmod5,time6,distmod6)
  legend('kp = 1.3','kp = 0.7')
  xlabel('t, min')
  ylabel('dest')
  title('comparision of disturbance estimates')
% ----------------------------------------------------------------
%  deadbeat final example
%
  qnum = [1 -1.8195 1.1035  -0.2231]./0.01439;  % IMC numerator
  qnum = modden./modnum(1)
  qden = modnum./modnum(1)                      % IMC denominator
  tplot = [];
  yplot = [];  % save more points for plotting in between samples
  tbeg = 0;    % simulation start time
  tend = 5;   % simulation end time
  time = tbeg:delt:tend;       % generate the time vector
  rsp = [zeros(1,1);ones(length(time)-1,1)]; % setpoint change at second step
  x0  = zeros(3,1);
  y0  = 0;
  u0  = 0;
  xdis(:,1) = x0;              % discrete plant state
  ydis(1)   = y0;              % discrete plant output (measurement) at first step
  ymod0     = y0;              % initially, model = plant output
  ymod(1)   = ymod0;
  for k = 1:length(time)-1;
    if k == 1;
        rmodvec = [0;0;0;rsp(1)];
        uvec = [u0;u0];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
        umodvec = [0;u0;u(k)];
        ymodvec = [0;0;y0];
        ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 2;
        rmodvec = [0;0;rsp(1);rsp(2)-distmod(2)];
        uvec = [u0;u(k-1)];
        u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u0;u(k-1);u(k)];
         ymodvec = [y0;y0;ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    elseif k == 3;
         rmodvec = [0;rsp(k-2:k)-distmod(k-2:k)'];
         uvec =  [u(k-1);u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [y0;ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    else
         rmodvec = rsp(k-3:k)-distmod(k-3:k)';
         uvec = [u(k-2);u(k-1)];
         u(k) = dimc(qnum,qden,rmodvec,uvec);
         umodvec = [u(k-2);u(k-1);u(k)];
         ymodvec = [ymod(k-2);ymod(k-1);ymod(k)];
         ymod(k+1) = dimcmod(modnum,modden,ymodvec,umodvec);
    end
      [tdummy,xdummy] = ode45('linodepar',[time(k) time(k+1)],xdis(:,k),[],a,b,u(k));
      ndum = length(tdummy);
      xdis(:,k+1) = xdummy(ndum,:);   % plant state
      ydis(k+1) = c*xdis(:,k+1);      % plant output (measured)
      distmod(k+1) = ydis(k+1) - ymod(k+1);
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
  end
%
  u(k+1) = u(k);    % makes the input vector the same length as the time vector
%
  [tt,uu] = stairs(time,u);
  [ttrsp,rrsp] = stairs(time,rsp);
  figure(231)
  subplot(2,1,1)
  plot(tplot,yplot,ttrsp,rrsp,'--')
  legend('y','sp')
  ylabel('y')
  axis([0 3 -10 10])
  title('three tank process, IMC implementation')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
  axis([0 5 -10000 10000])
%  save vectors for plotting
  tplot7 = tplot; yplot7 = yplot; tt7 = tt; uu7 = uu; rrsp7 = rrsp; ttrsp7 = ttrsp;
  