%  r_hw5.m
%  show the difference between two different solutions to a P-step-ahead
%     control problem
%  Problem 1 -- consider a single control move, which is then help constant
%  Problem 2 -- consider P control moves, where the objective is to
%               minimize the 2-norm of the control move vector (minimize
%               the sum of the squares of the control changes
%               The best solution is obtained using the MATLAB pseudo-
%               inverse, pinv
%  Problem 2, extended -- we further consider a control horizon of M
%               control moves, where M < P
%  
%  b.w. bequette
%  13 Oct 11
%   6 Oct 13 -- additional "feedback" simulation -- implement just the 
%               first move at each time step, then recalculate moves after
%               next measurement
%   
%
   clear
%
%  use the three tank example
%   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
%
%  extract the matrix information
%
  [phi,gamma,cmat,dmat] = ssdata(tankssz)   % cmat will equal c
%
  x0        = zeros(3,1);   % starting at steady-state
  uinit     = 0;
%
  xdis(:,1) = x0;
%
  P = 3 ;    % coincidence point
%
  rspP = 1; % for convenience, setpoint change to 1
%
  time = 0:delt:P*delt;
%
  for i = 1:P;
      H(i) = cmat*(phi^(i-1))*gamma;   % impulse response coefficients
      S(i) = sum(H(1:i));              % step response coefficients
  end
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  ydis(1) = cmat*xdis(:,1);       % first measured output
%
%  one control move for entire horizon
%
   usingle = (rspP - cmat*(phi^P)*x0)/S(P);
%
  for k = 1:P;
      u(k) = usingle;
      [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)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
  end
%
  u(k+1) = u(k);
%  
% figure(3000) % plotting states
% plot(tplot,xplot)
% legend('states')
% title('P = 3, coincidence point, single control move')
% xlabel('time, min')
% ylabel('x')
% 
  [tt,uu] = stairs(time,u);  % zero-order hold on input
  figure(3001)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('one move, P = 3')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%           now, save vectors for plotting comparison later
  tplot1 = tplot; xplot1 = xplot; tt1 = tt; uu1 = uu; yplot1 = yplot;
  
% Now, perform simulations for P = 10
%
   P =10;    % coincidence point
%
  rspP = 1; % for convenience, setpoint change to 1
%
  time = 0:delt:P*delt;
%
  for i = 1:P;
      H(i) = cmat*(phi^(i-1))*gamma;   % impulse response coefficients
      S(i) = sum(H(1:i));              % step response coefficients
  end
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  ydis(1) = cmat*xdis(:,1);       % first measured output
%
%  one control move for entire horizon
%
   usingle = (rspP - cmat*(phi^P)*x0)/S(P);
%
  for k = 1:P;
      u(k) = usingle;
      [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)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
  end
%
  u(k+1) = u(k);
%  
% figure(3002)
% plot(tplot,xplot)
% legend('states')
% title('P = 10, coincidence point, single control move')
% xlabel('time, min')
% ylabel('x')
% 
  [tt,uu] = stairs(time,u);  % zero-order hold on input
  figure(3003)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('one move, P = 10')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%           now, save vectors for plotting comparison later
  tplot2 = tplot; xplot2 = xplot; tt2 = tt; uu2 = uu; yplot2 = yplot;
% 
% ------------------another simulation -------------
%
     P =20;    % coincidence point
%
  rspP = 1; % for convenience, setpoint change to 1
%
  time = 0:delt:P*delt;
%
  for i = 1:P;
      H(i) = cmat*(phi^(i-1))*gamma;   % impulse response coefficients
      S(i) = sum(H(1:i));              % step response coefficients
  end
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  ydis(1) = cmat*xdis(:,1);       % first measured output
%
%  one control move for entire horizon
%
   usingle = (rspP - cmat*(phi^P)*x0)/S(P);
%
  for k = 1:P;
      u(k) = usingle;
      [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)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
  end
%
  u(k+1) = u(k);
%  
%  figure(3004)
%  plot(tplot,xplot)
% legend('states')
% title('P = 20, coincidence point, single control move')
% xlabel('time, min')
% ylabel('x')
% 
  [tt,uu] = stairs(time,u);  % zero-order hold on input
  figure(3005)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('one move, P = 20')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%           now, save vectors for plotting comparison later
  tplot3 = tplot; xplot3 = xplot; tt3 = tt; uu3 = uu; yplot3 = yplot;
%
  figure(3006)
  subplot(2,1,1)
  plot(tplot1,yplot1,tplot2,yplot2,tplot3,yplot3,[0 10], [1 1],'--')
  legend('P =  3','P = 10','P = 20')
  xlabel('t, min')
  ylabel('y')
  title('one move, P = 3, 10, 20')
  subplot(2,1,2)
  plot(tt1,uu1,tt2,uu2,tt3,uu3)
  legend('P =  3','P = 10','P = 20')
  xlabel('t, min')
  ylabel('u')
  axis([0 10 0 6])
%
%--------------------------------------------------------------------------
%- Now, the "minimum energy" solution
%
  clear H; clear S; clear t; clear u;
%
  P = 3;
%  
  time = 0:delt:P*delt;
%
  for i = 1:P;
      H(i) = cmat*(phi^(i-1))*gamma;   % impulse response coefficients
      S(i) = sum(H(1:i));              % step response coefficients
  end
  freeresp = cmat*(phi^P)*x0 + S(P)*uinit;
  Efree    = rspP - freeresp;
%
   Sf = S(P:-1:1);   % orient Sf in the opposite direction
%
% deluvec  = (Sf'*Sf)\(Sf')*Efree;   % Could not get good results with this
% deluvec  = Sf\Efree;               % Could not get good results with this
  deluvec  = pinv(Sf)*Efree;         % Appears to be the best implementation
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  for k = 1:P;
      if k == 1;
        u(k) = uinit + deluvec(k);
      else
        u(k) = u(k-1) + deluvec(k);
      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)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
  end
