hydPos = [625 0; 375 0; 125 0; -125 0; -375 0; -625 0;];
hydPosNoise = 5 * rand(6,2);
hydPos = hydPos + hydPosNoise;

targetYOffset = 500;

c = 1500;
toa = [sqrt(targetYOffset^2+ hydPos(1,1)^2) / c ...
       sqrt(targetYOffset^2+ hydPos(2,1)^2) / c ...
       sqrt(targetYOffset^2+ hydPos(3,1)^2) / c ...
       sqrt(targetYOffset^2+ hydPos(4,1)^2) / c ...
       sqrt(targetYOffset^2+ hydPos(5,1)^2) / c ...
       sqrt(targetYOffset^2+ hydPos(6,1)^2) / c];
toaNoise = .001 * rand(1,6);
toa = toa + toaNoise;

A = [hydPos(1,1) hydPos(1,2) -(c*toa(1)); ...
     hydPos(2,1) hydPos(2,2) -(c*toa(2)); ...
     hydPos(3,1) hydPos(3,2) -(c*toa(3)); ...
     hydPos(4,1) hydPos(4,2) -(c*toa(4)); ...
     hydPos(5,1) hydPos(5,2) -(c*toa(5)); ...
     hydPos(6,1) hydPos(6,2) -(c*toa(6)) ];
 
b = [hydPos(1,1)^2 + hydPos(1,2)^2 - (c^2*toa(1)^2); ...
     hydPos(2,1)^2 + hydPos(2,2)^2 - (c^2*toa(2)^2); ...
     hydPos(3,1)^2 + hydPos(3,2)^2 - (c^2*toa(3)^2); ...
     hydPos(4,1)^2 + hydPos(4,2)^2 - (c^2*toa(4)^2); ...
     hydPos(5,1)^2 + hydPos(5,2)^2 - (c^2*toa(5)^2); ...
     hydPos(6,1)^2 + hydPos(6,2)^2 - (c^2*toa(6)^2)];
 
targetPos = .5*(inv(A'*A)*(A'*b))