%
%  this routine attempts to optimize the parameters to fit a 
%  second order transfer function to measured frequency response
%  
%  Inputs: data = vector of frequency (in Hz) and Magnitude for x1 (theta1) and x2 (theta2) (not in dB)
%  The following are intial estimates
%             K2       = estimated static gain
%             wa       = estimated  first resonance of the second cart
%             za       = estimated first damping ratio
%             wb       = estimated second resonance of the second cart
%             zb       = estiamted second damping ratio
%             w1       = estimated natural frequency of the first cart
%             z1       = estimated damping ratio of the first cart
%             w2       = estimated natural frequency of the second cart
%             z2       = estimated damping ratio of the second cart
%
%  This version fixes wa, za, wb, zb and K2 after the frequency response of
%  cart2 is determined
%
function A = model_2carts(data,K2,wa,za,wb,zb,w1,z1,w2,z2);
%
%  Assign the global variable so we can use it in the fit function
%
global  MEASURED_FREQUENCY_RESPONSE
global PARAMETERS
%
  MEASURED_FREQUENCY_RESPONSE = data;
%
%==========================================================================
%
%  First we fit the data to the transfer function for the second cart (it has fewer parameters to
%  fit), then we use these final values to fit parameters to the transfer
%  function of the first cart
%
%==========================================================================
%
%  Set the initial conditions
%
  X0 = [ K2 wa za wb zb ];
%
%  set the options for the minimization routine
%
  options = optimset(@fminsearch);
  options = optimset(options,'Display','iter','MaxIter',2000,'MaxFunEvals',2000,'TolFun',1e-8,'TolX',1e-8);
%
%  do the minimization
%
  x = fminsearch(@fit2,X0,options);
%
  disp('... finished TF for X2, press any key to continue ...')
  pause;
%
%  Note: a negative zeta gives the same magnitude as a positive zeta
%  so we have to be sure all of the zeta's are positve
%
  K2 = x(1); 
  wa = x(2);
  za = abs(x(3));
  wb = x(4);
  zb = abs(x(5));
%
%  These values are now assumed to be fixed, save them
%
  PARAMETERS(2) = K2;
  PARAMETERS(3) = wa;
  PARAMETERS(4) = za;
  PARAMETERS(5) = wb;
  PARAMETERS(6) = zb;
%
%======================================================================
%
%  Now we fit the parameters to determine the transfer function of the
%  first cart
%
%======================================================================
%
%  Set the initial conditions
%
  X0 = [K2 w2 z2];
%
%  set the options for the minimization routine
%
  options = optimset(@fminsearch);
  options = optimset(options,'Display','iter','MaxIter',2000,'MaxFunEvals',2000,'TolFun',1e-8,'TolX',1e-8);
%
%  do the minimization
%
  x = fminsearch(@fit1,X0,options);
%
  disp('... finished TF for X1, press any key to continue ...')
  pause;
%
%  Note: a negative zeta gives the same magnitude as a positive zeta
%  so we have to be sure all of the zeta's are positve
%
  K1 = x(1); 
  w2 = x(2);
  z2 = abs(x(3));
%
%
%======================================================
%  Now lets determine the best value of w1 based on our
%  estimated transfer functions
%
%======================================================
%
  PARAMETERS(1) = K1;
  PARAMETERS(7) = w2;
  PARAMETERS(8) = z2;
%
%  Set the initial conditions
%
  X0 = [w1 z1];
%
%  set the options for the minimization routine
%
  options = optimset(@fminsearch);
  options = optimset(options,'Display','iter','MaxIter',2000,'MaxFunEvals',2000,'TolFun',1e-8,'TolX',1e-8);
%
%  do the minimization
%
  x = fminsearch(@fit3,X0,options);
