% krótki wyciąg z rozwiązania zadania przedstawiony na wykładzie
XY_osnowy = [ 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;
XY_0_P = [1250.180, 2409.860];
P = diag(md.^(-2));
d0 = sqrt( (XY_0_P(1)-XY_osnowy(:,1)).^2 + (XY_0_P(2)-XY_osnowy(:,2)).^2);
A0 = atan2((XY_osnowy(:,2)-XY_0_P(2)), (XY_osnowy(:,1)-XY_0_P(1)));
A = [ -cos(A0) -sin(A0) ];
L = d0 - d;
dX = -inv(A'*P*A)*(A'*P*L);
Xw = XY_0_P + dX'

