GnuRadio/matlab/peak_detect.m

## Copyright (C) 2019 Jens
## 
## This program is free software: you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation, either version 3 of the License, or
## (at your option) any later version.
## 
## This program is distributed in the hope that it will be useful, but
## WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
## 
## You should have received a copy of the GNU General Public License
## along with this program.  If not, see
## <https://www.gnu.org/licenses/>.

## -*- texinfo -*- 
## @deftypefn {} {@var{retval} =} peak_detect (@var{input1}, @var{input2})
##
## @seealso{}
## @end deftypefn

## Author: Jens <jens@orion>
## Created: 2019-05-20

function retval = peak_detect ()

frames = load ('-ascii', '/home/jens/frames.dat');
[numFrames, N] = size(frames)

DO_SIM_PEAKS = 0;

Nh = N;
kn = 0.1
ks = 0.5;

pa = 0.3;
d_max = 30;

am = 0.04;
as = 0.9;
Ad = 15;
Cm = 0;
pbh = 5;

s = zeros(1, Nh);
a = zeros(1, Nh);
Xm = zeros(1, Nh);
Xs = zeros(1, Nh);
peak_count = zeros(1, Nh);

close all;
figure

for k=1:numFrames
	% Peak generation
	
	if DO_SIM_PEAKS
		z = fix(Nh*rand() + 1);
		
		if (rand() < pa)
			if (s(z) == 0)
				a(z) = ks;
				s(z) = fix(d_max*rand());
			end
		end
		x = kn*randn(1, N);
		for n = 1:Nh
			if s(n) > 0
				s(n) = s(n) - 1;
				f = 0.5-rand() + n;
				x = x + a(n)*cos(2*pi*f*(0:N-1)/N);
			end
		end
		X = abs(fft(x))/sqrt(N);
		X = X(1:Nh);
		X = 10*log10(X.*X);
	else
		X = frames(k,:);
		X = X(1:Nh);
	end
	
	% Init 
	if k == 1
		Xm = Xm + mean(X);
	endif
	
	% Peak detection
	Xs = (1-as)*Xs + as*X;
	gate_peak = Xs > (Ad+Xm);
	peak_count = gate_peak.*peak_count + gate_peak;

	% Output
	numPeaksLast = 0;
	while(1)
		numPeaks = 0;
		for x = find(peak_count > 0),
			_x = x;
			while(1)
				y0 = X(_x);
				xp = min(Nh, _x+1);
				xn = max(1, _x-1);
				if X(xp) > y0
					peak_count(_x) = 0;
					peak_count(xn) = 0;
					_x = xp;
				elseif X(xn) > y0
					peak_count(_x) = 0;
					peak_count(xp) = 0;
					_x = xn;
				else
					peak_count(xn) = 0;
					peak_count(xp) = 0;
					numPeaks = numPeaks + 1;
					break;
				end	
			end
		endfor
		if numPeaks == numPeaksLast
			break;
		end
		numPeaksLast = numPeaks;
	end
	peak_cand = find(peak_count > 0);
	
	% Sort peaks
	[v, n] = sort(X(peak_cand), 'descend');
	
	peak_cand_sorted = peak_cand(n);
	
	% Construct peak boxes
	peak_final = [];
	peak_boxes_rel = zeros(1, Nh);
	peak_boxes_abs = zeros(1, Nh);
	for x = peak_cand_sorted,
			pbox_start = max(1, x-pbh);
			pbox_end = min(Nh, x+pbh);
			
			% Find intersection
			box_height_rel = Xs(x) - Xm(x);
			box_height_abs = X(x);
			does_intersect = 0;
			for n=pbox_start:pbox_end
				if peak_boxes_rel(n) > box_height_rel
					does_intersect = 1;
					break
				end
			endfor
			if does_intersect == 0
				peak_boxes_rel(pbox_start:pbox_end) = box_height_rel * ones(1, pbox_end - pbox_start + 1);
				peak_boxes_abs(pbox_start:pbox_end) = box_height_abs * ones(1, pbox_end - pbox_start + 1);
				peak_final = [peak_final x];
			end
	endfor

	% Update Xm at no-peak bins
	gate_upd = peak_boxes_rel > 0;
	Xm = (1-am)*Xm + am*X;

	% Fill peak gaps with guessed values
	last = Xm(1);
	for n=2:Nh,
		if gate_upd(n) == 0
			last = 0.5*last + 0.5*Xm(n);
		else
			Xm(n) = last;	
		end
	end
 
	% Create peak boxes with correct absolute height
	peak_box_values = Xm;
	peak_box_values((peak_boxes_rel > 0)) = peak_boxes_abs(peak_boxes_rel > 0);
	
	% Output
	subplot(2, 1, 1)
	plot(1:Nh, X, peak_final, X(peak_final), 'ro', 1:Nh, peak_box_values, 'm-'); grid; axis([0, Nh, -60, 40]);
	subplot(2, 1, 2)
	plot(1:Nh, Xs, 1:Nh, Xm); grid; axis([0, Nh, -60, 40]);
	pause(1/30);
end

function s = matchedFilter(N, B)
n2 = (N-1)/2
n = (0:N-1)-n2
s = sinc(n/B)

endfunction
	
endfunction