%
%  only change the estimated values of zeta_1 if it 
%   is not too small. Otherwise, go with the initial estiamtes
%
  tol = z1/4; 
  if (abs(x(2)) > tol )
    w1 = x(1);
    z1 = abs(x(2));
  else
    fprintf('... program estimates for omega_1 and zeta_1 are %f and %e \n',x(1), abs(x(2)));
    fprintf('... we will use the estimtes omega_1 = %f and  zeta_1 = %f \n', x(1), z1/4);
    w1 = x(1);
    z1 = z1/4;
  end;
  PARAMETERS(9) = w1;
  PARAMETERS(10) = z1;
%
%=================================
% Now lets make the plots
%=================================
%
%  Construct the transfer function for the second cart
%
  G1 = tf(K2,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G = G1*G2;
%
%  extract the data
%
  f = data(:,1);
  M = data(:,3);
%
%  convert to dB and rad/sec
%
  MdB = 20*log10(M);  
  w = 2*pi*f;
%
  omega_low = min(w);
  omega_high = max(w);
  omega = logspace(log10(omega_low),log10(omega_high),1e3);
%
%  Now plot the two 
%
  [estimated_M,estimated_P,omega2] = bode(G,omega);
%
% Get the values for the estimated transfer functions
%
  eM = [];
  for i = 1:length(omega)
    eM = [eM estimated_M(1,1,i)];
  end;
  eMdB = 20*log10(eM);
%
%  Now plot the results
%
  figure;
  orient landscape
  semilogx(w,MdB,'d',omega,eMdB,'-');
  min_mag = min(min(eMdB),min(MdB));
  max_mag = max(max(eMdB),max(MdB));
  axis([floor(min(w)) floor(max(w))+2 min_mag max_mag ]);
  legend('Measured','Estimated',2); grid; ylabel('Magnitude (dB)'); xlabel('Frequency (rad/sec)')
  title(['K_2 = ' num2str(K2,6) ', \omega_a = ' num2str(wa,3) ', \zeta_a = ' num2str(za,3) ', \omega_b = ' num2str(wb,3) ', \zeta_b = ' num2str(zb,3)]);
%
%  Construct the transfer function for the first cart
%
  G1 = tf(K1,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G3 = tf([1/w2^2 2*z2/w2 1],1);
  G = G1*G2*G3;
%
%  extract the data
%
  f = data(:,1);
  M = data(:,2);
%
%  convert to dB and rad/sec
%
  MdB = 20*log10(M);  
  w = 2*pi*f;
%
  omega_low = min(w);
  omega_high = max(w);
  omega = logspace(log10(omega_low),log10(omega_high),1e3);
%
%  Now plot the two 
%
  [estimated_M,estimated_P,omega2] = bode(G,omega);
%
% Get the values for the estimated transfer functions
%
  eM = [];
  for i = 1:length(omega)
    eM = [eM estimated_M(1,1,i)];
  end;
  eMdB = 20*log10(eM);
  figure
  orient landscape
  semilogx(w,MdB,'d',omega,eMdB,'-');
  min_mag = min(min(eMdB),min(MdB));
  max_mag = max(max(eMdB),max(MdB));
  axis([floor(min(w)) floor(max(w))+2 min_mag max_mag ]);
  legend('Measured','Estimated',2); grid; ylabel('Magnitude (dB)'); xlabel('Frequency (rad/sec)')
  title([' K_1 = ' num2str(K1,6) ', \omega_a = ' num2str(wa,3) ', \zeta_a = ' num2str(za,3) ', \omega_b = ' num2str(wb,3) ...
          ', \zeta_b = ' num2str(zb,3) ', \omega_1 = ' num2str(w1,3) ', \zeta_1 = ' num2str(z1,3) ', \omega_2 = ' num2str(w2,3) ', \zeta_2 = ' num2str(z2,3)]);
%
%=======================================================================
%
%  Now we construc the A matrix with the values we've found
%
  A = zeros(4,4);
  A(1,2) = 1;
  A(3,4) = 1;
  A(2,1) = -w1^2;
  A(2,2) = -2*z1*w1;
  A(4,3) = -w2^2;
  A(4,4) = -2*z2*w2;
  A(4,1) = (K2/K1)*w2^2;
  A(2,3) = (w1^2*w2^2-wa^2*wb^2)/(A(4,1));
  B = zeros(4,1);
  B(2) = K2*wa^2*wb^2/A(4,1);
  C = [0 0 1 0];
  D = [0]; 
%
  save state_model_2dof A B C D
%   
return;
%
%=========================================================================
%
%  This function is what is used to determine the fit of the transfer
%  function to the measured frequency response data for the second cart
%  (x2)
%
function J = fit2(x)
%
global MEASURED_FREQUENCY_RESPONSE
%
%  extract the data
%
  f = MEASURED_FREQUENCY_RESPONSE(:,1);
  M = MEASURED_FREQUENCY_RESPONSE(:,3);
%
%  convert to dB and rad/sec
%
  MdB = 20*log10(M);  
  w = 2*pi*f;
%
% get the parameters
% 
  K2 = x(1); 
  wa = x(2);
  za = x(3);
  wb = x(4);
  zb = x(5);
%
%  Construct the transfer function
%
  G1 = tf(K2,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G = G1*G2;
%
%  determine the Magnitude of G at the specified frequencies
%
    [estimated_M,estimated_P,omega2] = bode(G,w);
%
%   Extract from the horrible way Matlab stores these
%    and convert to dB
%
    eM = reshape(estimated_M, length(estimated_M),1);
    eMdB = 20*log10(eM);
 %
 %  Determine the squared error (in dB)
 %
   J = norm(eMdB - MdB);
 %
 %  all done
 %
   return;
%
%  =========================================
%
%  This function is what is used to determine the fit of the transfer
%  function to the measured frequency response data for the first cart
%  (x1) assuming wa, za, wb, zb are fixed
%
function J = fit1(x)
%
global MEASURED_FREQUENCY_RESPONSE PARAMETERS
%
%  extract the data
%
  f = MEASURED_FREQUENCY_RESPONSE(:,1);
  M = MEASURED_FREQUENCY_RESPONSE(:,2);
  wa = PARAMETERS(3);
  za = PARAMETERS(4);
  wb = PARAMETERS(5);
  zb = PARAMETERS(6);
%
%  convert to dB and rad/sec
%
  MdB = 20*log10(M);  
  w = 2*pi*f;
%
% get the parameters
% 
  K1 = x(1); 
  w2 = x(2);
  z2 = abs(x(3));
%
%  Construct the transfer function
%
  G1 = tf(K1,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G3 = tf([1/w2^2 2*z2/w2 1],1);
  G = G1*G2*G3;
%
%  determine the Magnitude of G at the specified frequencies
%
    [estimated_M,estimated_P,omega2] = bode(G,w);
%
%   Extract from the horrible way Matlab stores these
%    and convert to dB
%
    eM = reshape(estimated_M, length(estimated_M),1);
    eMdB = 20*log10(eM);
 %
 %  Determine the squared error (in dB)
 %
   J = norm(eMdB - MdB);
 %
 %  all done
 %
   return;
%
%  =========================================
%
%  This function is what is used to determine best values of
%  w1 and z1 based on the transfer functions for the first two carts
%
function J = fit3(x)
%
global PARAMETERS
%
%  extract the data
%
  wa = PARAMETERS(3);
  za = PARAMETERS(4);
  wb = PARAMETERS(5);
  zb = PARAMETERS(6);
  z2 = PARAMETERS(8);
  w2 = PARAMETERS(7);
  w1 = x(1);
  z1 = abs(x(2));
  %
  %  The following need to be scaled, or e1 dominates!
  %
  e3 = 1-(2*z1*w1+2*z2*w2)/(2*za*wa+2*zb*wb);
  e2 = 1 - (w1^2+w2^2+(2*z1*w1)*(2*z2*w2))/(wa^2+wb^2+(2*za*wa)*(2*zb*wb));
  e1 = 1 - (w1^2*(2*z2*w2)+w2^2*(2*z1*w1))/(wa^2*(2*zb*wb)+wb^2*(2*za*wa));
%
%  Compute the squared error
%
  J = e1^2+e2^2+e3^2;
%
%  all done
%
  return;