diff --git a/matlab/peak_detect.m b/matlab/peak_detect.m
index d91c7cc..155d5bb 100644
--- a/matlab/peak_detect.m
+++ b/matlab/peak_detect.m
@@ -25,61 +25,109 @@
 
 function retval = peak_detect ()
 
-N = 1024
+frames = load ('-ascii', '/home/jens/frames.dat');
+[numFrames, N] = size(frames)
+
 Nh = N/2;
 kn = 0.1
-ks = 0.1;
+ks = 0.5;
 
 pa = 0.3;
 d_max = 30;
 
 am = 0.1;
 as = 0.9;
-Ad = 10;
-Cm = 1;
+Ad = 20;
+Cm = 0;
 
 s = zeros(1, Nh);
 a = zeros(1, Nh);
-Xm = zeros(1, Nh);
+Xm = -40*ones(1, Nh);
 Xs = zeros(1, Nh);
 peak_count = zeros(1, Nh);
 
 close all;
 figure
-for k=1:128
+
+
+for k=1:numFrames
 	% Peak generation
 	
-	k = fix(Nh*rand() + 1);
-	
-	if (rand() < pa)
-		if (s(k) == 0)
-			a(k) = ks;
-			s(k) = fix(d_max*rand());
+	if 0
+		z = fix(Nh*rand() + 1);
+		
+		if (rand() < pa)
+			if (s(z) == 0)
+				a(z) = ks;
+				s(z) = fix(d_max*rand());
+			end
 		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);
+		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
-	X = abs(fft(x))/sqrt(N);
-	X = X(1:Nh);
-	X = 10*log10(X.*X);
-
 	% Peak detection
 	Xs = (1-as)*Xs + as*X;
-	Xm_gate = Xs > (Ad+Xm);
-	peak_count = Xm_gate.*peak_count + Xm_gate;
-	peak = peak_count > Cm;
+	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-peak) .* ((1-am)*Xm + am*X) + peak.*Xm;
+	Xm = (1-gate_upd) .* ((1-am)*Xm + am*X) + gate_upd.*Xm;
+	
+	% Filtering
+	if 0
+		Nhf = 21;
+		Ndf = (Nhf-1)/2;
+		hf = [ones(1, Ndf), 0, -ones(1, Ndf)];
+		Xf = abs(filter(hf, 1, X)/Nhf);
+	end
 	
 	% Output
-	marker_X = find(peak ~= 0);
+	numPeaksLast = 0;
+	while(1)
+		numPeaks = 0;
+		for x = find(peak_cand > 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 = peak_count > Cm;
 	marker_Y = X.*peak;
+	marker_X = find(peak > 0);
 	subplot(2, 1, 1)
 	plot(1:Nh, X, marker_X, marker_Y(marker_X), '+'); grid; axis([0, Nh, -60, 40]);
 	subplot(2, 1, 2)