XY = [
  1400.200 2389.750
  1450.080 2550.150
  1359.880 2640.360
  1219.960 2589.840 ];
d = [151.581
244.275
255.235
182.312];

md = [8 15 15 12]'/1e3;
XY0 =[1250.180 2409.860];

P = diag(md.^(-2));

Az0 = atan2(XY(:,2)-XY0(2), XY(:,1)-XY0(1));
A = [-cos(Az0) -sin(Az0)];

d_0 = sqrt((XY(:,1)-XY0(1)).^2+ (XY(:,2)-XY0(2)).^2);
L = d_0 - d;

dX = -inv(A'*P*A)*(A'*P*L);
Xw = XY0 + dX'

V = A*dX + L;
A'*P*V;
