%
%  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, x2, and x3 (not in dB)
%  The following are intial estimates
%             K3       = static gain
%             wa       = estimated  first resonance of the third cart
%             za       = estimated first damping ratio
%             wb       = estimated second resonance of the third cart
%             zb       = estiamted second damping ratio
%             wc       = estimated third resonance of the third cart
%             zc       = estimated third 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
%             w3       = estimated natural frequency of the third cart
%             z3       = estimated damping ratio of the third cart
%
function A = model_3carts(data,K3,wa,za,wb,zb,wc,zc,w1,z1,w2,z2,w3,z3);
%
%  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 third cart (it has fewer parameters to
%  fit), then we use these final values to fit parameters to the transfer
%  function of the first and second carts
%
%==========================================================================
%
%  Set the initial conditions
%
  X0 = [ K3 wa za wb zb wc zc ];
%
%  set the options for the minimization routine
%
  options = optimset(@fminsearch);
  options = optimset(options,'Display','iter','MaxIter',3000,'MaxFunEvals',3000,'TolFun',1e-8,'TolX',1e-8);
%
%  do the minimization
%
  x = fminsearch(@fit3,X0,options);
%
  disp('... finished TF for X3, 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
%
  K3 = x(1); 
  wa = x(2);
  za = abs(x(3));
  wb = x(4);
  zb = abs(x(5));
  wc = x(6);
  zc = abs(x(7));
%
%  These parameters are now assumed to be known and fixed
%
  PARAMETERS(3) = K3;
  PARAMETERS(4) = wa;
  PARAMETERS(5) = za;
  PARAMETERS(6) = wb;
  PARAMETERS(7) = zb;
  PARAMETERS(8) = wc;
  PARAMETERS(9) = zc;
%
%======================================================================
%
%  Now we fit the parameters to determine the transfer function of the
%  second cart, we do not change the estimates we already have from the
%  third cart (wa, za, wb, zb, wc, zc)
%
%======================================================================
%
%  Set the initial conditions
%
  X0 = [K3 w3 z3];
%
%  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); 
  w3 = x(2);
  z3 = abs(x(3));
%
%======================================================================
%
%  Now we fit the parameters to determine the transfer function of the
%  first cart
%
%======================================================================
%
%  Set the initial conditions and save the parameters we have found
%
  PARAMETERS(2) = K2;
  PARAMETERS(3) = K3;
  PARAMETERS(4) = wa;
  PARAMETERS(5) = za;
  PARAMETERS(6) = wb;
  PARAMETERS(7) = zb;
  PARAMETERS(8) = wc;
  PARAMETERS(9) = zc;

  X0 = [ K2 min([w1 w2 w3]) z2 max([w1 w2 w3]) z3];
%
%  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); 
  wx = x(2);
  zx = abs(x(3));
  wy = x(4);
  zy = abs(x(5));
%
%  Now store the parameters we have found
%
  PARAMETERS(1) = K1;
  PARAMETERS(10) = w3;
  PARAMETERS(11) = z3;
  PARAMETERS(12) = wx;
  PARAMETERS(13) = zx;
  PARAMETERS(14) = wy;
  PARAMETERS(15) = zy;
%
%  Now we want to estimate z2 and w2 from the measured data and the derived
%  formulas
%
  X0 = [w2 z2];
%
%  set the options for the minimization routine
%
  options = optimset(@fminsearch);
  options = optimset(options,'Display','iter','MaxIter',1000,'MaxFunEvals',1000,'TolFun',1e-8,'TolX',1e-8);
%
%  do the minimization
%
  x = fminsearch(@fitw2z2,X0,options);
%
  disp('... finished fit for omega2 and zeta2, press any key to continue ...')
  pause;
%  
  w2 = x(1);
  z2 = abs(x(2));
  PARAMETERS(16) = w2;
  PARAMETERS(17) = z2;
%
%  Now we want to estimate z1 and w1 from the measured data and the derived
%  formulas
%
  X0 = [w1 z1];
%
%  set the options for the minimization routine
%
  options = optimset(@fminsearch);
  options = optimset(options,'Display','iter','MaxIter',1000,'MaxFunEvals',1000,'TolFun',1e-8,'TolX',1e-8);
