diff --git a/matlab/peak_detect.m b/matlab/peak_detect.m
new file mode 100644
index 0000000..3c4046d
--- /dev/null
+++ b/matlab/peak_detect.m
@@ -0,0 +1,93 @@
+## 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 ()
+
+N = 1024
+Nh = N/2;
+kn = 0.1
+
+close all;
+figure
+
+pa = 0.3;
+d_max = 30;
+
+s = zeros(1, Nh);
+a = zeros(1, Nh);
+Xm = -0*ones(1, Nh);
+Xs = zeros(1, Nh);
+peak_count = zeros(1, Nh);
+am = 0.1;
+as = 0.9;
+
+for k=1:64
+	% Peak generation
+	f = fix(N/2*rand());
+	k = f + 1;
+	
+	if (rand() < pa)
+		if (s(k) == 0)
+			a(k) = 1.0;
+			s(k) = fix(d_max*rand());
+		end
+	end
+	x = kn*randn(1, N);
+	for n = 1:Nh
+		if s(n) > 0
+			s(n) = s(n) - 1;
+			x = x + a(n)*cos(2*pi*n*(0:N-1)/N);
+		end
+	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 > (10+Xm);
+	peak_count = Xm_gate.*peak_count + Xm_gate;
+	peak = peak_count > 1;
+	% Update Xm at no-peak bins
+	Xm = (1-peak) .* ((1-am)*Xm + am*X) + peak.*Xm;
+	
+	% Output
+	marker_X = find(peak ~= 0);
+	marker_Y = X.*peak;
+	subplot(2, 1, 1)
+	plot(1:Nh, X, marker_X, marker_Y(marker_X), '+'); 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