%
  u(k+1) = u(k);
%
%  figure(3007)
%  plot(tplot,xplot)
%
  [tt,uu] = stairs(time,u);
%
  figure(3008)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('min norm(del u), P = 3')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%           now, save vectors for plotting comparison later
  tplot4 = tplot; xplot4 = xplot; tt4 = tt; uu4 = uu; yplot4 = yplot;
%
%  more simulations
%
  P = 10;
%  
  time = 0:delt:P*delt;
%
  for i = 1:P;
      H(i) = cmat*(phi^(i-1))*gamma;   % impulse response coefficients
      S(i) = sum(H(1:i));              % step response coefficients
  end
  freeresp = cmat*(phi^P)*x0 + S(P)*uinit;
  Efree    = rspP - freeresp;
%
   Sf = S(P:-1:1);   % orient Sf in the opposite direction
%
% deluvec  = (Sf'*Sf)\(Sf')*Efree;   % Could not get good results with this
% deluvec  = Sf\Efree;               % Could not get good results with this
  deluvec  = pinv(Sf)*Efree;         % Appears to be the best implementation
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  for k = 1:P;
      if k == 1;
        u(k) = uinit + deluvec(k);
      else
        u(k) = u(k-1) + deluvec(k);
      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)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
  end
%
  u(k+1) = u(k);
%
%  figure(3009)
%  plot(tplot,xplot)
%
  [tt,uu] = stairs(time,u);
  figure(3010)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('min norm(del u), P = 10')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%           now, save vectors for plotting comparison later
  tplot5 = tplot; xplot5 = xplot; tt5 = tt; uu5 = uu; yplot5 = yplot;
  
% another simulation
  P = 20;
%  
  time = 0:delt:P*delt;
%
  for i = 1:P;
      H(i) = cmat*(phi^(i-1))*gamma;   % impulse response coefficients
      S(i) = sum(H(1:i));              % step response coefficients
  end
  freeresp = cmat*(phi^P)*x0 + S(P)*uinit;
  Efree    = rspP - freeresp;
%
   Sf = S(P:-1:1);   % orient Sf in the opposite direction
%
% deluvec  = (Sf'*Sf)\(Sf')*Efree;   % Could not get good results with this
% deluvec  = Sf\Efree;               % Could not get good results with this
  deluvec  = pinv(Sf)*Efree;         % Appears to be the best implementation
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  for k = 1:P;
      if k == 1;
        u(k) = uinit + deluvec(k);
      else
        u(k) = u(k-1) + deluvec(k);
      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)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
  end
%
  u(k+1) = u(k);
%
%  figure(3011)
%  plot(tplot,xplot)
%
  [tt,uu] = stairs(time,u);
  figure(3012)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('min norm(del u), P = 20')
  subplot(2,1,2)
  plot(tt,uu)
  xlabel('t, min')
  ylabel('u')
%           now, save vectors for plotting comparison later
  tplot6 = tplot; xplot6 = xplot; tt6 = tt; uu6 = uu; yplot6 = yplot;
%
  figure(3013)
  subplot(2,1,1)
  plot(tplot4,yplot4,tplot5,yplot5,tplot6,yplot6)
  xlabel('t, min')
  ylabel('y')
  title('min norm(del u), P = 3,10, 20')
  subplot(2,1,2)
  plot(tt4,uu4,tt5,uu5,tt6,uu6)
  xlabel('t, min')
  ylabel('u')
% ----------------------------------------------------
  figure(3019)
  subplot(2,1,1)
  plot(tplot3,yplot3,tplot6,yplot6)
  legend('one move','min energy')
  xlabel('t, min')
  ylabel('y')
  title('comparison of control strategies, P = 20')
  subplot(2,1,2)
  plot(tt3,uu3,tt6,uu6)
  legend('one move','min energy')
  xlabel('t, min')
  ylabel('u')
