function subpixpos = interpolate_pos(H,pixpos)

dH = first_deriv_H(H)
ddH = second_deriv_H(H)
subpixpos = pixpos+findmax(dH,ddH);

function pos = findmax(dH,ddH)

% Gaussian elimination (need not fully invert the matrix)

R = [ddH(1,1) ddH(1,2) ddH(1,3) -dH(1);
     ddH(2,1) ddH(2,2) ddH(2,3) -dH(2);
     ddH(3,1) ddH(3,2) ddH(3,3) -dH(3)];

% scale equations so that coefficient for x is 1
for u = 1:3
    if abs(R(u,1))>eps
        R(u,:) = R(u,:)./R(u,1);
    end
 end

% eliminate x from 2 equations
if abs(R(1,1))>eps
    if abs(R(2,1))>eps
        R(2,:) = R(2,:)-R(1,:);
    end
    if abs(R(3,1))>eps
        R(3,:) = R(3,:)-R(1,:);
    end
    elimorder = [9 9 1];
    redeq = [2,3];
elseif abs(R(2,1))>eps
    if abs(R(3,1))>eps
        R(3,:) = R(3,:)-R(2,:);
    end
    elimorder = [9 9 2];
    redeq = [1,3];
elseif abs(R(3,1))>eps
    elimorder = [9 9 3];
    redeq = [1,2];
else
    error('equation sytem is singular!');
end

% eliminate y from 1 equation (if still necessary)
if abs(R(redeq(1),2))>eps && abs(R(redeq(2),2))>eps % leading coeffs not already 0
    R(redeq(1),:) = R(redeq(1),:)./R(redeq(1),2);
    R(redeq(2),:) = R(redeq(2),:)./R(redeq(2),2);
    R(redeq(2),:) = R(redeq(2),:)-R(redeq(1),:);
    elimorder(1:2) = [redeq(2) redeq(1)];
elseif abs(R(redeq(1),2))>eps
    elimorder(1:2) = [redeq(2) redeq(1)];
elseif abs(R(redeq(2),2))>eps
    elimorder(1:2) = [redeq(1) redeq(2)];
else % both leading coeffs are 0!
    error('equation system is singular!');
end

% back-substitution
pos(elimorder(1)) = R(elimorder(1),4)/R(elimorder(1),3);
pos(elimorder(2)) = (R(elimorder(2),4)-R(elimorder(2),3)*pos(elimorder(1)))/R(elimorder(2),2);
pos(elimorder(3)) = (R(elimorder(3),4)-R(elimorder(3),3)*pos(elimorder(1))-R(elimorder(3),2)*pos(elimorder(2)))/R(elimorder(3),1);


function ddH = second_deriv_H(H)

ddH = zeros(3,3);

ddH(1,1) = -2*H(2,2,2)+H(1,2,2)+H(3,2,2);
ddH(2,2) = -2*H(2,2,2)+H(2,1,2)+H(2,3,2);
ddH(3,3) = -2*H(2,2,2)+H(2,2,1)+H(2,2,3);

ddH(1,2) = .25*(H(1,1,2)+H(3,3,2)-H(1,3,2)-H(3,1,2));
ddH(1,3) = .25*(H(1,2,1)+H(3,2,3)-H(1,2,3)-H(3,2,1));
ddH(2,3) = .25*(H(2,1,1)+H(2,3,3)-H(2,1,3)-H(2,3,1));
ddH(2,1) = ddH(1,2);
ddH(3,1) = ddH(1,3);
ddH(3,2) = ddH(2,3);

function dH = first_deriv_H(H)

dH(1) = 0.5*(H(3,2,2)-H(1,2,2));
dH(2) = 0.5*(H(2,3,2)-H(2,1,2));
dH(3) = 0.5*(H(2,2,3)-H(2,2,1));