margin.m

数字通信第四版原书的例程

function [gm,pm,Wcg,Wcp] = margin(a,b,c,d)
%MARGIN	Gain margin, phase margin, and crossover frequencies.
%
%	[Gm,Pm,Wcg,Wcp] = MARGIN(A,B,C,D) returns gain margin Gm,
%	phase margin Pm, and associated frequencies Wcg and Wcp, given
%	the continuous state-space system (A,B,C,D).
%
%	[Gm,Pm,Wcg,Wcp] = MARGIN(NUM,DEN) returns the gain and phase
%	margins for a system in s-domain transfer function form (NUM,DEN).
%
%	[Gm,Pm,Wcg,Wcp] = MARGIN(MAG,PHASE,W)  returns the gain and phase
%       margins given the Bode magnitude, phase, and frequency vectors 
%	MAG, PHASE, and W from a system.  In this case interpolation is 
%	performed between frequency points to find the values. This works
%	for both continuous and discrete systems.
%
%	When invoked without left hand arguments, MARGIN(A,B,C,D) plots
%	the Bode plot with the gain and phase margins marked with a 
%	vertical line. The gain margin, Gm, is defined as 1/G where G 
%	is the gain at the -180 phase frequency. 
%	20*log10(Gm) gives the gain margin in dB.  
%
%	See also, IMARGIN.

% 	Note: if there is more than one crossover point, margin will
%	return the worst case gain and phase margins. 
%

%	Andrew Grace 12-5-91
%	Revised ACWG 6-21-92
%	Copyright (c) 1986-93 by the MathWorks, Inc.


if nargin==0, eval('exresp(''margin'')'), return, end
error(nargchk(2,4,nargin));

% Convert to state-space if in state-space form
if nargin == 4
	[mb,nb]=size(b);  [mc,nc]=size(c);
	if nb > 1 | mc > 1
		error(...
		['State-space matrices must be SISO, e.g, use: margin(a,b(:,1),c(1,:),d(1,1))']);
	end
	[numin,denin]=ss2tf(a,b,c,d);
elseif nargin == 3
	if nargout > 0
		[gm,pm,Wcg,Wcp] = imargin(a,b,c);
	else 
		imargin(a,b,c);
	end
	return;
else
	numin = a; denin = b;
end

% Remove leading zeros
while numin(1) == 0
	numin(1) = [];
end

% ----------------- Calculate: num(j)(w) and den(j)(w) --------------
% Calculate num(j)
% Less accurate:  num = numin.*j.^[(length(numin)-1):-1:0];
nl = length(numin);
num   = fliplr(numin); 
num(2:2:nl) =    num(2:2:nl)*j;
num([3:4:nl,4:4:nl])   = -num([3:4:nl,4:4:nl]);
num = fliplr(num);

% Calculate den(j)
% Less accurate:  den = denin.*j.^[(length(denin)-1):-1:0];
dl = length(denin);
den   = fliplr(denin); 
den(2:2:dl) =    den(2:2:dl)*j;
den([3:4:dl,4:4:dl])   = -den([3:4:dl,4:4:dl]);
den = fliplr(den);



% ------------------------- Gain Margin ---------------------------
% 1. Gain margin:  find 180 degree crossovers
%
%	Frequency response is purely real and -ve at crossover: 
%
%	num(jw)/den(jw) = -k  
%		
%	num(jw)conj(den(jw))  
%	----------------------    =  -k
%	den(jw)conj(den(jw))
%
%	imag(num(jw)conj(den(jw))) = 0  i.e take roots

% Multiply top and bottom of transfer function by 
% conjugate of denominator to get real part on bottom.

rp = conv(num, conj(den)); 
fphase = roots(imag(rp));
[ind] = find(fphase >= 0 & abs(imag(fphase)) < 1e-10);

% Work out worst case gain margin
Gm = Inf;
Wcg = NaN;
if length(ind) 
	if nargin == 2
		h = polyval(num, fphase(ind))./polyval(den,fphase(ind));
	else 
		% More accurate to use state-space if available
		h = freqresp(a,b,c,d,1,j*fphase(ind));
	end
	% Find biggest value with real(h) less than zero.
	% Note: there should be no imaginary part to h unless
	% 	it is extremely badly conditioned.
	indr = find(real(h) < 0 & abs(imag(h)) < 100 );
	if any(abs(imag(h)) > 100)
		disp('Warning: gain margin may be inaccurate.')
		disp('Try using magnitude and phase data from bode: margin(mag,phase,w)')
	end
	if length(indr)
		[invGm, indm]  = max(abs(h(indr)));
		hindm = h(indr(indm));
		ahindm = abs(angle(hindm));
		if ahindm < 0.9*pi | ahindm > 1.1*pi 
		    disp('Warning: gain margin is inaccurate.')
		    disp('Try using magnitude and phase data from bode: margin(mag,phase,w)')
		end
		% Uncomment the next line if want gain margin in dB.
		% Gm =  20 * log10(Gm);
		Gm = 1/invGm;
		Wcg = fphase(ind(indr(indm)));
	end
