imargin.m

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

function [gm,pm,Wcg,Wcp] = imargin(mag,phase,w)
%IMARGIN Gain and phase margins using interpolation.
%
%	[Gm,Pm,Wcg,Wcp] = IMARGIN(MAG,PHASE,W) returns gain margin Gm,
%	phase margin Pm, and associated frequencies Wcg and Wcp, given
%	the Bode magnitude, phase, and frequency vectors MAG, PHASE,
%	and W from a system.  Interpolation is performed between frequency
%	points to find the correct values. 
%
%	When invoked without left hand arguments IMARGIN(MAG,PHASE,W) plots
%	the Bode plot with the gain and phase margins marked with a 
%	vertical line.
%
%	IMARGIN works with the frequency response of both continuous and
%	discrete systems. For continuous systems it preferable to use the
%	MARGIN function which calculates margins analytically given transfer
%	function or state-space form.
%
%	Example of IMARGIN:
%	  [mag,phase.w] = dbode(a,b,c,d,1) or [mag,phase,w] = dbode(num,den)
%	  [Gm,Pm,Wcg,Wcp] = imargin(mag,phase,w)
%
%	See also: BODE, DBODE, and MARGIN.

%	Clay M. Thompson  7-25-90
% 	Revised A.C.W.Grace 3-2-91, 6-21-92
%	Copyright (c) 1986-93 by the MathWorks, Inc.

error(nargchk(3,3,nargin));

Gm = []; Pm = []; Wcg = []; Wcp = [];

w = w(:);				  	% Make sure freq. is a column
magdb = 20*log10(mag);
logw  = log10(w);

% Find the points where the phase wraps.
% The following code is based on the algorithm in the unwrap function.

cutoff = 200;					% Arbitrary value > 180
[m, n] = size(phase);				% Assume column orientation.
p = rem(phase-360, 360);                        % Phases modulo 360.
dp = [p(1,:);diff(p)];				% Differentiate phases.
jumps = (dp > cutoff) + (dp < -cutoff);		% Array of jumps locations
jvec = (jumps~=0);

% Find points where phase crosses -180 degrees

pad = 360*ones(1,n);
upcross = (([p;-pad] >= -180)&([pad;p] < -180));
downcross = (([p;pad] <= -180)&([-pad;p] > -180));
crossings = upcross + downcross;
pvec = (crossings~=0);

% Find points where magnitude crosses 0 db

pad = ones(1,n);
upcross = (([magdb;-pad] >= 0)&([pad;magdb] < 0));
downcross = (([magdb;pad] <= 0)&([-pad;magdb] > 0));
crossings = upcross + downcross;
mvec = (crossings~=0);

for i=1:n
	jloc = find(jvec(:,i)~=0);
	nj = length(jloc);

	% Remove points where phase has wrapped from pvec
	if ~isempty(jloc), pvec(jloc,i) = zeros(nj,1); end

	ploc = find(pvec(:,i)~=0);
	mloc = find(mvec(:,i)~=0);

	% Find phase crossover points and interpolate gain in db and log(freq)
	% at each point.
	lambda = (-180-p(ploc-1,i)) ./ (p(ploc,i)-p(ploc-1,i));
	gain = magdb(ploc-1,i) + lambda .* (magdb(ploc,i)-magdb(ploc-1,i));
	freq = logw(ploc-1) + lambda .* (logw(ploc)-logw(ploc-1));

	% Look for asymptotic behavior near -180 degrees.  (30 degree tolerance).
	% Linearly extrapolate gain and frequency based on first 2 or last 2 points.
	tol = 30;
	if m>=2,
		if (abs(p(1,i)+180)<tol),	% Starts near -180 degrees.
		    lambda = (-180-p(1,i)) / (p(2,i)-p(1,i));
		    exgain = magdb(1,i) + lambda * (magdb(2,i)-magdb(1,i));
		    exfreq = logw(1) + lambda * (logw(2)-logw(1));
		    gain = [gain;exgain];  freq = [freq;exfreq];
		end
		if (abs(p(m,i)+180)<tol),	% Ends near -180 degrees.
		    lambda = (-180-p(m-1,i)) / (p(m,i)-p(m-1,i));
		    exgain = magdb(m-1,i) + lambda * (magdb(m,i)-magdb(m-1,i));
		    exfreq = logw(m-1) + lambda * (logw(m)-logw(m-1));
		    gain = [gain;exgain];  freq = [freq;exfreq];
		end
	end
	
	if isempty(gain),
		Gm = [Gm,inf]; Wcg = [Wcg,NaN];
		ndx = [];
	else
		[Gmargin,ndx] = min(abs(gain));
		Gm = [Gm,-gain(ndx)];    Wcg = [Wcg,freq(ndx)];
	end

	% Find gain crossover points and interpolate phase in degrees and log(freq)
	% at each point.
	lambda = -magdb(mloc-1,i) ./ (magdb(mloc,i)-magdb(mloc-1,i));
	ph   = p(mloc-1,i) + lambda .* (p(mloc,i)-p(mloc-1,i));
	freq = logw(mloc-1) + lambda .* (logw(mloc)-logw(mloc-1));

	if isempty(ph),
		Pm = [Pm,inf];   Wcp = [Wcp,NaN];
		ndx = [];
	else
		[Pmargin,ndx] = min(abs(ph+180));
		Pm = [Pm,(ph(ndx)+180)];     Wcp = [Wcp,freq(ndx)];
	end

end

% Convert frequency back to rad/sec and gain back to magnitudes.
Wcg(finite(Wcg)) = 10 .^ Wcg(finite(Wcg)); 
Wcp(finite(Wcp)) = 10 .^ Wcp(finite(Wcp));  
Gm(finite(Gm)) = 10 .^ (Gm(finite(Gm))/20);

% If no left hand arguments then plot graph and show location of margins.
if nargout==0,
	holdon = ishold;
	subplot(211)
	if holdon
		hold on
	end

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

	limits = axis;
	% 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],limits(3:4)','w:') %,[Wcp;Wcp],[0;limits(3)],'w:')
	end
	if finite(Wcp)
		semilogx([Wcp;Wcp],[0;limits(3)],'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 ~holdon, hold off, end;

	subplot(212) 
	semilogx(w,p)
	hold on
	xlabel('Frequency (rad/sec)'), ylabel('Phase deg')

	phase180 = -180*ones(1,length(Pm));
	if finite(Wcp)
		semilogx([Wcp;Wcp],[Pm+phase180;phase180],'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 ~holdon, hold off, end; 	% Return hold to previous status
	subplot(111)
else
	gm = Gm;
	pm = Pm;
end