home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l455 / 10.ddi / CONTROL.DI$ / MARGIN.M < prev    next >
Encoding:
Text File  |  1993-03-11  |  6.8 KB  |  249 lines
  1. function [gm,pm,Wcg,Wcp] = margin(a,b,c,d)
  2. %MARGIN    Gain margin, phase margin, and crossover frequencies.
  3. %
  4. %    [Gm,Pm,Wcg,Wcp] = MARGIN(A,B,C,D) returns gain margin Gm,
  5. %    phase margin Pm, and associated frequencies Wcg and Wcp, given
  6. %    the continuous state-space system (A,B,C,D).
  7. %
  8. %    [Gm,Pm,Wcg,Wcp] = MARGIN(NUM,DEN) returns the gain and phase
  9. %    margins for a system in s-domain transfer function form (NUM,DEN).
  10. %
  11. %    [Gm,Pm,Wcg,Wcp] = MARGIN(MAG,PHASE,W)  returns the gain and phase
  12. %       margins given the Bode magnitude, phase, and frequency vectors 
  13. %    MAG, PHASE, and W from a system.  In this case interpolation is 
  14. %    performed between frequency points to find the values. This works
  15. %    for both continuous and discrete systems.
  16. %
  17. %    When invoked without left hand arguments, MARGIN(A,B,C,D) plots
  18. %    the Bode plot with the gain and phase margins marked with a 
  19. %    vertical line. The gain margin, Gm, is defined as 1/G where G 
  20. %    is the gain at the -180 phase frequency. 
  21. %    20*log10(Gm) gives the gain margin in dB.  
  22. %
  23. %    See also, IMARGIN.
  24.  
  25. %     Note: if there is more than one crossover point, margin will
  26. %    return the worst case gain and phase margins. 
  27. %
  28.  
  29. %    Andrew Grace 12-5-91
  30. %    Revised ACWG 6-21-92
  31. %    Copyright (c) 1986-93 by the MathWorks, Inc.
  32.  
  33.  
  34. if nargin==0, eval('exresp(''margin'')'), return, end
  35. error(nargchk(2,4,nargin));
  36.  
  37. % Convert to state-space if in state-space form
  38. if nargin == 4
  39.     [mb,nb]=size(b);  [mc,nc]=size(c);
  40.     if nb > 1 | mc > 1
  41.         error(...
  42.         ['State-space matrices must be SISO, e.g, use: margin(a,b(:,1),c(1,:),d(1,1))']);
  43.     end
  44.     [numin,denin]=ss2tf(a,b,c,d);
  45. elseif nargin == 3
  46.     if nargout > 0
  47.         [gm,pm,Wcg,Wcp] = imargin(a,b,c);
  48.     else 
  49.         imargin(a,b,c);
  50.     end
  51.     return;
  52. else
  53.     numin = a; denin = b;
  54. end
  55.  
  56. % Remove leading zeros
  57. while numin(1) == 0
  58.     numin(1) = [];
  59. end
  60.  
  61. % ----------------- Calculate: num(j)(w) and den(j)(w) --------------
  62. % Calculate num(j)
  63. % Less accurate:  num = numin.*j.^[(length(numin)-1):-1:0];
  64. nl = length(numin);
  65. num   = fliplr(numin); 
  66. num(2:2:nl) =    num(2:2:nl)*j;
  67. num([3:4:nl,4:4:nl])   = -num([3:4:nl,4:4:nl]);
  68. num = fliplr(num);
  69.  
  70. % Calculate den(j)
  71. % Less accurate:  den = denin.*j.^[(length(denin)-1):-1:0];
  72. dl = length(denin);
  73. den   = fliplr(denin); 
  74. den(2:2:dl) =    den(2:2:dl)*j;
  75. den([3:4:dl,4:4:dl])   = -den([3:4:dl,4:4:dl]);
  76. den = fliplr(den);
  77.  
  78.  
  79.  
  80. % ------------------------- Gain Margin ---------------------------
  81. % 1. Gain margin:  find 180 degree crossovers
  82. %
  83. %    Frequency response is purely real and -ve at crossover: 
  84. %
  85. %    num(jw)/den(jw) = -k  
  86. %        
  87. %    num(jw)conj(den(jw))  
  88. %    ----------------------    =  -k
  89. %    den(jw)conj(den(jw))
  90. %
  91. %    imag(num(jw)conj(den(jw))) = 0  i.e take roots
  92.  
  93. % Multiply top and bottom of transfer function by 
  94. % conjugate of denominator to get real part on bottom.
  95.  
  96. rp = conv(num, conj(den)); 
  97. fphase = roots(imag(rp));
  98. [ind] = find(fphase >= 0 & abs(imag(fphase)) < 1e-10);
  99.  
  100. % Work out worst case gain margin
  101. Gm = Inf;
  102. Wcg = NaN;
  103. if length(ind) 
  104.     if nargin == 2
  105.         h = polyval(num, fphase(ind))./polyval(den,fphase(ind));
  106.     else 
  107.         % More accurate to use state-space if available
  108.         h = freqresp(a,b,c,d,1,j*fphase(ind));
  109.     end
  110.     % Find biggest value with real(h) less than zero.
  111.     % Note: there should be no imaginary part to h unless
  112.     %     it is extremely badly conditioned.
  113.     indr = find(real(h) < 0 & abs(imag(h)) < 100 );
  114.     if any(abs(imag(h)) > 100)
  115.         disp('Warning: gain margin may be inaccurate.')
  116.         disp('Try using magnitude and phase data from bode: margin(mag,phase,w)')
  117.     end
  118.     if length(indr)
  119.         [invGm, indm]  = max(abs(h(indr)));
  120.         hindm = h(indr(indm));
  121.         ahindm = abs(angle(hindm));
  122.         if ahindm < 0.9*pi | ahindm > 1.1*pi 
  123.             disp('Warning: gain margin is inaccurate.')
  124.             disp('Try using magnitude and phase data from bode: margin(mag,phase,w)')
  125.         end
  126.         % Uncomment the next line if want gain margin in dB.
  127.         % Gm =  20 * log10(Gm);
  128.         Gm = 1/invGm;
  129.         Wcg = fphase(ind(indr(indm)));
  130.     end
  131. end
  132.  
  133.  
  134. % --------------------- Phase Margin ---------------------------
  135. % 2. Phase margin:  find 0db crossovers
  136. %
  137. %    i.e |num(jw)| / |den(jw)| = 1
  138. %        
  139. %    |den(jw)| - |num(jw)| = 0
  140. %
  141. %    den(jw)conj(den(jw)) - num(jw)conj(num(jw)) = 0  i.e. take roots
  142.  
  143. num2 = conv(num,conj(num));
  144. den2 = conv(den,conj(den));
  145.  
  146. % Pad numerator
  147. num2 = [zeros(1,length(den2)-length(num2)),num2];
  148.  
  149. cl = den2 - num2;
  150.  
  151. fmags = roots(cl);
  152. [ind] = find(abs(imag(fmags)) < 1e-8 * real(fmags) & abs(imag(fmags)) < 1000);
  153. % Work out worst case phase margin
  154. if length(ind) 
  155.     if nargin == 2
  156.         h = polyval(num, fmags(ind))./polyval(den,fmags(ind));
  157.     else 
  158.         % More accurate to use state-space if available
  159.         h = freqresp(a,b,c,d,1,j*fmags(ind));
  160.     end
  161.     ang = angle(h);
  162.     fi = find(ang < 0);
  163.     ang(fi) = ang(fi) + 2*pi;
  164.     [Pm, indm]  = min(abs(ang-pi));
  165.     hindm = h(indm);
  166.     % Comment out the next line if you don't want gain margin in degrees.
  167.     if abs(hindm) < 0.9 | abs(hindm) > 1.1 
  168.         disp('Warning: phase margin is inaccurate.')
  169.         disp('Try using magnitude and phase data from bode: margin(mag,phase,w)')
  170.     end
  171.     Pm = 180/pi * angle(hindm) - 180;
  172.     % -180 < Pm < 180 
  173.     if Pm < -180, Pm = Pm + 360; end  
  174.     Wcp  = real(fmags(ind(indm)));
  175. else
  176.     Pm = Inf;
  177.     Wcp = NaN;
  178. end
  179.  
  180.  
  181. % ------------------------------ Plots ----------------------------------
  182. % If no left hand arguments then plot graph and show location of margins.
  183. if nargout==0,
  184.     if nargin == 2
  185.         [mag,phase,w] = bode(numin,denin);
  186.     else 
  187.         [mag,phase,w] = bode(a,b,c,d);
  188.     end
  189.     status = ishold;
  190.  
  191.     % Plot bode magnitudes
  192.     subplot(211)
  193.     if status
  194.         hold on
  195.     end
  196.     semilogx(w,20*log10(mag))
  197.     hold on
  198.     xlabel('Frequency (rad/sec)'), ylabel('Gain dB')
  199.  
  200.     ylim = get(gca,'ylim');
  201.     % Plot gain and phase margin lines
  202.     if Wcg ~= 0 & finite(Wcg)
  203.         semilogx([Wcg;Wcg],[-20*log10(Gm);zeros(1,length(Gm))],'w-')
  204.         semilogx([Wcg;Wcg],ylim,'w:')
  205.     end
  206.     if finite(Wcp)
  207.         semilogx([Wcp;Wcp],[0;ylim(1)],'w:')
  208.     end
  209.  
  210.     limits = axis;
  211.     plot(limits(1:2),[0,0],'w:')    % 0 dB line
  212.  
  213.     title(['Gm=',num2str(20*log10(Gm)), ' dB,',...
  214.             ' (w= ',num2str(Wcg), ...
  215.             ')   Pm=', num2str(Pm), ' deg.', ...
  216.             ' (w=', num2str(Wcp),')' ]);
  217.  
  218.     if ~status, hold off, end;
  219.  
  220.  
  221.     p = rem(phase-360, 360);                        % Phases modulo 360.
  222.     subplot(212)
  223.     semilogx(w,p)
  224.     limits = axis;
  225.     xlabel('Frequency (rad/sec)'), ylabel('Phase deg')
  226.     hold on
  227.  
  228.     phase180 = -180*ones(1,length(Pm));
  229.     if finite(Wcp)
  230.         semilogx([Wcp;Wcp],[Pm-180;-180],'w-') % Plot phase margins
  231.         semilogx([Wcp;Wcp],[0 -360],'w:')
  232.     end
  233.     if Wcg ~= 0 & finite(Wcg)
  234.         semilogx([Wcg;Wcg],[0;-180],'w:')
  235.     end
  236.  
  237.     limits = axis;
  238.     plot(limits(1:2),[-180,-180],'w:')  % 180 degree line
  239.  
  240.     set(gca,'ylim',[-360, 0]);
  241.     set(gca,'ytick',[0 -90 -180 -270 -360])
  242.  
  243.     if ~status, hold off, end;     % Return hold to previous status
  244.     subplot(111)
  245. else
  246.     gm = Gm;
  247.     pm = Pm;
  248. end
  249.