output_precision(7)
% współrzędne osnowy
xy_osnowy  = [
  1400.200 2389.750
  1450.080 2550.150
  1359.880 2640.360
  1219.960 2589.840
  ]; %[m]

% odległosci obserwowane
d_obs = [151.581, 244.275, 255.235, 182.312]'; %[m]

% błędy średnie obserwacji a priori
m     = [8, 15, 15, 12]'/1000; % [mm/1000] = [m]

% współrzędne przybliżone punkt P
X0 = [1250.18, 2409.86];

% macierz różnic współrzędnych pomiędzy punktami osnowy a punktem P
% w pierwszej kolumnie składowa X, w drugiej Y
dXdY = (xy_osnowy - X0); % [m]

% można też tak jak poniżej
% dXdY = [ xy_osnowy(:,1)-X0(1) xy_osnowy(:,2)-X0(2)]

% odległości przybliżone
d0 = sqrt(sum(dXdY.^2, 2)); % [m]

% azymuty przybliżone 
Az0 = atan2(dXdY(:,2), dXdY(:,1)); % [rad]
% lub jak poniżej, ale wtedy uwaga na ćwiartki
% atan(dXdY(:,2)./dXdY(:,1))

% mcierz współczynników przy niewiadomych
A = [-cos(Az0) -sin(Az0)];

% wektor wyrazów wolnych
L = d0 - d_obs;

% macierz wag
P = diag(m.^(-2));

% rozwiązanie układu równań normalnych
% w wyniku mamy przyrosty do składowych X i Y punktu P
dX = - inv(A'*P*A) * (A'*P*L);

% wektor poprawek
V = A*dX + L;

% kilka kontroli
A'*P*V ;
L'*P*A*dX + L'*P*L;
VTPV = V'*P*V;

% błąd średni typowego spostrzeżenia
sigma0 = sqrt(VTPV/(size(A,1)-size(A,2)))

% współrzędne wyrównane
Xw = X0 + dX';

% wyrównane obserwacje (długości)
dw = d_obs + V;

% kontrola: sprawdzenie równości odległości wyrównanych z odległościami
% policzonymi na podstawie współrzędnych wyrównanych
dw_z_Xw = sqrt(sum((xy_osnowy - Xw).^2, 2));
maksymalna_rozbieznosc = max(abs(dw - dw_z_Xw)); % [m]

% macierz kowariancyjna wektora niewiadomych
Cx = sigma0^2*(A'*P*A)^(-1);

% błędy średnie wektora niewiadomych
mX = sqrt(diag(Cx));

% błąd położenia punktu P (pierwiastek z sumy kwadratów błędów średnich składowych)
mP = sqrt(sum(mX.^2));
