diff --git a/matlab/peak_detect.m b/matlab/peak_detect.m
index 2b5dab4..081ad42 100644
--- a/matlab/peak_detect.m
+++ b/matlab/peak_detect.m
@@ -90,7 +90,7 @@ for k=1:numFrames
 	
 	% Peak detection
 	Xs = (1-as)*Xs + as*X;
-	gate_peak = (Xs-Xm) >= Ad;
+	gate_peak = (X-Xm) >= Ad;
 	peak_count = gate_peak.*peak_count + gate_peak;
 	
 	peaks = peak_count > 0;
@@ -129,7 +129,7 @@ for k=1:numFrames
 	peak_pos = find(peaks);
 	
 	% Sort peaks
-	[v, n] = sort((Xs-Xm)(peak_pos), 'descend');
+	[v, n] = sort((X-Xm)(peak_pos), 'descend');
 	
 	peak_cand_sorted = peak_pos(n);
 
@@ -216,9 +216,9 @@ for k=1:numFrames
 	
 	% Output
 	subplot(2, 1, 1)
-	plot(1:Nh, X, peak_pos, X(peak_pos), 'ro', 1:Nh, peak_box_values, 'm-'); grid; axis([0, Nh, -60, 40]);
+	plot(1:Nh, X, peak_pos, X(peak_pos), 'ro', 1:Nh, peak_box_values, 'm-'); grid; axis([0, Nh, -60, 40]); legend('X', 'Peaks', 'BBox')
 	subplot(2, 1, 2)
-	plot(1:Nh, Xs-Xm, 1:Nh, Xm); grid; axis([0, Nh, -60, 40]);
+	plot(1:Nh, Xs-Xm, 1:Nh, Xm); grid; axis([0, Nh, -60, 40]); legend('X_s - Xm', 'X_m')
 	pause(1/30);
 end