format long g

source "rw.m"

% 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]

%% nowe
K_obs  = grad2rad([20.3450 67.7770 100.5460]'); % [rad]
mK     = grad2rad([10 10 10]') / 1e4 ; % [rad]

% wpsół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) ];

%% nowe
A = [A  zeros(size(A,1),1)];
A = [A ; dme_bearing(X0, xy_osnowy(1:3,:))(:,[1,2,5])];


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

%% nowe
L = [L ; Az0(1:3) - K_obs ];

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

%% nowe
P = blkdiag(P, diag(mK.^-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'

%% nowe
Xw = X0 + dX'(1:2)

%% nowe
% wyrównane obserwacje (długości)
yw = [d_obs; K_obs] + V;

% 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));

% macierz kowariancyjna obserwacji
Cy = sigma0^2 * A * (A'*P*A)^(-1) * A';

% i błędy średnie obserwacji po wyrównaniu
my = sqrt(diag(Cy));

% błąd azumutu Az_P-P2
dXdYw = (xy_osnowy - Xw);

D = [ dXdYw(2,2) , -dXdYw(2,1) 0 ]/yw(2)^2;
C_Az = D*Cx*D';
m_Az = sqrt(C_Az);
m_Az_sek = rad2deg(m_Az)*3600;

% błąd drugiej funkcji
D = [ 1./[dXdYw(1,1), -dXdYw(1,2)] , 0];
Cu = D*Cx*D';
mu = sqrt(Cu);

% % korelacja 
% rho_XY = Cx(1,2)/(prod(mX))
%% nowe
rho_XY = Cx(1,2)/(prod(mX(1:2)));

% elipsa błędu średniego
Q = inv(A'*P*A);

QP1 = Q(1:2,1:2);
lambda = eig(QP1);

ab = sigma0 *sqrt(2*lambda);
fi =  1/2*atan2(2*QP1(1,2), QP1(1,1)-QP1(2,2));
fi_st = rad2deg(fi);