end


% --------------------- Phase Margin ---------------------------
% 2. Phase margin:  find 0db crossovers
%
%	i.e |num(jw)| / |den(jw)| = 1
%		
%	|den(jw)| - |num(jw)| = 0
%
%	den(jw)conj(den(jw)) - num(jw)conj(num(jw)) = 0  i.e. take roots

num2 = conv(num,conj(num));
den2 = conv(den,conj(den));

% Pad numerator
num2 = [zeros(1,length(den2)-length(num2)),num2];

cl = den2 - num2;

fmags = roots(cl);
[ind] = find(abs(imag(fmags)) < 1e-8 * real(fmags) & abs(imag(fmags)) < 1000);
% Work out worst case phase margin
if length(ind) 
	if nargin == 2
		h = polyval(num, fmags(ind))./polyval(den,fmags(ind));
	else 
		% More accurate to use state-space if available
		h = freqresp(a,b,c,d,1,j*fmags(ind));
	end
	ang = angle(h);
	fi = find(ang < 0);
	ang(fi) = ang(fi) + 2*pi;
	[Pm, indm]  = min(abs(ang-pi));
	hindm = h(indm);
	% Comment out the next line if you don't want gain margin in degrees.
	if abs(hindm) < 0.9 | abs(hindm) > 1.1 
		disp('Warning: phase margin is inaccurate.')
		disp('Try using magnitude and phase data from bode: margin(mag,phase,w)')
	end
	Pm = 180/pi * angle(hindm) - 180;
	% -180 < Pm < 180 
	if Pm < -180, Pm = Pm + 360; end  
	Wcp  = real(fmags(ind(indm)));
else
	Pm = Inf;
	Wcp = NaN;
end


% ------------------------------ Plots ----------------------------------
% If no left hand arguments then plot graph and show location of margins.
if nargout==0,
	if nargin == 2
		[mag,phase,w] = bode(numin,denin);
	else 
		[mag,phase,w] = bode(a,b,c,d);
	end
	status = ishold;

	% Plot bode magnitudes
	subplot(211)
	if status
		hold on
	end
	semilogx(w,20*log10(mag))
	hold on
	xlabel('Frequency (rad/sec)'), ylabel('Gain dB')

	ylim = get(gca,'ylim');
	% Plot gain and phase margin lines
	if Wcg ~= 0 & finite(Wcg)
		semilogx([Wcg;Wcg],[-20*log10(Gm);zeros(1,length(Gm))],'w-')
		semilogx([Wcg;Wcg],ylim,'w:')
	end
	if finite(Wcp)
		semilogx([Wcp;Wcp],[0;ylim(1)],'w:')
	end

	limits = axis;
	plot(limits(1:2),[0,0],'w:')	% 0 dB line

	title(['Gm=',num2str(20*log10(Gm)), ' dB,',...
			' (w= ',num2str(Wcg), ...
			')   Pm=', num2str(Pm), ' deg.', ...
			' (w=', num2str(Wcp),')' ]);

	if ~status, hold off, end;


	p = rem(phase-360, 360);                        % Phases modulo 360.
	subplot(212)
	semilogx(w,p)
	limits = axis;
	xlabel('Frequency (rad/sec)'), ylabel('Phase deg')
	hold on

	phase180 = -180*ones(1,length(Pm));
	if finite(Wcp)
		semilogx([Wcp;Wcp],[Pm-180;-180],'w-') % Plot phase margins
		semilogx([Wcp;Wcp],[0 -360],'w:')
	end
	if Wcg ~= 0 & finite(Wcg)
		semilogx([Wcg;Wcg],[0;-180],'w:')
	end

	limits = axis;
	plot(limits(1:2),[-180,-180],'w:')  % 180 degree line

	set(gca,'ylim',[-360, 0]);
	set(gca,'ytick',[0 -90 -180 -270 -360])

	if ~status, hold off, end; 	% Return hold to previous status
	subplot(111)
else
	gm = Gm;
	pm = Pm;
end