%----------------------------------------------------------------------
%----------------- Now, consider a "control horizon" of M moves -------
%----------------- where M < P
%
  P = 20;          % prediction horizon
  M = 5;           % control horizon
  
  time = 0:delt:P*delt;
  
  SfM = Sf(1:M);   % only the leading M step response coefficients are used
  
% deluvec  = (SfM'*SfM)\(SfM')*Efree;   % Could not get good results with this
% deluvec  = SfM\Efree;                 % Could not get good results with this
  deluvec  = pinv(SfM)*Efree;           % Appears to be the best implementation
%                         % no changes in control action after first M
  deluvec  = [deluvec;zeros(P-length(deluvec),1)];
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  for k = 1:P;
      if k == 1;
        u(k) = uinit + deluvec(k);
      else
        u(k) = u(k-1) + deluvec(k);
      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)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
  end
%
  u(k+1) = u(k);
  [tt,uu] = stairs(time,u);
  %           now, save vectors for plotting comparison later
  tplot7 = tplot; xplot7 = xplot; tt7 = tt; uu7 = uu; yplot7 = yplot;
%
  figure(3020)
  subplot(2,1,1)
  plot(tplot6,yplot6,tplot7,yplot7)
  legend('M = 20','M = 5')
  xlabel('t, min')
  ylabel('y')
  title('comparison of control horizons, P = 20')
  subplot(2,1,2)
  plot(tt6,uu6,tt7,uu7)
  legend('M = 20','M = 5')
  xlabel('t, min')
  ylabel('u')
% ------------------------------------------------------------------------
% ------------------------------------------------------------------------
% ------------- The previous simulations were for open-loop --------------
% ------------- A single set of control moves were calculated ------------
% ------------- then applied. No feedback was used. ----------------------
% -------------
% ------------- Now, perform feedback based on the model states. ---------
% ------------- Calculate a set of control moves at each time step, ------
% ------------- just apply the first one, update the model state ---------
% ------------- at the end of the time step and repeat. ------------------
% -------------
%
  P = 3;          % prediction horizon
  M = 3;           % control horizon
%
  clear H
  clear S
%
  Nsim = 20; % simulate for 20 points
  time = 0:delt:Nsim*delt;
%
    for i = 1:P;
      H(i) = cmat*(phi^(i-1))*gamma;   % impulse response coefficients
      S(i) = sum(H(1:i));              % step response coefficients
    end
%
   Sf = S(P:-1:1);   % orient Sf in the opposite direction
%
  SfM = Sf(1:M);   % only the leading M step response coefficients are used
  
% deluvec  = (SfM'*SfM)\(SfM')*Efree;   % Could not get good results with this
% deluvec  = SfM\Efree;                 % Could not get good results with this

%
  tplot = [];
  yplot = [];
  xplot = [];
% 
  
  xmod(:,1) = x0;
  xdis(:,1) = x0;
%
  for k = 1:Nsim;
      if k == 1;
        dhat(k) = c*xdis(:,k) - c*xmod(:,k);
        freeresp = cmat*(phi^P)*x0 + S(P)*uinit;
        Efree    = rspP - freeresp; 
        deluvec  = pinv(SfM)*Efree; 
        u(k) = uinit + deluvec(1);
      else
        dhat(k) = ydis(k) - c*xmod(:,k);
        freeresp = cmat*(phi^P)*xmod(:,k) + S(P)*u(k-1)+dhat(k);
         Efree    = rspP - freeresp;
         deluvec  = pinv(SfM)*Efree;           % Appears to be the best implementation
         u(k) = u(k-1) + deluvec(1);
       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)
      tplot = [tplot;tdummy];
      yplot = [yplot;xdummy*c'];
      xplot = [xplot;xdummy];
      xmod(:,k+1) = phi*xmod(:,k) + gamma*u(k);
  end
%
  u(k+1) = u(k);
  [tt,uu] = stairs(time,u);
  %           now, save vectors for plotting comparison later
  tplot8 = tplot; xplot8 = xplot; tt8 = tt; uu8 = uu; yplot8 = yplot;
%
  figure(3025)
  subplot(2,1,1)
  plot(tplot8,yplot8)
  xlabel('t, min')
  ylabel('y')
  title('feedback implementation')
  subplot(2,1,2)
  plot(tt8,uu8)
  xlabel('t, min')
  ylabel('u')
%
  figure(3030)
  plot(tplot8,yplot8,time,xmod(3,:))
  legend('plant','model')
  title('comparison of plant and model outputs, FB')
  xlabel('t, min')
  ylabel('y')
%
  figure(3031)
  subplot(2,1,1)
  plot(tplot8,yplot8,tplot4,yplot4,'--')
  legend('feedback','open')
  title('comparison of open and feedback, M=P=3')
  xlabel('t, min')
  ylabel('y')
  subplot(2,1,2)
  plot(tt8,uu8,tt4,uu4,'--')
  legend('feedback','open')
  xlabel('t, min')
  ylabel('u')