diff --git a/matlab/peak_detect.m b/matlab/peak_detect.m
index 1fa98f8..f6b9ee1 100644
--- a/matlab/peak_detect.m
+++ b/matlab/peak_detect.m
@@ -108,10 +108,10 @@ for k=1:numFrames
 	end
 	
 	% Create peak distances by subtracting Xm
-	peak_dist = (X-Xm);
+	Xd = (X-Xm);
 
 	% Find peak candidates by thresholding
-	peaks = peak_dist >= Ad;
+	peaks = Xd >= Ad;
 	peak_pos = find(peaks);
 	
 	% Search true maximum peak using hill climbing
@@ -147,60 +147,70 @@ for k=1:numFrames
 	peak_pos = find(peaks);
 	
 	% Sort peaks
-	[v, n] = sort((X-Xm)(peak_pos), 'descend');
+	[v, n] = sort((Xd)(peak_pos), 'descend');
 	
 	peak_pos_sorted = peak_pos(n);
 
 	% Construct peak boxes
 	peak_pos = [];
+	peak_boxes_abs = Xm;
 	peak_boxes_rel = zeros(1, Nh);
-	peak_boxes_abs = zeros(1, Nh);
-	for x = peak_pos_sorted,
+	for pos = peak_pos_sorted,
 		
 		% Find peak widths
-		left = x;
-		right = x;
-		nom = X(x) - 10;
+		left = pos;
+		right = pos;
+		nom = Xd(pos) - 10;
 		left_found = 0;
 		right_found = 0;
+		pbh = 0;
 		while(~(left_found & right_found))
-			% Determine peak width left from center (x)
-			if X(left) > nom
+			% Determine peak width left from center (pos)
+			if Xd(left) > nom
 				if left > 1
 					left = left - 1;
 				else
-					left_found = 1;
+					break;
 				end
 			else
 				left_found = 1;
 			endif
-			% Determine peak width right from center (x)
-			if X(right) > nom
+			% Determine peak width right from center (pos)
+			if Xd(right) > nom
 				if right < Nh
 					right = right + 1;
 				else
-					right_found = 1;
+					break;
 				end
 			else
 				right_found = 1;
 			endif
-			pbh_left = x - left;
-			pbh_right = right - x;
+			pbh_left = pos - left;
+			pbh_right = right - pos;
 			
 			% Take larger peak width
 			pbh = max(pbh_left, pbh_right);
 			
 			% Ensure peak box is at least pb_min
 			pbh = max(pbh, pb_min);
+			
+			if pbh > pb_max
+				pbh = 0;
+				break;
+			end	
 		end
 		
+		if pbh == 0
+			continue;
+		endif
+
 		% Use peak width for peak box
-		pbox_start = max(1, x-pbh);
-		pbox_end = min(Nh, x+pbh);
+		pbox_start = max(1, pos-pbh);
+		pbox_end = min(Nh, pos+pbh);
 		
 		% Find intersection
-		box_height_rel = X(x) - Xm(x);
-		box_height_abs = X(x);
+		box_height_abs = X(pos);
+		box_height_rel = Xd(pos);
 		does_intersect = 0;
 		for n=pbox_start:pbox_end
 			if peak_boxes_rel(n) > box_height_rel
@@ -209,19 +219,15 @@ for k=1:numFrames
 			end
 		endfor
 		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_pos = [peak_pos x];
+			peak_boxes_rel(pbox_start:pbox_end) = box_height_rel * ones(1, pbox_end - pbox_start + 1);
+			peak_pos = [peak_pos pos];
 		end
 	endfor
 
-	% Create peak boxes with correct absolute height
-	peak_box_values = Xm;
-	peak_box_values((peak_boxes_rel > 0)) = peak_boxes_abs(peak_boxes_rel > 0);
-	
 	% 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, 60]); legend('X', 'Peaks', 'Peak Box')
+	plot(1:Nh, X, peak_pos, X(peak_pos), 'ro', 1:Nh, peak_boxes_abs, 'm-'); grid; axis([0, Nh, -60, 60]); legend('X', 'Peaks', 'Peak Box')
 	subplot(2, 1, 2)
 	plot(1:Nh, Xs-Xm, 1:Nh, Xm); grid; axis([0, Nh, -60, 60]); legend('X_s - Xm', 'X_m')
 	pause(1/30);