diff --git a/matlab/peak_detect.m b/matlab/peak_detect.m
index 12e0c27..1fa98f8 100644
--- a/matlab/peak_detect.m
+++ b/matlab/peak_detect.m
@@ -107,32 +107,33 @@ for k=1:numFrames
 		end
 	end
 	
-	gate_peak = (X-Xm) >= Ad;
-	peak_count = gate_peak.*peak_count + gate_peak;
-	
-	peaks = peak_count > 0;
+	% Create peak distances by subtracting Xm
+	peak_dist = (X-Xm);
+
+	% Find peak candidates by thresholding
+	peaks = peak_dist >= Ad;
 	peak_pos = find(peaks);
 	
-	% Output
+	% Search true maximum peak using hill climbing
 	numPeaksLast = 0;
 	while(1)
 		numPeaks = 0;
-		for x = peak_pos,
+		for pos = peak_pos,
 			while(1)
-				y0 = X(x);
-				xp = min(Nh, x+1);
-				xn = max(1, x-1);
-				if X(xp) > y0
-					peaks(x) = 0;
-					peaks(xn) = 0;
-					x = xp;
-				elseif X(xn) > y0
-					peaks(x) = 0;
-					peaks(xp) = 0;
-					x = xn;
+				y0 = X(pos);
+				pos_p = min(Nh, pos+1);
+				pos_n = max(1, pos-1);
+				if X(pos_p) > y0
+					peaks(pos) = 0;
+					peaks(pos_n) = 0;
+					pos = pos_p;
+				elseif X(pos_n) > y0
+					peaks(pos) = 0;
+					peaks(pos_p) = 0;
+					pos = pos_n;
 				else
-					peaks(xn) = 0;
-					peaks(xp) = 0;
+					peaks(pos_n) = 0;
+					peaks(pos_p) = 0;
 					numPeaks = numPeaks + 1;
 					break;
 				end