% QSSmpcNLPlant.m
% 22 Nov 03 - revised for presentations in Mexico City
%             unconstrained solution
%             use a driver file (such as r_FOmpc.m) to run simulations 
%                   and make plots
% 26 Nov 03 - added weighting on outputs weighty
% 30 Nov 03 - revised for constrained solution
% 18 Nov 07 - adding comments for documentation for MPC 07 course
% 18 Jan 09 - revised Kalman gain calculation: was using the wrong
%               argument for our formulation!
% 18 Nov 09 - revised to handle nonlinear plant ODEs
%             need to define planteqns in the calling file
%             for example, planteqns = 'ode3tnk'
% 17 Nov 11 - additional documentation
%
% -------------------------------------------------------------------------
% state space mpc w/kf simulation
%       can select DMC estimate (isim = 1), or KF (isim = 2)
%
% all variables are in deviation form
% calls qSSmpccalc.m and KmatQSS.m
%
% ---------------------------------------
% clear variables from previous simulations
  clear x_a_old; clear x_a_new; clear dhat; clear xmod; clear ymod; clear uinit; clear du;
  clear u; clear x; clear y; clear ymeas; clear dinput;
%
% the following are arrays for plotting, generated more frequently than the
% discrete sample times
  tplot = [];
  yplot = [];
  xplot = [];
%
  t = 0:delt:tfinal;       % time vector
  kfinal = length(t);      % number of time intervals
  ksp = fix(timesp/delt);  % time step for setpoint change
  r = [zeros(ny,ksp) ysp*ones(1,kfinal-ksp)]; % setpoint vector
  kdis = fix(timedis/delt);% time step for disturbance change
%
  for i = 1:kfinal;
    if i > kdis;
    dinput(:,i) = dist;   
    else
    dinput(:,i) = zeros(size(dist));
    end
  end
%
% ----------------------------------------
%     Additive output (DMC) vs. KF for plant-model mismatch compensation
%              isim = 1 (DMC), otherwise (KF)
  if isim == 1;
	  phi_a   = [phi_mod zeros(nstates,ny);zeros(ny,nstates) eye(ny)];
	  gamma_a = [gamma_mod;zeros(ny,nu)];
	  c_a     = [cd_mod eye(ny,ny)];
	  L = [zeros(nstates,ny);eye(ny)]; % Kalman gain matrix
  else
%     input disturbance with measurement noise
%     Q = input('Enter Q/R ratio')
%	  R = eye(ny);
      phi_a   = [phi_mod gammad_mod;zeros(nd_est,nstates) eye(nd_est)];
	  gamma_a = [gamma_mod;zeros(nd_est,nu)];
	  c_a     = [cd_mod zeros(ny,nd_est)];
      gamma_w = [zeros(nstates,nd_est);eye(nd_est)];
      aug_sys = ss(phi_a,[gamma_a gamma_w],c_a,0,-1);
%     [kest,L,P] = kalman(aug_sys,Q,R); % WRONG, need to use:
      [kest,Lwrong,P,L] = kalman(aug_sys,Q,R);
  end
% ------------------------------------------
% 
% generate dynamic matrix and control matrix
%
  [Sf,Kmat,c_phi,Sfd,Wumat,Wymat] = KmatQSS(p,m,weightu,cd_mod,phi_mod,...
                                         gamma_mod,nstates,gammad_mod,weighty);
%
% ------------------------------------------
% plant initial conditions (could perturb from 0 to be different than initial model)
%       the next three lines could be removed and put into the driver file
  xinit = zeros(nstates_p,1);
  uinit = zeros(nu,1);
  yinit = zeros(noutputs,1);
  x(:,1)  = xinit;
  y(:,1)  = yinit;
%
  NPTS = tfinal/delt + 1;
  meas_noise = zeros(ny,NPTS);
  unit_noise = randn(ny,NPTS);
  for i = 1:ny;
    meas_noise(i,:) = noisemag(i)*unit_noise(i,:);
  end
%
  ymeas(:,1)  = yinit + meas_noise(:,1);
% 
% initialize model (again, could be put into the driver file)
  xmod(:,1) = zeros(nstates,1);
  ymod(:,1) = cd_mod*xmod(:,1);
% 
  if isim == 2                       % KF plant-model compensation
% 
    xa0 = [xmod(:,1);zeros(nd_est,1)]; % augmented state i.c.
    dhat0 = zeros(nd_est,1);
  else                               % additive output assumption
%
    xa0 = [xmod(:,1);zeros(ny,1)];
    dhat0 = zeros(ny,1);
  end
  x_a_old(:,1) = xa0;
  x_a_new(:,1) = xa0; 
  dhat(:,1) = dhat0;
%
% ------------------------------------------
% set-up is done, start simulations
%
 for k = 1:kfinal;
%
%      perform control calculation, delta u
   if k >1;
%
   du = qSSmpccalc(Sf,Sfd,c_phi,xmod(:,k),dhat(:,k),r(:,k),u(:,k-1),p,m,...
                    Wumat,Wymat,umin,umax,dumin,dumax,Kmat,isim,iqp);
    else  
%  
   du = qSSmpccalc(Sf,Sfd,c_phi,xmod(:,k),dhat(:,k),r(:,k),uinit,p,m,...
                    Wumat,Wymat,umin,umax,dumin,dumax,Kmat,isim,iqp);
   end
%
%     convert to actual applied control action, u
%
  if k > 1;
   u(:,k) = u(:,k-1)+du; % control input
  else
   u(:,k) = uinit + du;
  end
%
% plant equations, integrate over one sample time using ode45 and the 
%                planteqns function file
%
% stop at kfinal to keep correct vector lengths
%
 if k < kfinal
%
 [tdummy,xdummy] = ode45(planteqns,[t(k) t(k+1)],x(:,k),[],parvec,u(:,k),dinput(:,k));
%
 ndum = length(tdummy);
 x(:,k+1) = xdummy(ndum,:);             % plant state at discrete sample time
 y(:,k+1) = cplant*x(:,k+1);             % plant output
 tplot = [tplot;tdummy];
 yplot = [yplot;xdummy*cplant'];
 xplot = [xplot;xdummy];
%
 ymeas(:,k+1) = y(:,k+1) + meas_noise(:,k+1); % noisy measured output
%
% model state estimates using kalman filter
%
 x_a_old(:,k+1) = phi_a*x_a_old(:,k) + gamma_a*u(:,k);
 x_a_new(:,k+1) = x_a_old(:,k+1) + L*(ymeas(:,k+1)-c_a*x_a_old(:,k+1));
 x_a_old(:,k+1) = x_a_new(:,k+1);
 xmod(:,k+1)    = x_a_new(1:nstates,k+1); % model state estimate
 ymod(:,k+1)    = cd_mod*xmod(:,k+1);     % model output estimate
 dhat(:,k+1)    = x_a_new(nstates+1:nstates+length(dhat0),k+1); % disturbance estimate
   end;      % end of k < kfinal test 
%
   end;      % end of integration loop
%
% --------------------------------------------
% back to calling file and make plots
