% ******************************************
% exercicio 28: ajustamento de uma poligonal
% ******************************************

clc
clear all
close all

format long;

disp ('Calculo de uma poligonal.')
disp ('   ')

% constantes

sigma02=1;
NSIGN=0.05;
CONV=0.001;
NMAX=10;

% coordenadas pontos extremos da poligonal

MB=8478.139;
PB=2483.826;
ME=7709.336;
PE=2263.411;

% rumos de orientacao em cada extremo da poligonal

RBA=(68.0+15.0/60.0+20.7/3600.0)*pi/180.0;
REF=(300.0+11.0/60.0+30.5/3600.0)*pi/180.0;

% coordenadas provisorias dos pontos intermedios da poligonal

MC=8200;
PC=2340;
MD=7980;
PD=2230;

% modelo funcional

n=7;
n0=4;
r=n-n0;
u=4;
c=r+u;

% valores observados

alfa1=(172.0+53.0/60.0+34.0/3600.0)*pi/180.0;
alfa2=(185.0+22.0/60.0+14.0/3600.0)*pi/180.0;
alfa3=(208.0+26.0/60.0+19.0/3600.0)*pi/180.0;
alfa4=(205.0+13.0/60.0+51.0/3600.0)*pi/180.0;
d1=281.832;
d2=271.300;
d3=274.100;

l=[alfa1;alfa2;alfa3;alfa4;d1;d2;d3];

% incertezas

sigma_alfa1=4.0/3600.0*pi/180.0;
sigma_alfa2=4.0/3600.0*pi/180.0;
sigma_alfa3=4.0/3600.0*pi/180.0;
sigma_alfa4=4.0/3600.0*pi/180.0;
sigma_d1=0.016;
sigma_d2=0.016;
sigma_d3=0.016;

Sigma_l=eye(7);
Sigma_l(1,1)=sigma_alfa1^2;
Sigma_l(2,2)=sigma_alfa2^2;
Sigma_l(3,3)=sigma_alfa3^2;
Sigma_l(4,4)=sigma_alfa4^2;
Sigma_l(5,5)=sigma_d1^2;
Sigma_l(6,6)=sigma_d2^2;
Sigma_l(7,7)=sigma_d3^2;

Q_l=Sigma_l/sigma02;

W=inv(Q_l);

% R_BA+alfa1+v1-R_BC=0
% R_CB+alfa2+v2-R_CD=0
% R_DC+alfa3+v3-R_DE=0
% R_ED+alfa4+v4-R_EF=0
% dBC+v5-BC=0
% dCD+v6-CD=0
% dDE+v7-DE=0

% linearizando:

% (R_BA0+R_BA_lin)+alfa1+v1-(R_BC0+R_BC_lin)=0
% (R_CB0+R_CB_lin)+alfa2+v2-(R_CD0+R_CD_lin)=0
% (R_DC0+R_DC_lin)+alfa3+v3-(R_DE0+R_DE_lin)=0
% (R_ED0+R_ED_lin)+alfa4+v4-(R_EF0+R_EF_lin)=0
% dBC+v5-(BC0+BC_lin)=0
% dCD+v6-(CD0+CD_lin)=0
% dDE+v7-(DE0+DE_lin)=0

% v1+R_BA_lin-R_BC_lin=-alfa1-R_BA0+R_BC0
% v2+R_CB_lin-R_CD_lin=-alfa2-R_CB0+R_CD0
% v3+R_DC_lin-R_DE_lin=-alfa3-R_DC0+R_DE0
% v4+R_ED_lin-R_EF_lin=-alfa4-R_ED0+R_EF0
% v5-BC_lin=-dBC+BC0
% v6-CD_lin=-dCD+CD0
% V7-DE_lin=-dDE+DE0

delta=[MC;PC;MD;PD];

