Problems with initializing CamShift algorithm - matlab

Question

I use matlab to implement a tracker using cam shift. I need to implement it myself hence I'm not using the built in method.
I let the user choose the object to track and after the first iteration the convergence makes it start tracking a different object. pictures speaks louder than words so:
I choose my face in the first frame:
It converges to my shoulder and starting tracking it:
Now the tracking is overall ok throughout the video so I think the issue is in the initialization phase.
SomeCode: Get position from user:
function [pos] = getObjectPosition(F)
f = figure;
imshow(F);
pos = getPosition(imrect(gca));
close(f);
end
Main Loop:
firstFrame = readFrame(input);
pos = getObjectPosition(firstFrame);
while hasFrame(input)
% init next frame
nextFrame = readFrame(input);
[currFrameHSV, ~, ~] = rgb2hsv(nextFrame);
frameHue = double(currFrameHSV(:,:,1));
% init while loop vars
prevX = -1;
prevY = -1;
i = 0;
while (i < numOfIterations) && ~(prevX == pos(1) && prevY == pos(2))
% updating the curr iteration number
i = i + 1;
% getting search window location and dimensions
envRect = round(getEnvRect(pos,pos(4)/yRatio,pos(3)/xRatio,height,width));
[X,Y] = meshgrid(envRect(1):envRect(1) + envRect(3),...
envRect(2):envRect(2) + envRect(4));
% getting the sub image
I = imcrop(frameHue,envRect);
M00 = sum(sum(I));
M10 = sum(sum(X.*I));
M01 = sum(sum(Y.*I));
% getting center mass location
Xc = round(M10/M00);
Yc = round(M01/M00);
% updating object position
prevX = pos(1);
prevY = pos(2);
pos(1) = floor(Xc - pos(3)/2);
pos(2) = floor(Yc - pos(4)/2);
end
% adding the object's bounding box to the frame to write
% show the image
end
getEnv method:
function [ envRect ] = getEnvRect( feature,hShift,wShift,rows,cols )
hEnvSize = feature(4) + 2*hShift;
wEnvSize = feature(3) + 2*wShift;
xPatch = feature(1);
xEnv = max(1, xPatch - wShift);
yPatch = feature(2);
yEnv = max(1, yPatch - hShift);
width = min(cols, xEnv + wEnvSize) - xEnv;
height = min(rows, yEnv + hEnvSize) - yEnv;
envRect = [xEnv, yEnv, width, height];
end