h = [2.370; 4.718; -8.224; 2.909; -1.810];
m = [4; 4; 2; 2; 4]*1e-3;
H = [100.127; 115.430];

H_przyb = [H(1) + h(1), H(2) + h(3), H(1) + h(1) - h(5)]';
printf("Przybliżone wysokości\n")
printf("%10.4f\n", H_przyb)


A = [
   1  0  0
  -1  1  0
   0  1  0
   0  1 -1
   1  0 -1
  ] ;

P = diag(m.^-2) ;

L = [ -H(1) ; 0 ; -H(2) ; 0 ; 0 ] - h ;
L = L +  [
  H_przyb(1) 
  -H_przyb(1) + H_przyb(2)
  H_przyb(2)
  H_przyb(2) - H_przyb(3)
  H_przyb(1) - H_przyb(3)
  ]

% L = [ -H(1) ; 0 ; -H(2) ; 0 ; 0 ] - h + A*H_przyb; % albo tak

dX = -inv(A'*P*A)*(A'*P*L);
printf("\nWyrównane przyrosty do wysokości:\n", dX)
printf("%10.4f\n", dX)

H_wyrownane = H_przyb + dX;
printf("\nWyrównane wysokości:\n", dX)
printf("%10.4f\n", H_wyrownane)

V = A*dX + L ;

A'*P*V
[V'*P*V, L'*P*A*dX + L'*P*L]
hw = h + V;

printf("Wyrównane przewyższenia:\n")
printf("%10.4f\n", hw)

disp("Pamietajcie o kontroli wyrównanych przewyższeń z przewyższeniami policzonymi z wyrównanych wysokości!")

[n,r] = size(A);
sigma0 = sqrt(V'*P*V/(n-r))

