H_R  = 1.00 ;

h_ob = [ 1.04 2.05 3.03 ]'; 

HA_0  = H_R + h_ob(1);
HB_0  = H_R + h_ob(3);

A    = [ 1 0 ; -1 1 ; 0 1 ]; 
m    = [ .02 ;  .02 ; .04 ];
P = inv(diag(m.^2));

L = [ -H_R ; 0 ; -H_R ] - h_ob; % [m]
L = L + [HA_0 ; HB_0 - HA_0; HB_0 ];
dX = -inv(A'*P*A) * (A'*P*L)


HA_W = HA_0 + dX(1)
HB_W = HB_0 + dX(2)

V = A*dX + L

A'*P*V
V'*P*V, L'*P*A*dX + L'*P*L

h_w = h_ob + V 

HA_W - H_R
HB_W - HA_W
HB_W - H_R 

sigma = sqrt(V'*P*V/ (size(A)(1) - size(A)(2)))
