%
%  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 (not in dB)
%  The following are intial estimates
%             K2       = estimated static gain
%             wt       = estimated natural frequency of the pendulum
%             w1       = estimated natural frequency of the first cart
%             z1       = estimated damping ratio of the first cart
%
%
function G = model_inverted_pendulum(data,K2,wt,z1,w1);
%
%  Assign the global variable so we can use it in the fit function
%
global  MEASURED_FREQUENCY_RESPONSE
global PARAMETERS
%
  MEASURED_FREQUENCY_RESPONSE = data;
%
%==========================================================================
%
%  Set the initial conditions
%
%  We really won't be able to determine K2, and w1 so use what we measured
%
  PARAMETERS(1) = K2;
  PARAMETERS(2) = wt;
%
  delta = 1.00;
%  
  X0 = [delta z1 w1 ];
%
%  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);
%
%  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
%
  delta = x(1); 
  z1 = abs(x(2));
  w1 = x(3);
%
%=================================
% Now lets make the frequency response plot 
%=================================
%
%  Construct the transfer function for the cart
%
  G = tf([(K2*w1*w1) 0 (K2*w1*w1*wt*wt)],[delta 2*z1*w1 (w1*w1+wt*wt) (2*z1*w1*wt*wt) (w1*w1*wt*wt)]);
%
%  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
%
  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_t = ' num2str(wt,3) ', \zeta_1 = ' num2str(z1,3) ', \omega_1 = ' num2str(w1,3) ', \Delta = ', num2str(delta,3)]);
%
%=======================================================================
%
%  Now we construc the A matrix with the values we've found
%
%   q_1 = x, q_2 = x_dot, q_3 = theta, q_4 = theta_dot
%
%  First find the parameters
%
   g = 980 % g  in terms of cm/s^2
   K1 = (1-delta)/(w1*w1*wt*wt/g)
%
   A = zeros(4,4);
   A(1,2) = 1;
   A(3,4) = 1;
   A(2,1) = -w1^2/delta;
   A(2,2) = (-2*z1*w1)/delta;
   A(2,3) = (K1*w1^2*wt^2)/delta;
   A(4,1) = (-wt^2*w1^2/g)/delta;
   A(4,2) = (-wt^2*2*z1*w1/g)/delta;
   A(4,3) = (wt^2)/delta;
   B = zeros(4,1);
   B(2) = (K2*w1^2)/delta;
   B(4) = (K2*w1^2*wt^2/g)/delta;
   C = eye(4,4);
   D = 0;
   save state_model A B C D
%
%  find transfer funtion from F to theta
% 
   Gt = tf([K2*w1^2*wt^2/g 0 0],[delta 2*z1*w1 (w1*w1-wt*wt) (-2*z1*w1*wt^2) (-w1^2*wt^2)])
%
  C = zeros(1,4);
  C(3) = 1;
  [num,den] = ss2tf(A,B,C,D);
  Gest = tf(num,den)
%
%  find transfer funtion from F to X
% 
   Gx = tf([K2*w1^2 0 -K2*w1^2*wt^2],[delta 2*z1*w1 (w1*w1-wt*wt) (-2*z1*w1*wt^2) (-w1^2*wt^2)])
%
  C = zeros(1,4);
  C(1) = 1;
  [num,den] = ss2tf(A,B,C,D);
  Gest = tf(num,den)
%

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
% 
  K2 = PARAMETERS(1);
  wt = PARAMETERS(2);
  delta = x(1);
  z1 = x(2);
  w1 = x(3);
%
%  Construct the transfer function
%
   G = tf([(K2*w1*w1) 0 (K2*w1*w1*wt*wt)],[delta 2*z1*w1 (w1*w1+wt*wt) (2*z1*w1*wt*wt) (w1*w1*wt*wt)]);
%
%  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;