function Fnew = updateSpeed(normal, lambda, k1, k2)

l = norm(normal);
if l~= 0
	sine = abs(normal(1))/l;
	cosine = abs(normal(2))/l;
    Fnew = abs(cosine - sine) + lambda*abs(k2*cosine - k1*sine);
else
    Fnew = 0;
end

% if normal(2) ~= 0
%     dtds = normal(1)/normal(2);
%     Fnew = abs(k2*dtds - k1) + lambda*abs(1 - dtds);
% else
%     Fnew = abs(k2 - k1);
% end
%     