%
%  do the minimization
%
  x = fminsearch(@fitw1z1,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(18) = w1;
  PARAMETERS(19) = z1;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Now plot all of the results
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Construct the transfer function for X3
%
  G1 = tf(K3,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G3 = tf(1,[1/wc^2 2*zc/wc 1]);
  G = G1*G2*G3;
%
%  extract the data for the third cart
%
  f = data(:,1);
  M = data(:,4);
%
%  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_3 = ' num2str(K3,4) ', \omega_a = ' num2str(wa,3) ', \zeta_a = ' num2str(za,3) ', \omega_b = ' num2str(wb,3) ', \zeta_b = ' num2str(zb,3) ', \omega_c = ' num2str(wc,3) ', \zeta_c = ' num2str(zc,3)]);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Construct the transfer function for x2
%
  G1 = tf(K2,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G3 = tf(1,[1/wc^2 2*zc/wc 1]);
  G4 = tf([1/w3^2 2*z3/w3 1],1);
  G = G1*G2*G3*G4;
%
%  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);
  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,4) ', \omega_1 = ' num2str(w1,3) ', \zeta_1 = ' num2str(z1,3) ...
                                 ', \omega_2 = ',num2str(w2,3) ', \zeta_2 = ' num2str(z2,3) ...
                                 ', \omega_3 = ' num2str(w3,3) ', \zeta_3 = ' num2str(z3,3) ]);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Construct the transfer function for x1
%
  G1 = tf(K1,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G3 = tf(1,[1/wc^2 2*zc/wc 1]);
  G4 = tf([1/wx^2 2*zx/wx 1],1);
  G5 = tf([1/wy^2 2*zy/wy 1],1);
  G = G1*G2*G3*G4*G5;
%
%  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,4) ', \omega_x = ' num2str(wx,3) ', \zeta_x = ' num2str(zx,3) ...
                                 ', \omega_y = ' num2str(wy,3) ', \zeta_y = ' num2str(zy,3)]);
%
%=======================================================================
%
%  Now we construc the A matrix with the values we've found
%
  A = zeros(6,6);
  A(1,2) = 1;
  A(3,4) = 1;
  A(5,6) = 1;
  A(2,1) = -w1^2;
  A(2,2) = -2*z1*w1;
  A(4,3) = -w2^2;
  A(4,4) = -2*z2*w2;
  A(6,5) = -w3^2;
  A(6,6) = -2*z3*w3;
  A(6,3) = w3^2*K3/K2;
  A(4,5) = (w2^2*w3^2-wx^2*wy^2)/A(6,3);
  A(4,1) = K2*wx^2*wy^2/(K1*w3^2);
  A(2,3) = (w1^2*w2^2*w3^2 - wa^2*wb^2*wc^2 - A(4,5)*A(6,3)*w1^2)/(A(4,1)*w3^2);
  scale = wa^2*wb^2*wc^2/(A(4,1)*A(6,3));
 fprintf('Scaling Factor is %f \n', scale);
  
return;
%
%=========================================================================
%
%  This function is what is used to determine the fit of the transfer
%  function to the measured frequency response data for the third cart
%  (x2)
%
function J = fit3(x)
%
global MEASURED_FREQUENCY_RESPONSE
%
%  extract the data
%
  f = MEASURED_FREQUENCY_RESPONSE(:,1);
  M = MEASURED_FREQUENCY_RESPONSE(:,4);
%
%  convert to dB and rad/sec
%
  MdB = 20*log10(M);  
  w = 2*pi*f;
%
% get the parameters
% 
  K3 = x(1); 
  wa = x(2);
  za = abs(x(3));
  wb = x(4);
  zb = abs(x(5));
  wc = x(6);
  zc = abs(x(7));