itera=1;
while (itera <= NMAX)
    %1
    RUMO=-rumo_lin(MB,PB,delta(1),delta(2));
    B(1,1)=RUMO(3,1);
    B(1,2)=RUMO(4,1);
    B(1,3)=0;
    B(1,4)=0;
    f(1,1)=-(RBA+l(1)-rumo_cal(MB,PB,delta(1),delta(2)));
    %2
    RUMO=rumo_lin(delta(1),delta(2),MB,PB);
    RUMO1=-rumo_lin(delta(1),delta(2),delta(3),delta(4));
    B(2,1)=RUMO(1,1)+RUMO1(1,1);
    B(2,2)=RUMO(2,1)+RUMO1(2,1);
    B(2,3)=RUMO1(3,1);
    B(2,4)=RUMO1(4,1);
    f(2,1)=-(rumo_cal(delta(1),delta(2),MB,PB)+l(2)-rumo_cal(delta(1),delta(2),delta(3),delta(4)));
    %3 
    RUMO=rumo_lin(delta(3),delta(4),delta(1),delta(2));
    RUMO1=-rumo_lin(delta(3),delta(4),ME,PE);
    B(3,1)=RUMO(3,1);
    B(3,2)=RUMO(4,1);
    B(3,3)=RUMO(1,1)+RUMO1(1,1);
    B(3,4)=RUMO(2,1)+RUMO1(2,1);
    f(3,1)=-(rumo_cal(delta(3),delta(4),delta(1),delta(2))+l(3)-rumo_cal(delta(3),delta(4),ME,PE));
    %4
    RUMO=rumo_lin(ME,PE,delta(3),delta(4));
    B(4,1)=0;
    B(4,2)=0;
    B(4,3)=RUMO(3,1);
    B(4,4)=RUMO(4,1);
    f(4,1)=-(rumo_cal(ME,PE,delta(3),delta(4))+l(4)-REF);
    %5
    DIST=-dist_hor_lin(MB,PB,delta(1),delta(2));
    B(5,1)=DIST(3,1);
    B(5,2)=DIST(4,1);
    B(5,3)=0;
    B(5,4)=0;
    f(5,1)=-(l(5)-dist_hor_cal(delta(1),delta(2),MB,PB));
    %6
    DIST=-dist_hor_lin(delta(1),delta(2),delta(3),delta(4));
    B(6,1)=DIST(1,1);
    B(6,2)=DIST(2,1);
    B(6,3)=DIST(3,1);
    B(6,4)=DIST(4,1);
    f(6,1)=-(l(6)-dist_hor_cal(delta(3),delta(4),delta(1),delta(2)));
    %7
    DIST=-dist_hor_lin(delta(3),delta(4),ME,PE);
    B(7,1)=0;
    B(7,2)=0;
    B(7,3)=DIST(1,1);
    B(7,4)=DIST(2,1);
    f(7,1)=-(l(7)-dist_hor_cal(ME,PE,delta(3),delta(4)));
    
    B;
    f;
       
    N=B'*W*B;
   
    t=B'*W*f;
    
    delta_acrescimo=inv(N)*t;
    
    %pause
    
    delta=delta+delta_acrescimo;
    
    itera=itera+1;
    
    if (abs(delta_acrescimo(1)) < CONV & abs(delta_acrescimo(2)) < CONV & abs(delta_acrescimo(3)) < CONV & abs(delta_acrescimo(4)) < CONV)
        break;
    end
    
end

disp ('Fim do processo iterativo.')
disp ('     ')

X=sprintf('Numero de iteracoes: %d',itera);
disp (X)

fprintf('Coordenadas do ponto C: (%.3f, %.3f)\n', delta(1), delta(2));
fprintf('Coordenadas do ponto D: (%.3f, %.3f)\n', delta(3), delta(4));

Qdelta=inv(N);

traco_Qdelta=trace(Qdelta);  

v=f-B*delta_acrescimo;

v_segundos(1)=v(1)*180/pi*3600;
v_segundos(2)=v(2)*180/pi*3600;
v_segundos(3)=v(3)*180/pi*3600;
v_segundos(4)=v(4)*180/pi*3600;
v_segundos
v(5);
v(6);
v(7);
v_m(1)=v(5);
v_m(2)=v(6);
v_m(3)=v(7);
v_m

Q_v=Q_l-B*inv(N)*B';

l_ajust=l+v;

Q_l_ajust=Q_l-Q_v;
        
% confirmacao
        
zero=v+B*delta_acrescimo-f

s02=v'*W*v/r

disp 'Teste da razao de variancias:'
disp '            '

quiM=icdf('chi2',1-NSIGN/2,r);
quim=icdf('chi2',NSIGN/2,r);
        
if (quim < s02*r/sigma02 & s02*r/sigma02 < quiM)
    disp 'Aceitar o ajustamento'
    op2=0;
else
    disp 'Rejeitar o ajustamento'  
    op2=1;
end

if (op2==0)

    sigma_delta=s02*Qdelta
           
    traco_sigma_delta=trace(sigma_delta)

    traco_sigma_delta=sqrt(traco_sigma_delta)
    
    sigma_l_ajust=s02*Q_l_ajust
    
end          
              
disp('    ')
disp('Fim do programa.')  
   