%
%  This routine implements a dynamic programming algorithm
%
%  Nx = the number of state values between (and including) xmin and xmax
%  Nu = the number of control values between (and including) umin and umax
%  Nt = the number of time steps between (and including ) tmin and tmax
%  J = 0.5 (y-r)*Qf*(r-y) + 0.5 sum ((y-r)*Q*(y-r) + u*R*u)
%  Continuous time state variable model is x' = Ax+Bu  y = Cx
%  a simple discrete-time model will be constructed in the program
%     x(k+1) = G x(k) + H u(k), y(k) = C x(k) ... G = (I+Adt), H = B*dt
%
function dynamic_tracker(Nt,Nx,Nu,Q,R,Qf,A,B,C);
%
%  tmin and tmax determine the range of times 
%
  tmin = 0;
  tmax = 2;
%
%  determine the actual times
%
   time = linspace(tmin,tmax,Nt);
%
% determine the signal we are to track
%
  r = ones(1,length(time));
%  r = cos(2*pi*time);
% r = time;
%
%  umin and umax are the minimum and maximum values of u
%
  umin = -5.0;
  umax = 5.0;
%  
%  get the allowed control inputs
%
   u_allowed = linspace(umin,umax,Nu)';
%
%  xmim and xmax are the maximum and minimum values of x
%
  xmin = -3.0;
  xmax = 3.0;