%
%  Construct the transfer function
%
  G1 = tf(K3,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G3 = tf(1,[1/wc^2 2*zc/wc 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 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 PARAMETERS
%
%  extract the data
%
  f = MEASURED_FREQUENCY_RESPONSE(:,1);
  M = MEASURED_FREQUENCY_RESPONSE(:,3);
%
  wa = PARAMETERS(4);
  za = PARAMETERS(5);
  wb = PARAMETERS(6);
  zb = PARAMETERS(7);
  wc = PARAMETERS(8);
  zc = PARAMETERS(9);
%
%  convert to dB and rad/sec
%
  MdB = 20*log10(M);  
  w = 2*pi*f;
%
% get the parameters
% 
  K2 = x(1); 
  w3 = x(2);
  z3 = abs(x(3));
%
%  Construct the transfer function
%
  G1 = tf(K2,[1/wa^2 2*za/wa 1]);
  G2 = tf(1,[1/wb^2 2*zb/wb 1]);
  G3 = tf(1,[1/wc^2 2*zc/wc 1]);
  G4 = tf([1/w3^2 2*z3/w3 1],1);
  G = G1*G2*G3*G4;
%
%  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)
%
function J = fit1(x)
%
global MEASURED_FREQUENCY_RESPONSE
global PARAMETERS
%
%
%  extract the data
%
  f = MEASURED_FREQUENCY_RESPONSE(:,1);
  M = MEASURED_FREQUENCY_RESPONSE(:,2);
%
%  convert to dB and rad/sec
%
  MdB = 20*log10(M);  
  w = 2*pi*f;
%
% get the parameters
% 
  wa = PARAMETERS(4);
  za = PARAMETERS(5);
  wb = PARAMETERS(6);
  zb = PARAMETERS(7);
  wc = PARAMETERS(8);
  zc = PARAMETERS(9);
%  
  K1 = x(1);
  wx = x(2);
  zx = abs(x(3));
  wy = x(4);
  zy = abs(x(5));
%
%  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,[1/wc^2 2*zc/wc 1]);
  G4 = tf([1/wx^2 2*zx/wx 1],1);
  G5 = tf([1/wy^2 2*zy/wy 1],1);
  G = G1*G2*G3*G4*G5;
%
%  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 = fitw2z2(x)
%
global PARAMETERS
%
%  extract the data
%
  w3 = PARAMETERS(10);
  z3 = PARAMETERS(11);
  wx = PARAMETERS(12);
  zx = PARAMETERS(13);
  wy = PARAMETERS(14);
  zy = PARAMETERS(15);
  w2 = abs(x(1));
  z2 = abs(x(2));
%  
  bot = (zx*wx+zy*wy);
  top = (z2*w2+z3*w3);
  e3 = 1 - top/bot;
  top = (wx^2+wy^2+(2*zx*wx)*(2*zy*wy));
  bot = (w2^2+w3^2+(2*z2*w2)*(2*z3*w3));
  e2 = 1 - top/bot;
  top = (wy^2*(zx*wx)+wx^2*(zy*wy));
  bot = (w3^2*(z2*w2)+w2^2*(z3*w3));
  e1 = 1 - top/bot;
%  e3 = (zx*wx+zy*wy)-(z2*w2+z3*w3)
%  e2 = (wx^2+wy^2+(2*zx*wx)*(2*zy*wy)) - (w2^2+w3^2+(2*z2*w2)*(2*z3*w3))
%  e1 = (wy^2*(zx*wx)+wx^2*(zy*wy)) - (w3^2*(z2*w2)+w2^2*(z3*w3))
%
%  Compute the squared error
%
  J = e3^2+e2^2+e1^2;
%
%  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 = fitw1z1(x)
%
global PARAMETERS
%
%  extract the data
%
  wa = PARAMETERS(4);
  za = PARAMETERS(5);
  wb = PARAMETERS(6);
  zb = PARAMETERS(7);
  wc = PARAMETERS(8);
  zc = PARAMETERS(9);
  w3 = PARAMETERS(10);
  z3 = PARAMETERS(11);
  wx = PARAMETERS(12);
  zx = PARAMETERS(13);
  wy = PARAMETERS(14);
  zy = PARAMETERS(15);
  w2 = PARAMETERS(16);
  z2 = PARAMETERS(17);  
  w1 = x(1);
  z1 = abs(x(2));
  bot = (za*wa+zb*wb+zc*wc);
  top = (z1*w1+z2*w2+z3*w3);
  e5 = 1 - top/bot;
  bot = wa^2+wb^2+wc^2+(2*za*wa)*(2*zb*wb)+(2*za*wa)*(2*zc*wc)+(2*zb*wb)*(2*zc*wc);
  top = w1^2+w2^2+w3^2+(2*z1*w1)*(2*z2*w2)+(2*z1*w1)*(2*z3*w3)+(2*z2*w2)*(2*z3*w3);
  e4 = 1 - top/bot;
%  e3 = (4*za*wa*zb*wb*zc*wc+za*wa*wb^2 + za*wa*wc^2 + zb*wb*wa^2 + zb*wb*wc^2+zc*wc*wa^2+zc*wc*wb^2) ...
%      -(4*z1*w1*z2*w2*z3*w3+z1*w1*w2^2 + z1*w1*w3^2 + z2*w2*w1^2 + z2*w2*w3^2+z3*w3*w1^2+z3*w3*w2^2);
%
%  Compute the squared error
%
  J = e5^2+e4^2;
%
%  all done
%
  return;
