HR = [100.127; 115.430];
h = [2.370 ; 4.718; -8.224 ; 2.909; -1.810 ];
m = [ 4 ; 4 ; 2 ; 2 ; 4]/1e3;

%macierz współczynników przy niewiadomych
A = [ 1 0 0 ; -1 1 0 ; 0 1 0 ; 0 1 -1 ; 1 0 -1];

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

% wektor wyrazów wolnych
L = [-HR(1) ; 0 ; -HR(2) ; 0 ;0 ] - h;

% policzenie wysokości wyrównanych
X = -inv(A'*P*A)*(A'*P*L);
% policzenie poprawek
V = A*X+L;
% policzenie obserwacji wyrównanych
hw = h + V;

% kontrola => wektor 3x1 zer
A'*P*V;

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

% macierz kowariancyjna wektora niewiadomych
Cx = sigma^2*inv(A'*P*A);
% błędy wysokości
mH = sqrt(diag(Cx));

korelacje_H1H2 = Cx(1,2)/(mH(1)*mH(2));% musi być <-1,1>;
korelacje_H1H3 = Cx(1,3)/(mH(1)*mH(3));% musi być <-1,1>;
korelacje_H2H3 = Cx(2,3)/(mH(2)*mH(3));% musi być <-1,1>;

% %% macierz kowariancyjna wektora poprawek
CV = sigma^2*(inv(P) - A*inv(A'*P*A)*A');
% błędy poprawek
mV = sqrt(diag(CV));

% macierz kowariancyjna wektora obserwacji
Ch = sigma^2*(A*inv(A'*P*A)*A');
% błędy obserwacji po wyrównaniu
mh = sqrt(diag(Ch));

% błąd funkcji u = H2-H1
D = [ -1 1 0 ];
Cu = D*Cx*D';
mu = sqrt(Cu);

%ostatni punkt ćwiczenia
% do rozwiązania w domu

