%% cztery warianty rozwiązania
%% 1. tylko pomiar czterech odległości
%% 2.       pomiar czterech odległości i trzech kierunków
%% 3.       pomiar czterech odległości i dwóch kątów
%%          (zakładamy niezależeność pomiaru kątów z błędami wynikającymi
%%           z prawa przenoszenia błędów średnich Gaussa)
%% 4.  jak w wariancie 3. tylko przy założeniu zależności pomiędzy mierzonymi kątami

%% plik z moimi funkcjami używanymi w programie
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
md     = [8, 15, 15, 12]'/1000; % [mm/1000] = [m]

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

%% odległości przybliżone
%% na podstawie własnej funkcji w pliku rw.m
d0 = distance(X0, xy_osnowy);

%% azymuty przybliżone 
%% na podstawie własnej funkcji w pliku rw.m
Az0 = azimuth(X0, xy_osnowy);

%% wariant 1
%% mcierz współczynników przy niewiadomych
%% na podstawie własnej funkcji w pliku rw.m
%% tylko 1 i 2 kolumna
M(1).A = dme_dist(X0, xy_osnowy)(:,1:2) ;
%% wektor wyrazów wolnych
M(1).L = d0 - d_obs;
%% macierz wag
M(1).P = diag(md.^(-2));
M(1).y = d_obs;

%% wariant 2
%% dochodzą 3 obserwacje kierunków
K_obs  = grad2rad([ 20.3450 67.7770 100.5460 ]'); % [rad]
mK = grad2rad([10 10 10]') / 1e4 ; % [rad]

%% w przpadku pomiarów kierunków dochodzi jedna niewiadoma dla 
%% każdego stanowiska (orientacja ,,0'' limbusa instrumentu)
M(2).A = [M(1).A zeros(size(M(1).A,1),1)];
M(2).A = [M(2).A ; dme_bearing(X0, xy_osnowy(1:3,:))(:,[1,2,5])];
M(2).L = [M(1).L ; Az0(1:3) - K_obs ];
M(2).P = blkdiag(M(1).P, diag(mK.^-2));
M(2).y = [M(1).y ; K_obs ];

%% wariant 3
%% pseudoobserwacje kątów
alpha_obs = diff(K_obs); % [rad]

%% obliczenie kątów przybliżonych na podstawie wsp. przybliżonych
%% wykorzystanie zdefiniowanej funkcji
alpha0 = angle(xy_osnowy([1,2],:), X0 , xy_osnowy([2,3],:));
M(3).A = [M(1).A ; dme_angle(xy_osnowy([1,2],:), X0 , xy_osnowy([2,3],:))(:,5:6)];
M(3).L = [M(1).L ; alpha0 - alpha_obs];

%% macierz kofaktorów kierunków
QK = diag(mK.^(2));
%% macierz Jakobiego, kąty w funkcji kierunków
Dalpha = [-1 1 0 ; 0 -1 1 ];

%% macierz kowariancyjna kątów
Qalpha =  Dalpha * QK * Dalpha' ;
rho_alpha = Qalpha(2,1)/prod(sqrt(diag(Qalpha)(1:2)));

%% zaniedbujemy zależność pomiędy pomierzonymi kątami
M(3).P = blkdiag(M(1).P,inverse(diag((diag(Qalpha)))));
M(3).y = [M(1).y; alpha_obs ];

%% 4 wariant
M(4).A = M(3).A;
M(4).L = M(3).L;
M(4).y = M(3).y;
%% uwzględniamy zależność pomiędzy kątami
M(4).P = inv(blkdiag(inv(M(1).P), Qalpha));

%%% macierz Jakobiego do obu zadanych funkcji
D = [
  [ (xy_osnowy(2,2) - X0(2)), -(xy_osnowy(2,1) - X0(1)) ] / d0(2)**2  ; 
  +(xy_osnowy(1,1) - X0(1))^(-1) ,  -(xy_osnowy(1,2) - X0(2))^(-1)  
  ] ;

%% rozwiązania dla wszystkich wariantów
for i = 1:length(M)
  %% rozwiązanie układu równań normalnych
  %% w wyniku mamy przyrosty do składowych X i Y punktu P
  M(i).dX = -inverse(M(i).A' * M(i).P * M(i).A) * (M(i).A' * M(i).P * M(i).L);

  %% współrzędne wyrównane
  M(i).Xw = X0 + M(i).dX(1:2)';

  %% poprawki
  M(i).V = M(i).A * M(i).dX + M(i).L;

  %% wyrównane obserwacje
  M(i).yw = M(i).y + M(i).V;

  %% błąd średni typowego spostrzeżenia
  M(i).sigma0 = sqrt(M(i).V'*M(i).P*M(i).V/(size(M(i).A,1)-size(M(i).A,2)));

  %% macierz kowariancyjna wektora niewiadomych
  M(i).CX = M(i).sigma0^2*(M(i).A'*M(i).P*M(i).A)^(-1);

  %% błędy średnie wektora niewiadomych
  M(i).mX = sqrt(diag(M(i).CX));

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

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

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

  %% korelacja między X i Y punktu wyznaczanego
  M(i).rho_XY = M(i).CX(2,1)/(prod(M(i).mX(1:2)));

  %% obliczenie elips błędów średnich
  M(i).invATPA = inv(M(i).A'*M(i).P*M(i).A);
  [M(i).a, M(i).b, M(i).phi] = errorellipse(M(i).invATPA(1:2,1:2), M(i).sigma0);

  %% obliczenie błędów zadanych funkcji
  M(i).mu = sqrt(diag(D * M(i).CX(1:2,1:2) *D'));

end
