findmax2.m

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

function [gt,d]=findmax2(data, flag)
%FINDMAX2 Interpolates the maxima in a matrix of data.
%
%	Function used for returning all the maxima in a set of
%	data (matrices). The maxima are calculated by
%	quadratic interpolation.

if nargin < 2, flag = 0, end
% 2-D
d = [1.5, 1.5];
[m,n]=size(data);
% Factor for error control:    
factor = 5;  % Keep error five times less than nearest constraint

% Constants
diagix = [-1 -1 1 1];
diagix2 = [1 -1 -1 1];
% Use point [0,0], [1,0], [0,1], [0, -1], [-1 0] and [1,1] on grid:
diagix3 = [0 1 0 0 -1 1 ];
diagix4 = [0 0 1 -1 0 1 ];
% Point [-1, -1] used to check goodness of quadratic fit. 
xerr = [-1; -1];

invH = [  ... 
-1.0000    0.5000         0         0    0.5000         0
0.5000   -0.5000   -0.5000         0         0    0.5000
-1.0000         0    0.5000    0.5000         0         0
0    0.5000         0         0   -0.5000         0
0         0    0.5000   -0.5000         0         0
1.0000         0         0         0         0         0
];

% Find the peaks
crmax = (data>[data(1,:)-1;data(1:m-1,:)]) & (data>=[data(2:m,:);data(m,:)-1]) & ...
	(data>[data(:,1)-1,data(:,1:n-1)]) & (data>=[data(:, 2:n),data(:,n)-1]);

f = find(crmax);
for i = f'
	ni = 1 + floor((i-0.5)/m);
	mi = i - (ni-1)*m;
	ix = mi - diagix;
	iy = ni - diagix2;
	fix = find(ix > 0 & ix <= m & iy > 0 & iy <= n); 
	if any(data(mi, ni) <= data(ix(fix), iy(fix)))
		crmax(mi, ni) = 0; 
	end
end
f = find(crmax)';
H =zeros(2,2); 
% Fit a quadratic
cnt = 0; 
for i = f
	cnt = cnt + 1;
	ni = 1 + floor((i-0.5)/m);
	mi = i - (ni-1)*m;
	if (mi > 1 & mi < m & ni > 1 & ni < n) 
		ix = mi - diagix3;
		iy = ni - diagix4;
		for k = 1:6
		    y(k,1) = data(ix(k),iy(k)); 
		end
		coeffs = invH*y; 
		H(1,1) = coeffs(1);
		H(1,2) = coeffs(2);
		H(2,1) = H(1,2);
		H(2,2) = coeffs(3); 
		b = coeffs(4:5);
		c = coeffs(6);
		x = -0.5 * (H\b); 
		gt(cnt) = x'*H*x + b'*x + c;
% Error control 
		if flag 
		    err2 = abs(xerr'*H*xerr + b'*xerr + c - data(mi+1, ni+1 ));
		    mult = 1; 
		    if err2 < 1/20*abs(gt(cnt))
		        mult = 1.25;
		    elseif err2 > (5*abs(gt(cnt))+1e-5)
		        mult = 0.75;
		    end

		    pky = quad2(data(mi-1:mi+1,ni));
		    f = abs((pky + 1.1e-5) /(factor *(data(mi,ni)-pky) + 1e-5));
		    d(1,2) = min(d(1,2), mult * ((f>=0.3) + (0.7 + f)*(f< 0.3) +(f>1)*(log(f)/100)));
		    pkx =  quad2(data(mi, ni-1:ni+1)');
		    f = abs((pkx + 1.1e-5) /(factor  *(data(mi,ni) - pkx) + 1e-5));
		    d(1,1) = min(d(1,1), mult * ((f>=0.3) + (0.7 + f)*(f< 0.3) +(f>1)*(log(f)/100)));
		end
	elseif mi > 1 & mi < m
		gt(cnt) = quad2(data(mi-1:mi+1,ni));
		if flag 
		    pky = gt(cnt);
		    f = abs((pky + 1.1e-5) /(factor *(data(mi,ni) - pky) + 1e-5));
		    d(1,2) = min(d(1,2), (f>=0.3) + (0.7 + f)*(f< 0.3) +(f>1)*(log(f)/100));
		    if (pky > -1e-5), d(1,1) = min(1, d(1,1)); end
		end
	elseif ni > 1 & ni < n
		gt(cnt) = quad2(data(mi, ni-1:ni+1)');
		if flag 
		    pkx = gt(cnt); 
		    f = abs((pkx + 1.1e-5) /(factor  *(data(mi,ni) - pkx) + 1e-5));
		    d(1,1) = min(d(1,1), (f>=0.3) + (0.7 + f)*(f< 0.3) +(f>1)*(log(f)/100));
		    if (pkx > -1e-5), d(1,2) = min(1, d(1,2)); end
		end
	else
		gt(cnt) = data(mi, ni);
		if (gt(cnt) > -1e-5), 
		    d(1,1) = min(1, d(1,1)); 
		    d(1,2) = min(1, d(1,2)); 
		end
	end
end