%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  This routine determines the value of S and v
%  for the optimal digital tracker (with soft constraints)
%
function [K, uf, K_ss, uf_ss] = optimal_tracker(G,H,C,Q,R,Qf,r)
%
%  get the length of the vectors
%
  N = length(r);
  temp = C'*Qf*C;
  [mS,nS] = size(temp);
  temp = C'*Qf*r(N);
  [mv,nv]=size(temp);
%
%  initialize the structures
%
  for i = 1:N
   S(i).a = zeros(mS,nS);
   v(i).a = zeros(mv,nv);
  end;
  S(N).a = C'*Qf*C;
  v(N).a = C'*Qf*r(N);
  GG = H*inv(R)*H'; 
  FF = C'*Q*C;
  [n,n] = size(GG*S(N).a);
  In = eye(n,n);
  %
  %  now go backward in time
  %
  for k=N-1:-1:1
     Delta = inv(In+GG*S(k+1).a);
     S(k).a = G'*S(k+1).a*Delta*G+FF;
     v(k).a = (-G'*S(k+1).a*Delta*GG+G')*v(k+1).a+C'*Q*r(k);
  end;
%
%  Now compute the state feedback gain K, and the
%  feedforward control uf
%
  N=length(r)-1;
  for k=1:N
     Delta = inv(R+H'*S(k+1).a*H);
     K(k).a =  Delta*H'*S(k+1).a*G;
     uf(k).a = Delta*H'*v(k+1).a;
 %
 %  compute the values for constant k
 %
     Delta_z = inv(R+H'*S(1).a*H);
     K_ss = Delta_z*H'*S(1).a*G;
     uf_ss(k).a = Delta_z*H'*v(k+1).a;
  end;
  return;
     