%
%  get the states
%
   x = linspace(xmin,xmax,Nx);
   x = flipud(x');
%
%  initialize the optimal cost to go
%
   J_star = zeros(Nx,Nt);
%
%  initialize the optimal control signal
%
   u_star= zeros(Nx,Nt);
%
%  determine the cost at the final time
%
   J_star(:,Nt) = 0.5*Qf*(C*x - r(Nt)*ones(length(x),1)).*(C*x - r(Nt)*ones(length(x),1));   
%
%  get the discretized state model. We assume dt is small
%
  dt = time(2)-time(1);
  G = eye(size(A))+A*dt;
  H = B*dt;
%
%  now go through each time
%
   for kt=Nt-1:-1:1
      %
      %  now go through each state
      %
        for jx=1:Nx
            %
            %  determine the next state for all allowed control inputs
            %
            s = all_next_states(x(jx),u_allowed,time,kt,xmin,xmax,G,H);
            %
            %  determine the cost for each of the allowed control inputs
            %
            J = cost_for_input(J_star,x,s,u_allowed,time,kt,Q,R,C,r);
            %
            %  determine which input gives the minimum cost to go
            %
            [Jmin,index] = min(J);
            %
            %  update the cost to go and the control signal for this state
            %
            u_star(jx,kt) = u_allowed(index);
            J_star(jx,kt) = Jmin;
        end;
    end;
 %
 %  At this point we should have the optimal cost to go and optimal control
 %  signal for each time and each state
 %
 %  Now determine the optimal path from any initial state
 %
 %  Set the initial state
 %
   x0 = 0.5;
 %  x0 = 0;
 %
   xx(Nt) = 0;
   uu(Nt) = 0;
 %
   xx(1) = x0;
 %
   J_final = 0;
 %
   for kt=1:Nt-1
 %
     uu(kt) = interp1(x,u_star(:,kt),xx(kt),'linear','extrap');
     xx(kt+1) = G*xx(kt)+H*uu(kt);
     J_final = J_final+0.5*(C*xx(kt)-r(kt))*Q*(C*xx(kt)-r(kt)) + 0.5*uu(kt)*R*uu(kt); 
 %
   end;  
 %
   J_final = J_final + 0.5*(C*xx(Nt)-r(Nt))*Qf*(C*xx(Nt)-r(Nt));
   [xtracker,utracker,Jtracker] = sim_tracker(Qf,Q,R,x0,Nt,G,H,C,r);
 %
 %  Now plot the output
 %
   figure
   orient landscape
   subplot(2,1,1); plot(time,r,'.-',time,C*xx,'-',time,C*xtracker,':'); grid; legend('Input','Dynamic','Tracker');
                   title([{[' J_{DP} = ' num2str(J_final,3) ', J_{Tr} = ' num2str(Jtracker,3) ', x_0 = ' num2str(x0)]} 
                          {['N_t = ' num2str(Nt) ', N_x = ' num2str(Nx) ', N_u = ' num2str(Nu) ', Q = ' num2str(Q) ...
                                 ', R = ' num2str(R) ', Q_f = ' num2str(Qf)]} ]);
                   xlabel('Time (sec)'); ylabel('x(t)');
%
   subplot(2,1,2); plot(time,uu,'-',time,utracker,':'); grid; legend('Dynamic','Tracker');
                   xlabel('Time (sec)'); ylabel('u_{opt}(t)');
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %  
 %  the computation for all possible next states goes here
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 % 
 function s = all_next_states(x,u,time,kt,xmin,xmax,G,H);
 %
 %  compute all possible next states
 %
   s = (G*x)*ones(length(u),1)+H*u;
 %  
 %  return NaN if the input gives a state out of bounds
 %
   for kk = 1:length(s)
       if ((s(kk) < xmin) | (s(kk) > xmax)) s(kk) = NaN; end;
   end;
 %  
  return;
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %  
 %  the cost for the current input/state goes here
 %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 function  J = cost_for_input(J_star,x,s,u_allowed,time,kt,Q,R,C,r);
 %
   J = zeros(length(u_allowed),1);
   for ku = 1:length(u_allowed)
       if (~isnan(s(ku)))
       %
       %  get the cost based on where we end up
       %
       Jc = interp1(x,J_star(:,kt+1),s(ku),'linear','extrap');
       %
       %  get the cost of this input
       %
       Ju = 0.5*u_allowed(ku)*R*u_allowed(ku)+0.5*(C*s(ku)-r(kt))*Q*(C*s(ku)-r(kt));
       %
       %  get the total cost to go for this input
       %
       J(ku) = Jc+Ju;
       %
       else
           J(ku) = NaN;
       end;
   end;  
 return;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  This routine implements an optimal tracker and then simulates
%  the system. 
%
%  Inputs: Qf, Q, R matrices/vectors
%          x0, the initial state
%          Nt, the number of time steps
%          A,B,C the state model
%           r = the signal to be tracked
%  Outputs: x, the state for the optimal path
%           u, the optimal control signal
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
  function [xtracker,utracker,Jtracker] = sim_tracker(Qf,Q,R,x0,Nt,G,H,C,r);
%
% Compute the K's and S's
%
  S = zeros(Nt,1);
  V = zeros(Nt,1);
  K = zeros(Nt,1);
  uf = zeros(Nt,1);
  utracker = zeros(Nt,1);
  xtracker = zeros(Nt,1);
%
  S(Nt) = C*Qf*C;
  V(Nt) = C*Qf*r(Nt);
%
  for kt = Nt-1:-1:1
      Delta = inv(1+H*(1/R)*H*S(kt+1));
      V(kt) = [-G*S(kt+1)*Delta*H*(1/R)*H+G]*V(kt+1)+C*Q*r(kt);
      S(kt) = G*S(kt+1)*Delta*G+C*Q*C;
      K(kt) = inv(R+H*S(kt+1)*H)*H*S(kt+1)*G;
      uf(kt) = inv(R+H*S(kt+1)*H)*H*V(kt+1);
  end;
%
%  Now simulate the system
%
  xtracker(1) = x0;
  Jtracker = 0;
  for kt = 1:Nt-1
      utracker(kt) = uf(kt) - K(kt)*xtracker(kt);
      xtracker(kt+1) = G*xtracker(kt) + H*utracker(kt);
      Jtracker = Jtracker + 0.5*(C*xtracker(kt) - r(kt))*Q*(C*xtracker(kt)-r(kt))+0.5*(utracker(kt)*R*utracker(kt));
  end;
  Jtracker = Jtracker + 0.5*(C*xtracker(Nt)-r(Nt))*Qf*(C*xtracker(Nt)-r(Nt));
%
%  all done
%
  return