clc
clear all
close all

x=42+17/60 % grau
y=60+48/60 % grau
z=256+52/60 % grau

l=[x;y;z]

soma_l=l(1)+l(2)+l(3) % confirmação que há erros na medição dos ângulos!

W_l=eye(3)

n0=2;
n=3;
r=n-n0;

disp ('1. Ajustamento condicional')
disp ('2. Ajustamento parametrico')
disp ('    ')

n=input('    Escolha uma opcao:  ')

switch n

    case 1 % ajustamento condicional Av=f (B=0)

        % x+vx+y+vy+z+vz=360
        % vx+vy+vz=360-x-y-z

        A=[1 1 1]
        f=[360-l(1)-l(2)-l(3)]

        % procedimento de ajustamento

        k=inv(A*inv(W_l)*A')*f
        v=inv(W_l)*A'*k

        l_ajust=l+v

        soma_l_ajust=l_ajust(1)+l_ajust(2)+l_ajust(3)

    case 2 % ajustamento parametrico v+B*delta=f (A=I)

        u=2;

        % x+vx=u1
        % y+vy=u2
        % z+vz=360-u1-u2

        B=[-1 0;0 -1;1 1]
        f=[-x;-y;360-z]

        % procedimento de ajustamento

        N=B'*W_l*B
        t=B'*W_l*f
        delta=inv(N)*t
        v=f-B*delta
        l_ajust=l+v

end    
        
