%  r_hw6.m
%     Optimize M control moves for a prediction horizon of P output steps
%     into the future.
%         Assume no weight on the manipulated input
%
%  Problem 1 -- M = 1, P = 20
%
%  Problem 2 -- M = 5, P = 20
%  
%  b.w. bequette
%  24 Oct 11, revised 27 October 13 for 2013 MPC course
%
%  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 =20;    % multi-step-ahead control
%
  rsp = 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
      CPhi(i,:) = cmat*(phi^(i-1));      % multiples state in free response
  end
%
    Sf = S';  % the default for the above for loop creates a row vector, but Sf is a column vector
%
    rspvec = ones(P,1)*rsp
%  
    free_resp = CPhi*x0 + Sf*uinit;   % should be the zero vector
%
    Efree   = rspvec - free_resp;
%
    deluf    = inv(Sf'*Sf)*Sf'*Efree;
    ufirst   = uinit + deluf;
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  ydis(1) = cmat*xdis(:,1);       % first measured output
%
  for k = 1:P;
      u(k) = ufirst;
      [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(4000)
  plot(tplot,xplot)
%
  [tt,uu] = stairs(time,u);
  figure(4001)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('M = 1, P = 20')
  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;
%
%------------------ not requested in homework assignment -----------------
%-------------                                              --------------
%                 plot objective function as a fcn of delu
%                               for M = 1
%
   M = 1;
%
%  vary delu from 0.5 to 2
%
   deluM1 = 0.5:0.01:2;        % delta u for M = 1
  for i = 1:length(deluM1);
   J(i) = Efree'*Efree + deluM1(i)*Sf'*Sf*deluM1(i) - 2*deluM1(i)*Sf'*Efree;
  end
%
  figure(4003)
  plot(deluM1,J)
  grid minor
  title('Objective function, M = 1, P = 20')
  xlabel('delu')
  ylabel('J')
%--------------------------------------------------------------------------
%  "confirm" analytical result for P approaching infinity
%   let P = 300
%
   P = 300;    % multi-step-ahead control
%
  rsp = 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
      CPhi(i,:) = cmat*(phi^(i-1));      % multiples state in free response
  end
%
    Sf = S';  % the default for the above for loop creates a row vector, but Sf is a column vector
%
    rspvec = ones(P,1)*rsp
%  
    free_resp = CPhi*x0 + Sf*uinit;   % should be the zero vector
%
    Efree   = rspvec - free_resp;
%
    deluf    = inv(Sf'*Sf)*Sf'*Efree;
    ufirst   = uinit + deluf;
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  ydis(1) = cmat*xdis(:,1);       % first measured output
%
  for k = 1:P;
      u(k) = ufirst;
      [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(4004)
  plot(tplot,xplot)
%
  [tt,uu] = stairs(time,u);
  figure(4005)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('M = 1, P = 300')
  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;
%--------------------------------------------------------------------------
%  M = 5, P = 20
   P = 20;    % multi-step-ahead control
   M =  5;
%
   clear H; clear S; clear CPhi; clear u;
  rsp = 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
      CPhi(i,:) = cmat*(phi^(i-1));      % multiples state in free response
  end
%
    Sfvec = S';     % need column vector
%
    Sf = zeros(P,M);  % sets dimension of Sf matrix
%
    for j = 1:M;
    Sf(:,j) = [zeros(j-1,1);Sfvec(1:P-j+1)];
    end
%
    rspvec = ones(P,1)*rsp
%  
    free_resp = CPhi*x0 + Sfvec*uinit;   % should be the zero vector
%
    Efree   = rspvec - free_resp;
%
    deluf    = inv(Sf'*Sf)*Sf'*Efree
%
    delufvec = [deluf;zeros(P-M,1)];
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  ydis(1) = cmat*xdis(:,1);       % first measured output
%
  for k = 1:P;
      u(k) = uinit + sum(delufvec(1:k));
      [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(4008)
  plot(tplot,xplot)
%
  [tt,uu] = stairs(time,u);
  figure(4009)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('M = 5, 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;
%-------------------------------------------------------------------------
%  M = 2, P = 20
   P = 20;    % multi-step-ahead control
   M =  2;
%
   clear H; clear S; clear CPhi; clear u;
  rsp = 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
      CPhi(i,:) = cmat*(phi^(i-1));      % multiples state in free response
  end
%
    Sfvec = S';     % need column vector
%
    Sf = zeros(P,M);  % sets dimension of Sf matrix
%
    for j = 1:M;
    Sf(:,j) = [zeros(j-1,1);Sfvec(1:P-j+1)];
    end
%
    rspvec = ones(P,1)*rsp
%  
    free_resp = CPhi*x0 + Sfvec*uinit;   % should be the zero vector
%
    Efree   = rspvec - free_resp;
%
    deluf    = inv(Sf'*Sf)*Sf'*Efree
%
    delufvec = [deluf;zeros(P-M,1)];
%
  tplot = [];
  yplot = [];
  xplot = [];
%
  ydis(1) = cmat*xdis(:,1);       % first measured output
%
  for k = 1:P;
      u(k) = uinit + sum(delufvec(1:k));
      [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(4010)
  plot(tplot,xplot)
%
  [tt,uu] = stairs(time,u);
  figure(4011)
  subplot(2,1,1)
  plot(tplot,yplot)
  xlabel('t, min')
  ylabel('y')
  title('M = 2, P = 20')
  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;
  J2 = Efree'*Efree + deluf'*Sf'*Sf*deluf - 2*deluf'*Sf'*Efree
%-------------------------------------------------------------------------
  figure(4012)
  subplot(2,1,1)
  plot(tplot1,yplot1,tplot4,yplot4,tplot3,yplot3)
  title('Comparison of Control Horizon, for P = 20')
  legend('M = 1','M = 2','M = 5')
  xlabel('t, min')
  ylabel('y')
  subplot(2,1,2)
  plot(tt1,uu1,tt4,uu4,tt3,uu3)
  legend('M = 1','M = 2','M = 5')
  xlabel('t, min')
  ylabel('u')
%----------------------------------------------------------------------
   delu1M2 = -50:1:50;        % delta u1 for M = 2
   delu2M2 = -50:1:50;
   
  for i = 1:length(delu1M2);
      for j = 1:length(delu2M2);
          deluM2 = [delu1M2(i);delu2M2(j)];
   JM2(i,j) = Efree'*Efree + deluM2'*Sf'*Sf*deluM2 - 2*deluM2'*Sf'*Efree;
      end
  end
  figure(4013)
  contour(delu1M2,delu2M2,JM2,[50 500 1000 5000 10000])
  hold on
  plot(deluf(1),deluf(2),'+')
  figure(4014)
  plot3(delu1M2,delu2M2,JM2)
%-----------------------------------------------------------
%  finer discretization for contour plot -------------------
   delu1M2a = -5:0.5:15;        % delta u1 for M = 2
   delu2M2a = -15:0.5:5;
   
  for i = 1:length(delu1M2a);
      for j = 1:length(delu2M2a);
          deluM2a = [delu1M2a(i);delu2M2a(j)];
   JM2a(i,j) = Efree'*Efree + deluM2a'*Sf'*Sf*deluM2a - 2*deluM2a'*Sf'*Efree;
      end
  end
  figure(4015)
  contour(delu1M2a,delu2M2a,JM2a,[7.5 50 200 500])
  title('contour plot for M = 2, P = 20')
  hold on
  plot(deluf(1),deluf(2),'+')
  xlabel('del uk')
  ylabel('del uk+1')
    figure(4016)
  contourf(delu1M2a,delu2M2a,JM2a,[7.5 50 200 500])
  hold on
  plot(deluf(1),deluf(2),'+')