home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / IMARGIN.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  5.5 KB  |  182 lines
  1. function [gm,pm,Wcg,Wcp] = imargin(mag,phase,w)
  2. %IMARGIN Gain and phase margins using interpolation.
  3. %
  4. %    [Gm,Pm,Wcg,Wcp] = IMARGIN(MAG,PHASE,W) returns gain margin Gm,
  5. %    phase margin Pm, and associated frequencies Wcg and Wcp, given
  6. %    the Bode magnitude, phase, and frequency vectors MAG, PHASE,
  7. %    and W from a system.  Interpolation is performed between frequency
  8. %    points to find the correct values. 
  9. %
  10. %    When invoked without left hand arguments IMARGIN(MAG,PHASE,W) plots
  11. %    the Bode plot with the gain and phase margins marked with a 
  12. %    vertical line.
  13. %
  14. %    IMARGIN works with the frequency response of both continuous and
  15. %    discrete systems. For continuous systems it preferable to use the
  16. %    MARGIN function which calculates margins analytically given transfer
  17. %    function or state-space form.
  18. %
  19. %    Example of IMARGIN:
  20. %      [mag,phase.w] = dbode(a,b,c,d,1) or [mag,phase,w] = dbode(num,den)
  21. %      [Gm,Pm,Wcg,Wcp] = imargin(mag,phase,w)
  22. %
  23. %    See also: BODE, DBODE, and MARGIN.
  24.  
  25. %    Clay M. Thompson  7-25-90
  26. %     Revised A.C.W.Grace 3-2-91, 6-21-92
  27. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  28.  
  29. error(nargchk(3,3,nargin));
  30.  
  31. Gm = []; Pm = []; Wcg = []; Wcp = [];
  32.  
  33. w = w(:);                      % Make sure freq. is a column
  34. magdb = 20*log10(mag);
  35. logw  = log10(w);
  36.  
  37. % Find the points where the phase wraps.
  38. % The following code is based on the algorithm in the unwrap function.
  39.  
  40. cutoff = 200;                    % Arbitrary value > 180
  41. [m, n] = size(phase);                % Assume column orientation.
  42. p = rem(phase-360, 360);                        % Phases modulo 360.
  43. dp = [p(1,:);diff(p)];                % Differentiate phases.
  44. jumps = (dp > cutoff) + (dp < -cutoff);        % Array of jumps locations
  45. jvec = (jumps~=0);
  46.  
  47. % Find points where phase crosses -180 degrees
  48.  
  49. pad = 360*ones(1,n);
  50. upcross = (([p;-pad] >= -180)&([pad;p] < -180));
  51. downcross = (([p;pad] <= -180)&([-pad;p] > -180));
  52. crossings = upcross + downcross;
  53. pvec = (crossings~=0);
  54.  
  55. % Find points where magnitude crosses 0 db
  56.  
  57. pad = ones(1,n);
  58. upcross = (([magdb;-pad] >= 0)&([pad;magdb] < 0));
  59. downcross = (([magdb;pad] <= 0)&([-pad;magdb] > 0));
  60. crossings = upcross + downcross;
  61. mvec = (crossings~=0);
  62.  
  63. for i=1:n
  64.     jloc = find(jvec(:,i)~=0);
  65.     nj = length(jloc);
  66.  
  67.     % Remove points where phase has wrapped from pvec
  68.     if ~isempty(jloc), pvec(jloc,i) = zeros(nj,1); end
  69.  
  70.     ploc = find(pvec(:,i)~=0);
  71.     mloc = find(mvec(:,i)~=0);
  72.  
  73.     % Find phase crossover points and interpolate gain in db and log(freq)
  74.     % at each point.
  75.     lambda = (-180-p(ploc-1,i)) ./ (p(ploc,i)-p(ploc-1,i));
  76.     gain = magdb(ploc-1,i) + lambda .* (magdb(ploc,i)-magdb(ploc-1,i));
  77.     freq = logw(ploc-1) + lambda .* (logw(ploc)-logw(ploc-1));
  78.  
  79.     % Look for asymptotic behavior near -180 degrees.  (30 degree tolerance).
  80.     % Linearly extrapolate gain and frequency based on first 2 or last 2 points.
  81.     tol = 30;
  82.     if m>=2,
  83.         if (abs(p(1,i)+180)<tol),    % Starts near -180 degrees.
  84.             lambda = (-180-p(1,i)) / (p(2,i)-p(1,i));
  85.             exgain = magdb(1,i) + lambda * (magdb(2,i)-magdb(1,i));
  86.             exfreq = logw(1) + lambda * (logw(2)-logw(1));
  87.             gain = [gain;exgain];  freq = [freq;exfreq];
  88.         end
  89.         if (abs(p(m,i)+180)<tol),    % Ends near -180 degrees.
  90.             lambda = (-180-p(m-1,i)) / (p(m,i)-p(m-1,i));
  91.             exgain = magdb(m-1,i) + lambda * (magdb(m,i)-magdb(m-1,i));
  92.             exfreq = logw(m-1) + lambda * (logw(m)-logw(m-1));
  93.             gain = [gain;exgain];  freq = [freq;exfreq];
  94.         end
  95.     end
  96.     
  97.     if isempty(gain),
  98.         Gm = [Gm,inf]; Wcg = [Wcg,NaN];
  99.         ndx = [];
  100.     else
  101.         [Gmargin,ndx] = min(abs(gain));
  102.         Gm = [Gm,-gain(ndx)];    Wcg = [Wcg,freq(ndx)];
  103.     end
  104.  
  105.     % Find gain crossover points and interpolate phase in degrees and log(freq)
  106.     % at each point.
  107.     lambda = -magdb(mloc-1,i) ./ (magdb(mloc,i)-magdb(mloc-1,i));
  108.     ph   = p(mloc-1,i) + lambda .* (p(mloc,i)-p(mloc-1,i));
  109.     freq = logw(mloc-1) + lambda .* (logw(mloc)-logw(mloc-1));
  110.  
  111.     if isempty(ph),
  112.         Pm = [Pm,inf];   Wcp = [Wcp,NaN];
  113.         ndx = [];
  114.     else
  115.         [Pmargin,ndx] = min(abs(ph+180));
  116.         Pm = [Pm,(ph(ndx)+180)];     Wcp = [Wcp,freq(ndx)];
  117.     end
  118.  
  119. end
  120.  
  121. % Convert frequency back to rad/sec and gain back to magnitudes.
  122. Wcg(finite(Wcg)) = 10 .^ Wcg(finite(Wcg)); 
  123. Wcp(finite(Wcp)) = 10 .^ Wcp(finite(Wcp));  
  124. Gm(finite(Gm)) = 10 .^ (Gm(finite(Gm))/20);
  125.  
  126. % If no left hand arguments then plot graph and show location of margins.
  127. if nargout==0,
  128.     holdon = ishold;
  129.     subplot(211)
  130.     if holdon
  131.         hold on
  132.     end
  133.  
  134.     % Plot bode magnitudes
  135.     semilogx(w,20*log10(mag))
  136.     hold on
  137.     xlabel('Frequency (rad/sec)'), ylabel('Gain dB')
  138.  
  139.     limits = axis;
  140.     % Plot gain and phase margin lines
  141.     if Wcg ~= 0 & finite(Wcg)
  142.         semilogx([Wcg;Wcg],[-20*log10(Gm);zeros(1,length(Gm))],'w-')
  143.         semilogx([Wcg;Wcg],limits(3:4)','w:') %,[Wcp;Wcp],[0;limits(3)],'w:')
  144.     end
  145.     if finite(Wcp)
  146.         semilogx([Wcp;Wcp],[0;limits(3)],'w:')
  147.     end
  148.     limits = axis;
  149.     plot(limits(1:2),[0,0],'w:')    % 0 dB line
  150.  
  151.  
  152.     title(['Gm=',num2str(20*log10(Gm)), ' dB,',...
  153.     ' (w= ',num2str(Wcg), ...
  154.     ')   Pm=', num2str(Pm), ' deg.', ...
  155.     ' (w=', num2str(Wcp),')' ]);
  156.  
  157.     if ~holdon, hold off, end;
  158.  
  159.     subplot(212) 
  160.     semilogx(w,p)
  161.     hold on
  162.     xlabel('Frequency (rad/sec)'), ylabel('Phase deg')
  163.  
  164.     phase180 = -180*ones(1,length(Pm));
  165.     if finite(Wcp)
  166.         semilogx([Wcp;Wcp],[Pm+phase180;phase180],'w-') % Plot phase margins
  167.         semilogx([Wcp;Wcp],[0 -360],'W:') 
  168.     end
  169.     if Wcg ~= 0 & finite(Wcg)
  170.         semilogx([Wcg;Wcg],[0;-180],'w:')
  171.     end
  172.     limits = axis;
  173.     plot(limits(1:2),[-180 -180],'w:')  % 180 degree line
  174.     set(gca,'ylim',[-360, 0]);
  175.     set(gca,'ytick',[0 -90 -180 -270 -360])
  176.     if ~holdon, hold off, end;     % Return hold to previous status
  177.     subplot(111)
  178. else
  179.     gm = Gm;
  180.     pm = Pm;
  181. end
  182.