From 0b816e8113e5312ffa7e13ef4588bec78ab1f603 Mon Sep 17 00:00:00 2001
From: Jens Ahrensfeld <jens@jayfield.org>
Date: Wed, 22 May 2019 16:12:45 +0000
Subject: [PATCH] - proper init of Xm - improved creation of peak boxes

git-svn-id: http://moon:8086/svn/software/trunk/projects/GnuRadio@444 b431acfa-c32f-4a4a-93f1-934dc6c82436
---
 matlab/peak_detect.m | 82 ++++++++++++++++++++++++++++++--------------
 1 file changed, 56 insertions(+), 26 deletions(-)

diff --git a/matlab/peak_detect.m b/matlab/peak_detect.m
index 9165752..0dd9006 100644
--- a/matlab/peak_detect.m
+++ b/matlab/peak_detect.m
@@ -39,13 +39,13 @@ d_max = 30;
 
 am = 0.04;
 as = 0.9;
-Ad = 20;
+Ad = 15;
 Cm = 0;
-pbh = 10;
+pbh = 5;
 
 s = zeros(1, Nh);
 a = zeros(1, Nh);
-Xm = -120*ones(1, Nh);
+Xm = zeros(1, Nh);
 Xs = zeros(1, Nh);
 peak_count = zeros(1, Nh);
 
@@ -80,30 +80,21 @@ for k=1:numFrames
 		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);
-	gate_upd = Xs > (10+Xm);
 	peak_count = gate_peak.*peak_count + gate_peak;
-	peak_cand = peak_count > 0;
-	% Update Xm at no-peak bins
-	Xm = (1-gate_upd) .* ((1-am)*Xm + am*X) + gate_upd.*Xm*0.7;
-	
-	% Fill peak gaps with guessed values
-	last = Xm(1);
-	for n=2:Nh,
-		if gate_upd(n) == 0
-			last = Xm(n);
-		else
-			Xm(n) = last;	
-		end
-	end
- 
+
 	% Output
 	numPeaksLast = 0;
 	while(1)
 		numPeaks = 0;
-		for x = find(peak_cand > 0),
+		for x = find(peak_count > 0),
 			_x = x;
 			while(1)
 				y0 = X(_x);
@@ -130,20 +121,59 @@ for k=1:numFrames
 		end
 		numPeaksLast = numPeaks;
 	end
-	peak = peak_count > Cm;
-	marker_Y = X.*peak;
-	marker_X = find(peak > 0);
+	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_boxes = Xm;
-	for x = find(peak > 0),
+	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);
-			peak_boxes(pbox_start:pbox_end) = X(x) * ones(1, pbox_end - pbox_start + 1);
+			
+			% 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, marker_X, marker_Y(marker_X), 'ro', 1:Nh, peak_boxes, 'm-'); grid; axis([0, Nh, -60, 40]);
+	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);