%
function continuous_tracker(tf, Qf, Q, R);
%
%  the state variable description
%
  A = [1 1; 2 1];
  B = [1; 0];
  C = [1 0];
  load bobs_1dof_model205
%  load bobs_2dof_model205
%  C = [0 0 1 0];
%
%  Determine the values of the signal we are trying to track at
%  the times determined by Tr
%
  Tr = linspace(0,tf,1000);
  r = (pi/30)*sin(2*pi*Tr); 
%  r = Tr;
%
%  we also need time reversed versions of these
%
  r_rev = flipud(r);
  tau_r = tf-flipud(Tr);
%
%  first solve for time reversed values of S and TS 
%
  [tau_S S_rev] = time_varying_riccati(tf, A, B, C, Qf, Q, R);
% 
%  Now reverse the time for S
%
  S = flipud(S_rev);
  TS = tf - flipud(tau_S); 
%
%  now solve for v 
%
  [TV V] = solve_for_v(tf, A, B, C, Qf, Q, R, r_rev, tau_r, S_rev, tau_S);
%
%  Now simulate the system with the optimal control
%
  X0 = [0;0];
  [TX X] = ode45( @state_eqn, [0 tf], X0, [], A, B, C, R, V, TV, S, TS);
%
  y = C*X';
%
%  determine the optimal control signal and feedback gain
%
  for k=1:length(y)-1;
      t = TX(k);
      TU(k) = t;
%      
      Si = interp1(TS,S,t);
      Vi = interp1(TV,V,t);
%
%     Convert S and V from a vectors to a matrices
%
      Sm = reshape(Si, size(A));
      Vm = reshape(Vi, size(C'));
%
      K(k,:) = inv(R)*B'*Sm;
      u(k) = -K(k,:)*X(k,:)' + inv(R)*B'*Vm;
  end;
%  
%  plot the results
%
  figure;
  orient landscape
  plot(TX,y,'-',Tr,r,'--'); grid; xlabel('Time (sec)'); legend('y(t)','r(t)');
  title(['Q_f = ' num2str(Qf) ', Q = ' num2str(Q) ', R = ' num2str(R)]);
%
  figure;
  orient tall
% 
  subplot(4,1,1); plot(TX,y,'-',Tr,r,'--'); grid; legend('y(t)','r(t)');
  title(['Q_f = ' num2str(Qf) ', Q = ' num2str(Q) ', R = ' num2str(R)]);
% 
  subplot(4,1,2); plot(TU,u,'-'); grid; ylabel('u(t)');
%
  subplot(4,1,3); plot(TU,K); grid; ylabel('Feedback Gains');
%
  subplot(4,1,4); plot(TV,V); grid; ylabel('v(t)'); xlabel('Time (sec)');
%
%  all done
%
  return;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function [TV V] = solve_for_v(tf, A, B, C, Qf, Q, R, r, Tr, S, TS);
%
%  This equation is solved backwards in time
%
%  get the initial condition
%
  W0 = C'*Qf*r(end);
%
%  solve the equation backwards in time
%
  [tau W] = ode45( @m_aux, [0 tf], W0, [], A, B, C, Q, R, r, Tr, S, TS);
%
%  reverse the time for v
%
  V = flipud(W);
  TV = tf-flipud(tau);
%
%  all done
%
  return;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Solve the time-varying Riccati equation
%
% Note that Z is returned as an array, it needs to be reshaped
% before being used
%
% TZ contains the sample times for the Z array
%
%  Both of the returned values are in time-reversed order!
%
function [tau Z] = time_varying_riccati(tf, A, B, C, Qf, Q, R);
%
%  the general form of the equation we will solve is
%
%  dZ/dt = AZ'+ZA-Z*B*inv(R)*B'*Z+C'QC
% 
% where Z(t) = S(tf-t), so Z0 = S(tf)
%
%  this equation is generally solved backwards in time
%
%  set the initial condition
%
   Z0 = C'*Qf*C;
%
% Now solve
%
  [tau Z] = ode45( @mRiccati, [0 tf], Z0, [], A, B, C, Q, R);
%
%  all done
%
  return;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function dZdtau = mRiccati( tau, Z, A, B, C, Q, R );
  Z = reshape(Z, size(A));      % convert from "n^2" by 1 to "n" by "n"
  dZdtau = A.'*Z+Z*A-Z*B*inv(R)*B.'*Z+C'*Q*C; % Determine derivative
  dZdtau = dZdtau(:);               % make a column
  return;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function dWdtau = m_aux( tau, W, A, B, C, Q, R, r_rev, tau_r, S_rev, tau_S)
%
  W = reshape(W, size(C'));
%
%  interpolate to get the values of S and r for this time
%
  Si = interp1(tau_S,S_rev,tau);
  ri = interp1(tau_r,r_rev,tau);
%
%  Convert S from a vector to a matrix
%
  Sm = reshape(Si, size(A));
%  
  dWdtau = (A'-Sm*B*inv(R)*B')*W+C'*Q*ri;
  dWdtau = dWdtau(:);
  return;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
function dXdt = state_eqn( t, X, A, B, C, R, V, TV, S, TS)     
%
%  interpolate to get the values of S and V for this time
%
  Si = interp1(TS,S,t);
  Vi = interp1(TV,V,t);
%
%  Convert S and V from a vectors to a matrices
%
  Sm = reshape(Si, size(A));
  Vm = reshape(Vi, size(C'));
%
  dXdt = (A-B*inv(R)*B'*Sm)*X + B*inv(R)*B'*Vm;
  return; 
  