%
%  This function computes the controller for 
%  a linear quadratic problem
%
%    J = int( q(r-y)^2 + u^2) dt
%
%  Inputs:  Amp = amplitude of the step (in cm)
%           Gp = the plant transfer function
%           q = the weight on the deviation of output from input
%           final_time = final time to run the simulation for
%           filename = the name of the file containing the measured data
%
function quadratic(Amp,Gp,q,final_time,filename);
%
%  First find the values for D(s), D(-s), N(s) and N(-s)
%   where Gp(s) = N(s)/D(s)
%
%  first conpute D(s)*D(-s)
%
  [Gpnum,Gpden] = tfdata(Gp,'v');
  Dp = Gpden;
  Nden = length(Dp);
  entry = 1;
  scale = [entry];
  for i = 2:Nden
      entry = -entry;
      scale = [scale entry];
  end;
  Dm = Dp.*fliplr(scale);
  DD = conv(Dp,Dm);
%
%  Now compute N(s)*N(-s)
%
  Np = Gpnum;
  Nnum = length(Np);
  entry = 1;
  scale = [entry];
  for i = 2:Nnum
      entry = -entry;
      scale = [scale entry];
  end;
  Nm = Np.*fliplr(scale);
  NN = conv(Np,Nm);
%
%  Put it all together
%
  Q = DD + q*NN
%  
%  Now find the roots of this
%
  r = roots(Q);
%
%  Now we need to find the stable roots (negative real parts)
%
  stable = [];
  for i = 1:length(r)
      if (real(r(i)) < 0 ) 
          stable = [stable r(i)];
      end;
  end;
%
%  Now construct D0, the denominator of the transfer function
%
  D0 = poly(stable);
%
%  Now put it all together
%
  top = (q*Gpnum(end)/D0(end))*Gpnum;
  bot = D0;
  disp('The optimal closed loop transfer function ....');
  G0 = tf(top,bot)
%
%  Get the controller
%
%  Gc = G0/Gp(1-G0) = N0*D/(D0*N-N*N0)
%
  [numG0,denG0] = tfdata(G0,'v');
  [numGp,denGp] = tfdata(Gp,'v');
  top = conv(numG0,denGp);
  bot = conv(denG0,numGp)-conv(numGp,numG0);
  Gc = tf(top,bot);
%
%  Now we have to remove pole/zero cancellations
%
  [numGc,denGc] = tfdata(Gc,'v');
%
%  first determine the gain, so we can fix it later
%
  [z,gain] = zero(Gc);
%
%  now remove the poles and zeros that cancel
%
  z = roots(numGc);
  p = roots(denGc);
  p_index = [];
  z_index = [];
  tol = 1e-8;
  for i = 1:length(z)
      for k = 1:length(p)
          if ( abs(z(i)-p(k)) < tol )
              p_index = [p_index k];
              z_index = [z_index i];
          end;
      end;
  end;
%
%  Now construct the numerator and denominator
%
  p_final = [];
  for k=1:length(p)
     ok = 1;
     for i=1:length(p_index)
       if(k == p_index(i)) ok = 0; end;
     end;
     if (ok == 1) 
         p_final = [p_final p(k)];
     end;
  end;
  z_final = [];
  for k=1:length(z)
     ok = 1;
     for i=1:length(z_index)
       if(k == z_index(i)) ok = 0; end;
     end;
     if (ok == 1) 
         z_final = [z_final z(k)];
     end;
  end;
%
%  Now get the numerator and denominator polynomials
%
 top = poly(z_final);
 bot = poly(p_final);
%
%  any coefficient below a tolerance is assumed to be zero
%
  tol = 1e-8;
  level = tol * ones(1,length(top));
  test = abs(top) > level;
  top = test .* top;
  level = tol * ones(1,length(bot));
  test = abs(bot) > level;
  bot = test .* bot;
%
%  Now adjust the gain
%
  Gc = tf(top,bot);
  [z,newgain] = zero(Gc);
  Gc = Gc*gain/newgain;
%
  [numGc,denGc] = tfdata(Gc,'v');
%
%  Now scale the coefficients
%
  scale = 1/max(denGc);
  num = scale*numGc;
  den = scale*denGc;
  disp('The controller transfer function ...');
  Gc = tf(num,den)
%
%  Now print this out
%  
  fprintf('\n');
  fprintf(['S = (0 to N) ' num2str(fliplr(num),5) '\n']);
  fprintf(['R = (0 to N) ' num2str(fliplr(den),5) '\n']); 
  fprintf('\n');
  G1 = feedback(Gc*Gp,1);
%
%  Now simulate the system
%
  t = linspace(0,final_time,10000);
  u = Amp*ones(1,length(t));
  y = lsim(G1,u,t);
  y0 = lsim(G0,u,t);
%
%  plot the results
%
  if (~isempty(filename))
    orient landscape
    encoder = 1;
    data = importdata(filename);
    tdata = data(:,encoder+1);
    ydata = data(:,encoder+3)/2196;
    plot(t,y,':',tdata,ydata,'-'); grid;
    ymin = min(min(y),min(ydata));
    ymax = max(max(y),max(ydata));
    axis([0 final_time ymin ymax]);
    legend('Model y','Real System y',4);
    xlabel('Time (sec)');
    ylabel('Position (cm)');
    title(['Quadratic Performance Index, q = ' num2str(q)]);
  else    % no data to compare to
    orient landscape
    plot(t,y,'-',t,y0,':'); grid; legend('Estimated','True');
    title(['Quadratic Performance Index, q = ' num2str(q)]);
    ymin = min(y(:,1));
    ymax = max(y(:,1));
    axis([0 final_time ymin ymax]);
    xlabel('Time (sec)');
    ylabel('Position (cm)');
    legend('Model x_1',4);
  end;
%
%  Now spit out the coefficients for the ECP system
%
  fp = fopen('values.par','w');
  fprintf(fp,'[DYNFWD_C]\n');
%
%  get the numerator
%
  N =  length(num);
  temp = [zeros(1,8-N) num];
  S = fliplr(temp);
%
%  get the denominator
%
  N = length(den);
  temp = [zeros(1,8-N) den];
  R = fliplr(temp);
%
%  now write it out
%
  format long 
  fprintf(fp,'s0=%15.12e\n',S(1));
  fprintf(fp,'s1=%15.12e\n',S(2));
  fprintf(fp,'s2=%15.12e\n',S(3));
  fprintf(fp,'s3=%15.12e\n',S(4));
  fprintf(fp,'s4=%15.12e\n',S(5));
  fprintf(fp,'s5=%15.12e\n',S(6));
  fprintf(fp,'s6=%15.12e\n',S(7));
  fprintf(fp,'s7=%15.12e\n',S(8));
  fprintf(fp,'r0=%15.12e\n',R(1));
  fprintf(fp,'r1=%15.12e\n',R(2));
  fprintf(fp,'r2=%15.12e\n',R(3));
  fprintf(fp,'r3=%15.12e\n',R(4));
  fprintf(fp,'r4=%15.12e\n',R(5));
  fprintf(fp,'r5=%15.12e\n',R(6));
  fprintf(fp,'r6=%15.12e\n',R(7));
  fprintf(fp,'r7=%15.12e\n',R(8));
fclose(fp);
  return;