diff --git a/matlab/peak_detect.m b/matlab/peak_detect.m
index f2f7c6a..2b5dab4 100644
--- a/matlab/peak_detect.m
+++ b/matlab/peak_detect.m
@@ -92,28 +92,30 @@ for k=1:numFrames
 	Xs = (1-as)*Xs + as*X;
 	gate_peak = (Xs-Xm) >= Ad;
 	peak_count = gate_peak.*peak_count + gate_peak;
-
+	
+	peaks = peak_count > 0;
+	peak_pos = find(peaks);
+	
 	% Output
 	numPeaksLast = 0;
 	while(1)
 		numPeaks = 0;
-		for x = find(peak_count > 0),
-			_x = x;
+		for x = peak_pos,
 			while(1)
-				y0 = X(_x);
-				xp = min(Nh, _x+1);
-				xn = max(1, _x-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;
+					peaks(x) = 0;
+					peaks(xn) = 0;
+					x = xp;
 				elseif X(xn) > y0
-					peak_count(_x) = 0;
-					peak_count(xp) = 0;
-					_x = xn;
+					peaks(x) = 0;
+					peaks(xp) = 0;
+					x = xn;
 				else
-					peak_count(xn) = 0;
-					peak_count(xp) = 0;
+					peaks(xn) = 0;
+					peaks(xp) = 0;
 					numPeaks = numPeaks + 1;
 					break;
 				end	
@@ -124,15 +126,15 @@ for k=1:numFrames
 		end
 		numPeaksLast = numPeaks;
 	end
-	peak_cand = find(peak_count > 0);
+	peak_pos = find(peaks);
 	
 	% Sort peaks
-	[v, n] = sort(X(peak_cand), 'descend');
-	
-	peak_cand_sorted = peak_cand(n);
+	[v, n] = sort((Xs-Xm)(peak_pos), 'descend');
 	
+	peak_cand_sorted = peak_pos(n);
+
 	% Construct peak boxes
-	peak_final = [];
+	peak_pos = [];
 	peak_boxes_rel = zeros(1, Nh);
 	peak_boxes_abs = zeros(1, Nh);
 	for x = peak_cand_sorted,
@@ -191,7 +193,7 @@ for k=1:numFrames
 		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];
+			peak_pos = [peak_pos x];
 		end
 	endfor
 
@@ -214,7 +216,7 @@ for k=1:numFrames
 	
 	% 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]);
+	plot(1:Nh, X, peak_pos, X(peak_pos), 'ro', 1:Nh, peak_box_values, 'm-'); grid; axis([0, Nh, -60, 40]);
 	subplot(2, 1, 2)
 	plot(1:Nh, Xs-Xm, 1:Nh, Xm); grid; axis([0, Nh, -60, 40]);
 	pause(1/30);