  function [Sf,Kmat,c_phi,Sfd,Wumat,Wymat] = KmatQSS(p,m,weightu,cd_mod,phi_mod,...
            gamma_mod,nstates,gammad_mod,weighty)
%
% b.w. bequette
% 14 Aug 01 - revised 16 Aug 01
% 19 Oct 02 - for use with vdvkfmpc4.m and ivivmpc.m
% 25 Oct 02 - revised to make dlength possibly less than p (prediction horizon)
% 22 Nov 03 - revised for Mexico City presentation (removed dlength)
% 26 Nov 03 - added weighting for outputs, weighty
%             called by SSmpc.m
% 30 Nov 03 - use for constrained State Space MPC
%  3 Dec 03 - left inverse (\) for unconstrained solution
% 
% generates dynamic matrix and feedback gain matrix
% Sf   = Dynamic Matrix for future control moves (forced)
% Kmat = Feedback gain for unconstrained solution (multiplies unforced
%          predicted error
% c_phi= Matrix multiplying currect state for unforced contribution
% Sfd  = Dynamic Matrix for disturbance
%
% p          = prediction horizon
% m          = control horizon
% weightu    = control move weighting in objective function
% cd_mod     = discrete output measurement matrix
% phi_mod    = discrete state matrix
% gamma_mod  = discrete control matrix
% nstates    = number of states
% gammad_mod = discrete disturbance matrix
%     x(k+1) = phi_mod*x(k) + gamma_mod*u(k) + gammad_mod*d(k)
%     y(k)   = cd_mod*x(k)
%
% first, find the step response matrix for manipulated inputs
    ny = size(cd_mod,1);
	nu = size(gamma_mod,2);
	nd_est = size(gammad_mod,2);
%
     Smat(:,:,1) = cd_mod*gamma_mod;
   for i = 2:p;
     Smat(:,:,i) = Smat(:,:,i-1)+cd_mod*phi_mod^(i-1)*gamma_mod;
   end
%
      dum1 = zeros(ny,nu);
	  dum2 = zeros(ny,nd_est);
% now, find the step response matrix for disturbances
      Smatd(:,:,1) = cd_mod*gammad_mod;
   for i = 2:p;
     Smatd(:,:,i) = Smatd(:,:,i-1)+cd_mod*phi_mod^(i-1)*gammad_mod;
   end
% Now, find the dynamic matrix for control moves
   for i = 1:p;
   for j = 1:m;
	 nrowbeg = (i-1)*ny+1;
	 nrowend = i*ny;
	 ncolbeg = (j-1)*nu+1;
	 ncolend = j*nu;
	 if i >= j;
	 Sf(nrowbeg:nrowend,ncolbeg:ncolend) = Smat(:,:,i-j+1);
     else
	 Sf(nrowbeg:nrowend,ncolbeg:ncolend) = dum1;
	 end
   end
   end
% Now, find the dynamic matrix for disturbances
   for i = 1:p;
   for j = 1:1;
	 nrowbeg = (i-1)*ny+1;
	 nrowend = i*ny;
	 ncolbeg = (j-1)*nd_est+1;
	 ncolend = j*nd_est;
	 if i >= j;
	 Sfd(nrowbeg:nrowend,ncolbeg:ncolend) = Smatd(:,:,i-j+1);
     else
	 Sfd(nrowbeg:nrowend,ncolbeg:ncolend) = dum2;
	 end
   end
   end
% 
%  control weighting matrix
   dum3 = zeros(nu,nu);
   for i = 1:m;
   for j = 1:m;
	 nrowbeg = (i-1)*nu+1;
	 nrowend = i*nu;
	 ncolbeg = (j-1)*nu+1;
	 ncolend = j*nu;
	 if i == j;
	 Wumat(nrowbeg:nrowend,ncolbeg:ncolend) = weightu;
     else
	 Wumat(nrowbeg:nrowend,ncolbeg:ncolend) = dum3;
	 end
   end
   end
%
%  output weighting matrix
   dum4 = zeros(ny,ny);
   for i = 1:p;
   for j = 1:p;
	 nrowbeg = (i-1)*ny+1;
	 nrowend = i*ny;
	 ncolbeg = (j-1)*ny+1;
	 ncolend = j*ny;
	 if i == j;
	 Wymat(nrowbeg:nrowend,ncolbeg:ncolend) = weighty;
     else
	 Wymat(nrowbeg:nrowend,ncolbeg:ncolend) = dum4;
	 end
   end
   end
%
%    Wymat
%    Wumat
% 
% find the feedback gain matrix, Kmat
%    using left inverse (slash), rather than direct
%    matrix inverse
%
% Kmat = inv(Sf'*Wymat*Sf + Weightmat)*Sf'*Wymat;
  Kmat = (Sf'*Wymat*Sf + Wumat)\(Sf'*Wymat);
% 
% find c*phi matrix
    c_phi = cd_mod*phi_mod;
  for i = 2:p;
	c_phi = [c_phi;cd_mod*phi_mod^i];
  end
