%
%  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 ratio of magnitudes
%  (output/input). data is produced from file process_data.m
%
%  The following three parameters are the initial estimates:
%             K       = static gain
%             omega_n = estimated natural frequency
%             zeta    = estiamted damping ratio
%
function model_1cart(data,K,omega_n,zeta);
%
%  Assign the global variable so we can use it in the fit function
%
global  MEASURED_FREQUENCY_RESPONSE
%
  MEASURED_FREQUENCY_RESPONSE = data;
%
%  Set the initial conditions
%
  X0 = [ K omega_n zeta ];
%
%  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(@fit,X0,options);
%
  K = x(1); 
  omega_n = x(2);
  zeta = abs(x(3));
%
%  Construct the transfer function
%
  G = tf(K,[1/omega_n^2 2*zeta/omega_n 1]);

%  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);
%
%  Now plot the results
%
  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(['Optimal Fit To Frequency Response:  gain  = ' num2str(K,4) ', \omega_n = ' num2str(omega_n,3) ' (rad/sec), \zeta = ' num2str(zeta,3)]);
%
%  write out the state model
%
  A = [0 1; -omega_n^2 -2*zeta*omega_n];
  B = [0; K*omega_n^2];
  C = [1 0];
  D = 0;
  save state_model 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
%
function J = fit(x)
%
global MEASURED_FREQUENCY_RESPONSE
%
%  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
% 
  K = x(1); 
  omega_n = x(2);
  zeta = x(3);
%
%  Construct the transfer function
%
  G = tf(K,[1/omega_n^2 2*zeta/omega_n 1]);
%
%  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;