%ideal_angles = [150.2028 72.7162]; 
%sigma=5/3600;
%random_errors = sigma * randn(size(ideal_angles));
%angles=ideal_angles+random_errors

%ideal_distancies = [215.496 340.461 437.658 320.020]; 
%sigma=0.005;
%random_errors = sigma * randn(size(ideal_distancies));
%distancies=ideal_distancies+random_errors

% rede_2d_nulidade_3.m

clc
clear all
close all

n0=4;
n=6;
u=4;
r=n-n0;

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

% angulos observados

%a123=150.2028*pi/180;
%a412=72.7162*pi/180;

% distancias observadas

%d12=215.496;
%d23=340.461;
%d34=437.658;
%d41=320.020;

a123=150.2026*pi/180;
a412=72.7183*pi/180;

d12=215.499;
d23=340.455;
d34=437.662;
d41=320.028;
               
% incertezas dos angulos observados

sigma_a=5/3600*pi/180;
sigma_d=0.005;
sigma2=zeros(6,6);
for i=1:2
    sigma2(i,i)=sigma_a^2;
    i=i+1;
end
for i=3:6
    sigma2(i,i)=sigma_d^2;
    i=i+1;
end

Q=sigma2/sigma02;

format long

W=inv(Q);

% nome dos 4 pontos

ponto{1}='P1';
ponto{2}='P2';
ponto{3}='P3';
ponto{4}='P4';

ponto=ponto';

% coordenadas aproximadas dos parametros

X(1)=500;
X(2)=500;
X(3)=500;
X(4)=715.496;
X(5)=330.814;
X(6)=1010.945;
X(7)=194.430;
X(8)=595.079;

X=X';

% equacoes de condicao que relacionam as observacoes e os parametros

% R21+a123+v123=R23
% R14+a412+v412=R12
% 12-(d12+v12)=0
% 23-(d23+v23)=0
% 34-(d34+v34)=0
% 41-(d41+v41)=0

% rumo_cal(X2,Y2,X1,Y1)+rumo_lin(X2,Y2,X1,Y1)+a123+v123=rumo_cal(X2,Y2,X3,Y3)+rumo_lin(X2,Y2,X3,Y3)
% rumo_cal(X1,Y1,X4,Y4)+rumo_lin(X1,Y1,X4,Y4)+a412+v412=rumo_cal(X1,Y1,X2,Y2)+rumo_lin(X1,Y1,X2,Y2)
% dist_hor_cal(X1,Y1,X2,Y2)+dist_hor_lin(X1,Y1,X2,Y2)-d12-v12=0
% dist_hor_cal(X2,Y2,X3,Y3)+dist_hor_lin(X2,Y2,X3,Y3)-d23-v23=0
% dist_hor_cal(X3,Y3,X4,Y4)+dist_hor_lin(X3,Y3,X4,Y4)-d34-v34=0
% dist_hor_cal(X4,Y4,X1,Y1)+dist_hor_lin(X4,Y4,X1,Y1)-d41-v41=0

% construcao da matriz B e do vector f

B=zeros(6,8);
f=zeros(6,1);

% v123+rumo_lin(X2,Y2,X1,Y1)-rumo_lin(X2,Y2,X3,Y3)=-rumo_cal(X2,Y2,X1,Y1)+rumo_cal(X2,Y2,X3,Y3)-a123
RL=rumo_lin(X(3),X(4),X(1),X(2));
RL1=-rumo_lin(X(3),X(4),X(5),X(6));
B(1,1)=RL(3,1);
B(1,2)=RL(4,1);
B(1,3)=RL(1,1)+RL1(1,1);
B(1,4)=RL(2,1)+RL1(2,1);
B(1,5)=RL1(3,1);
B(1,6)=RL1(4,1);
f(1,1)=-rumo_cal(X(3),X(4),X(1),X(2))+rumo_cal(X(3),X(4),X(5),X(6))-a123;

% v412+rumo_lin(X1,Y1,X4,Y4)-rumo_lin(X1,Y1,X2,Y2)=-rumo_cal(X1,Y1,X2,Y2)+rumo_cal(X1,Y1,X4,Y4)-a412
RL=rumo_lin(X(1),X(2),X(7),X(8));
RL1=-rumo_lin(X(1),X(2),X(3),X(4));
B(2,1)=RL(1,1)+RL1(1,1);
B(2,2)=RL(2,1)+RL1(2,1);
B(2,3)=RL1(3,1);
B(2,4)=RL1(4,1);
B(2,7)=RL(3,1);
B(2,8)=RL(4,1);
f(2,1)=-rumo_cal(X(1),X(2),X(3),X(4))+rumo_cal(X(7),X(8),X(1),X(2))-a412;

% v12-dist_hor_lin(X1,Y1,X2,Y2)=dist_hor_cal(X1,Y1,X2,Y2)-d12  
DH=dist_hor_lin(X(1),X(2),X(3),X(4));
B(3,1)=DH(1,1);
B(3,2)=DH(2,1);
B(3,3)=DH(3,1);
B(3,4)=DH(4,1);
f(3,1)=dist_hor_cal(X(1),X(2),X(3),X(4))-d12;

% v23-dist_hor_lin(X2,Y2,X3,Y3)=dist_hor_cal(X2,Y2,X3,Y3)-d23
DH=dist_hor_lin(X(3),X(4),X(5),X(6));
B(4,3)=DH(1,1);
B(4,4)=DH(2,1);
B(4,5)=DH(3,1);
B(4,6)=DH(4,1);
f(4,1)=dist_hor_cal(X(3),X(4),X(5),X(6))-d23;

% v34-dist_hor_lin(X3,Y3,X4,Y4)=dist_hor_cal(X3,Y3,X4,Y4)-d34
DH=dist_hor_lin(X(5),X(6),X(7),X(8));
B(5,5)=DH(1,1);
B(5,6)=DH(2,1);
B(5,7)=DH(3,1);
B(5,8)=DH(4,1);
f(5,1)=dist_hor_cal(X(5),X(6),X(7),X(8))-d34;

% v41-dist_hor_lin(X4,Y4,X1,Y1)=dist_hor_cal(X4,Y4,X1,Y1)-d41 
DH=dist_hor_lin(X(7),X(8),X(1),X(2));
B(6,7)=DH(1,1);
B(6,8)=DH(2,1);
B(6,1)=DH(3,1);
B(6,2)=DH(4,1);
f(6,1)=dist_hor_cal(X(7),X(8),X(1),X(2))-d41;

% calculo da matriz normal

N=B'*W*B;
  
[linhas,colunas]=size (N);
   
dimensao=max(linhas,colunas);
    
caracteristica=rank(N);

nulidade=dimensao-caracteristica;

fprintf('     Nulidade da matriz N: %i\n', nulidade);

disp ('   ');

disp '*************************************************************************************'
disp '*                                                                                   *'
disp '*    1. Datum com variancia nula                                                    *'
disp '*    2. Datum com constrangimentos                                                  *'
disp '*    3. Datum com traco minimo da matriz cofactor dos parametros                    *'
disp '*    4. Datum com minimizacao parcial do traco da matriz cofactor dos parametros    *'
disp '*                                                                                   *'
disp '*************************************************************************************'
disp '                                                                    '

opcao=input ('        Opcao: ');
disp ('   ')

switch opcao

% *************************************************************************
    case 1 % datum com variancia nula 
% *************************************************************************

        for j=1:length(X)/2;
            if (j<10)
                str=['        ',num2str(j),' - ',char(ponto(j))];
            else
                str=['       ',num2str(j),' - ',char(ponto(j))];        
            end
            disp (str)
            j=j+1;
        end

        disp '                                                          '

        datum1=input ('Indique o primeiro ponto seleccionado para datum: ');

        disp '                                                          '

        for j=1:length(X)/2;
            if (j ~= datum1)
                if (j<10)
                    str=['        ',num2str(j),' - ',char(ponto(j))];
                else
                    str=['       ',num2str(j),' - ',char(ponto(j))];        
                end
                disp (str)
                j=j+1;
            end
        end

        disp ('   ')

        datum2=input ('Indique  o segundo ponto seleccionado para datum: ');

        c1=2*datum1-1;
        c2=2*datum1;
        c3=2*datum2-1;
        c4=2*datum2;       

        disp ('   ')

        for iteracao=1:MAXIT
    
            RL=rumo_lin(X(1),X(2),X(7),X(8));
            RL1=-rumo_lin(X(1),X(2),X(3),X(4));
            B(1,1)=RL(1,1)+RL1(1,1);
            B(1,2)=RL(2,1)+RL1(2,1);
            B(1,3)=RL1(3,1);
            B(1,4)=RL1(4,1);
            B(1,7)=RL(3,1);
            B(1,8)=RL(4,1);
            f(1,1)=-rumo_cal(X(1),X(2),X(7),X(8))+rumo_cal(X(1),X(2),X(3),X(4))-a412+2*pi;
            if f(1,1) > pi
                f(1,1)=f(1,1)-2*pi;
            end
            RL=rumo_lin(X(3),X(4),X(1),X(2));
            RL1=-rumo_lin(X(3),X(4),X(7),X(8));
            B(2,1)=RL(3,1);
            B(2,2)=RL(4,1);
            B(2,3)=RL(1,1)+RL1(1,1);
            B(2,4)=RL(2,1)+RL1(2,1);
            B(2,7)=RL1(3,1);
            B(2,8)=RL1(4,1);
            f(2,1)=-rumo_cal(X(3),X(4),X(1),X(2))+rumo_cal(X(3),X(4),X(7),X(8))-a124; 
            if f(2,1) > pi
                f(2,1)=f(2,1)-2*pi;
            end
            RL=rumo_lin(X(3),X(4),X(7),X(8));
            RL1=-rumo_lin(X(3),X(4),X(5),X(6));
            B(3,3)=RL(1,1)+RL1(1,1);
            B(3,4)=RL(2,1)+RL1(2,1);
            B(3,5)=RL1(3,1);
            B(3,6)=RL1(4,1);
            B(3,7)=RL(3,1);
            B(3,8)=RL(4,1);
            f(3,1)=-rumo_cal(X(3),X(4),X(7),X(8))+rumo_cal(X(3),X(4),X(5),X(6))-a423; 
            if f(3,1) > pi
                f(3,1)=f(3,1)-2*pi;
            end
            RL=rumo_lin(X(5),X(6),X(3),X(4));
            RL1=-rumo_lin(X(5),X(6),X(7),X(8));
            B(4,3)=RL(3,1);
            B(4,4)=RL(4,1);
            B(4,5)=RL(1,1)+RL1(1,1);
            B(4,6)=RL(2,1)+RL1(2,1);
            B(4,7)=RL1(3,1);
            B(4,8)=RL1(4,1);
            f(4,1)=-rumo_cal(X(5),X(6),X(3),X(4))+rumo_cal(X(5),X(6),X(7),X(8))-a234;
            if f(4,1) > pi
                f(4,1)=f(4,1)-2*pi;
            end
            RL=rumo_lin(X(7),X(8),X(5),X(6));
            RL1=-rumo_lin(X(7),X(8),X(3),X(4));
            B(5,3)=RL1(3,1);
            B(5,4)=RL1(4,1);
            B(5,5)=RL(3,1);
            B(5,6)=RL(4,1);
            B(5,7)=RL(1,1)+RL1(1,1);
            B(5,8)=RL(2,1)+RL1(2,1);
            f(5,1)=-rumo_cal(X(7),X(8),X(5),X(6))+rumo_cal(X(7),X(8),X(3),X(4))-a342;
            if f(5,1) > pi
                f(5,1)=f(5,1)-2*pi;
            end
            RL=rumo_lin(X(7),X(8),X(3),X(4));
            RL1=-rumo_lin(X(7),X(8),X(1),X(2));
            B(6,1)=RL1(3,1);
            B(6,2)=RL1(4,1);
            B(6,3)=RL(3,1);
            B(6,4)=RL(4,1);
            B(6,7)=RL(1,1)+RL1(1,1);
            B(6,8)=RL(2,1)+RL1(2,1);
            f(6,1)=-rumo_cal(X(7),X(8),X(3),X(4))+rumo_cal(X(7),X(8),X(1),X(2))-a241;
            if f(6,1) > pi
                f(6,1)=f(6,1)-2*pi;
            end

%       para eliminar uma coluna, utilizar o operador empty []
            
            B1=B;

            corta=[c1 c2 c3 c4];
            
            B1(:,corta)=[];
 
            B2=B(:,[c1 c2 c3 c4]);
  
%           calculo da matriz L tal que A1*L=A2 (A1_1 e a inversa generalizada de A1)

            B1_1=pinv(B1);

            L=B1_1*B2;
            
%           separacao do vector XY em XY1 e XY2, XY2 sendo o datum

            X1=X;

            X1T=transpose(X1);

            X1T(:,corta)=[];
            
            X1=transpose(X1T);
            
            XT=transpose(X);
            
            X2T=[XT(:,[c1 c2 c3 c4])];
            
            X2=transpose(X2T);
    
%           calculo do acrescimo as coordenadas aproximadas

            dX=inv(B1'*W*B1)*B1'*W*f
          
%           calculo das novas coordenadas aproximadas

            if (max(abs(dX)) > CONV)
                j=1;
                for i=1:length(X); 
                    if (i~=c1 & i~=c2 & i~=c3 & i~=c4)
                        X(i,1)=X1(j,1)+dX(j,1);
                         j=j+1;
                    end
                    i=i+1;
                end
            else
                disp ('O processo iterativo convergiu ...')
                disp '                                                         ')
                break;
            end
                     
            if (iteracao > MAXIT)
                disp ('O processo iterativo nao convergiu ...')                
                return
            end
            
            iteracao=iteracao+1;
  
        end
        
        fprintf('Numero de iterações:  %d\n', iteracao)

        disp ('   ')

        for i=1:length(X)/2; 
            fprintf('Coordenadas do ponto %s: (%9.3f, %9.3f)\n', ponto{i}, X(2*i-1), X(2*i));
            i=i+1;
        end

        disp ('   ')
       
        QX1=inv((B1'*W*B1));

        ii=1;
        for i=1:length(X);
            if (i~=c1 & i~=c2 & i~=c3 & i~=c4)
                jj=1;
                for j=1:length(X);
                    if (j~=c1 & j~=c2 & j~=c3 & j~=c4)
                        QX(i,j)=QX1(ii,jj);
                        jj=jj+1;
                    end
                    j=j+1;
                end
                ii=ii+1;
            end
            i=i+1;
        end

        QX;
        
%       disp 'Matriz cofactor das coordenadas ajustadas:'
        
        tracoQX=trace(QX);

        fprintf('Traco da matriz cofactor das coordenadas ajustadas:  %9.5f\n',tracoQX)

        disp ('   ')

%       vector dos residuos das observacoes

        v=f-B1*dX;

        v_segundos=v*180/pi*3600;

        disp ('Vector dos residuos das observacoes ("): ')
        for i=1:6
            fprintf('   %6.3f\n', v_segundos(i))
            i=i+1;
        end

        Qv=Q-B1*inv(B1'*W*B1)*B1';
        
%        l_adj=l+v;

        Ql_adj=Q-Qv;
        
        s02=v'*W*v/r;

        disp ('   ')

        fprintf('Variancia a posteriori:  %9.5f\n',s02)

        disp ('   ')

        
        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_l_ajust=s02*Q_l_ajust   
            incerteza_global=trace(sqrt(sigma_l_ajust))
        end    
               
        disp('    ')
        disp('Fim do programa.')     

% *************************************************************************
    case 2 % datum com constrangimentos
% *************************************************************************

        Rt=zeros(nulidade,length(X));
        c=zeros(nulidade,1);

        disp ('        Tipos de constrangimento numa rede 2D com nulidade = 4 ');
        disp '                                                          '
        disp ('        1. Fixar as coordenadas (X,Y) de 2 pontos da rede (d-4)');
        disp ('        2. Fixar as coordenadas (X,Y) de 1 ponto da rede (d-2)');
        disp ('        3. Fixar a distancia horizontal entre 2 pontos de rede (d-1)');
        disp ('        4. Fixar o rumo da direccao entre 2 pontos de rede (d-1)');
        disp '                                                          '

        disp(['Opcoes possiveis para o caso de o deficit de caracteristica da matriz N ser igual a d = ',num2str(nulidade)]); 
        disp ('                                                    ');
        disp ('                         1')
        disp ('               234 (2+3+4)')
        disp ('                                                    ');   
        opcao2=input('     Seleccione a opcao: ');
                                                        
        if (opcao2 == 1) % 2 pontos fixos
        
            disp ('                                                    ');   
            for j=1:length(X)/2
                if (j<10)
                    str=['        ',num2str(j),' - ',char(ponto(j))];
                else
                    str=['       ',num2str(j),' - ',char(ponto(j))];        
                end
                disp (str)
                j=j+1;
            end        
            disp ('                                                     ');
            datum1=input ('  Indique o primeiro ponto seleccionado para datum:  ');
        
            disp ('                                                    ');   
            for j=1:length(X)/2;
                if (j ~=datum1)
                    if (j<10)
                        str=['        ',num2str(j),' - ',char(ponto(j))];
                    else
                        str=['       ',num2str(j),' - ',char(ponto(j))];        
                    end
                    disp (str)
                    j=j+1;
                end
            end      
        
            disp ('                                                    ');   
        
            datum2=input ('  Indique  o segundo ponto seleccionado para datum:  ');
        
            Rt(1,2*datum1-1)=1;
            Rt(2,2*datum1)=1;
            Rt(3,2*datum2-1)=1;
            Rt(4,2*datum2)=1;
        end

        if (opcao2 == 234) % 1 ponto fixo + 1 distancia + 1 rumo
            disp ('                                                    ');   
            for j=1:length(X)/2;
                if (j<10)
                    str=['        ',num2str(j),' - ',char(ponto(j))];
                else
                    str=['       ',num2str(j),' - ',char(ponto(j))];        
                end
                disp (str)
                j=j+1;
            end        
            disp ('                                                    ');
            datum1=input ('  Indique o ponto seleccionado para datum:  ');
            disp ('                                                    ');
            disp ('  Fixe a distancia entre: ');
            ponto1=input ('    Ponto inicial: ');
            ponto2=input ('      Ponto final: ');
            disp ('                                                    ');
            disp ('  Fixe o rumo entre: ');
            ponto3=input ('    Ponto inicial: ');
            ponto4=input ('      Ponto final: ');
            Rt(1,2*datum1-1)=1;
            Rt(2,2*datum1)=1;
            Rt(3,2*ponto1-1)=-(X(2*ponto2-1)-X(2*ponto1-1));
            Rt(3,2*ponto1)=-(X(2*ponto2)-X(2*ponto1));
            Rt(3,2*ponto2-1)=(X(2*ponto2-1)-X(2*ponto1-1));
            Rt(3,2*ponto2)=(X(2*ponto2)-X(2*ponto1));
            Rt(4,2*ponto3-1)=-(X(2*ponto4)-X(2*ponto3));
            Rt(4,2*ponto3)=(X(2*ponto4-1)-X(2*ponto3-1));
            Rt(4,2*ponto4-1)=(X(2*ponto4)-X(2*ponto3));
            Rt(4,2*ponto4)=-(X(2*ponto4-1)-X(2*ponto3-1));
        end    

        R=Rt';

        for iteracao=1:MAXIT
    
            RL=rumo_lin(X(1),X(2),X(7),X(8));
            RL1=-rumo_lin(X(1),X(2),X(3),X(4));
            B(1,1)=RL(1,1)+RL1(1,1);
            B(1,2)=RL(2,1)+RL1(2,1);
            B(1,3)=RL1(3,1);
            B(1,4)=RL1(4,1);
            B(1,7)=RL(3,1);
            B(1,8)=RL(4,1);
            f(1,1)=-rumo_cal(X(1),X(2),X(7),X(8))+rumo_cal(X(1),X(2),X(3),X(4))-a412+2*pi;
            if f(1,1) > pi
                f(1,1)=f(1,1)-2*pi;
            end
            RL=rumo_lin(X(3),X(4),X(1),X(2));
            RL1=-rumo_lin(X(3),X(4),X(7),X(8));
            B(2,1)=RL(3,1);
            B(2,2)=RL(4,1);
            B(2,3)=RL(1,1)+RL1(1,1);
            B(2,4)=RL(2,1)+RL1(2,1);
            B(2,7)=RL1(3,1);
            B(2,8)=RL1(4,1);
            f(2,1)=-rumo_cal(X(3),X(4),X(1),X(2))+rumo_cal(X(3),X(4),X(7),X(8))-a124; 
            if f(2,1) > pi
                f(2,1)=f(2,1)-2*pi;
            end
            RL=rumo_lin(X(3),X(4),X(7),X(8));
            RL1=-rumo_lin(X(3),X(4),X(5),X(6));
            B(3,3)=RL(1,1)+RL1(1,1);
            B(3,4)=RL(2,1)+RL1(2,1);
            B(3,5)=RL1(3,1);
            B(3,6)=RL1(4,1);
            B(3,7)=RL(3,1);
            B(3,8)=RL(4,1);
            f(3,1)=-rumo_cal(X(3),X(4),X(7),X(8))+rumo_cal(X(3),X(4),X(5),X(6))-a423; 
            if f(3,1) > pi
                f(3,1)=f(3,1)-2*pi;
            end            
            RL=rumo_lin(X(5),X(6),X(3),X(4));
            RL1=-rumo_lin(X(5),X(6),X(7),X(8));
            B(4,3)=RL(3,1);
            B(4,4)=RL(4,1);
            B(4,5)=RL(1,1)+RL1(1,1);
            B(4,6)=RL(2,1)+RL1(2,1);
            B(4,7)=RL1(3,1);
            B(4,8)=RL1(4,1);
            f(4,1)=-rumo_cal(X(5),X(6),X(3),X(4))+rumo_cal(X(5),X(6),X(7),X(8))-a234;
            if f(4,1) > pi
                f(4,1)=f(4,1)-2*pi;
            end          
            RL=rumo_lin(X(7),X(8),X(5),X(6));
            RL1=-rumo_lin(X(7),X(8),X(3),X(4));
            B(5,3)=RL1(3,1);
            B(5,4)=RL1(4,1);
            B(5,5)=RL(3,1);
            B(5,6)=RL(4,1);
            B(5,7)=RL(1,1)+RL1(1,1);
            B(5,8)=RL(2,1)+RL1(2,1);
            f(5,1)=-rumo_cal(X(7),X(8),X(5),X(6))+rumo_cal(X(7),X(8),X(3),X(4))-a342;
            if f(5,1) > pi
                f(5,1)=f(5,1)-2*pi;
            end         
            RL=rumo_lin(X(7),X(8),X(3),X(4));
            RL1=-rumo_lin(X(7),X(8),X(1),X(2));
            B(6,1)=RL1(3,1);
            B(6,2)=RL1(4,1);
            B(6,3)=RL(3,1);
            B(6,4)=RL(4,1);
            B(6,7)=RL(1,1)+RL1(1,1);
            B(6,8)=RL(2,1)+RL1(2,1);
            f(6,1)=-rumo_cal(X(7),X(8),X(3),X(4))+rumo_cal(X(7),X(8),X(1),X(2))-a241;
            if f(6,1) > pi
                f(6,1)=f(6,1)-2*pi;
            end

%           calculo do acrescimo para as coordenadas aproximadas

            dX=inv(N+R*Rt)*N*inv(N+R*Rt)*B'*W*f + inv(N+R*Rt)*R*c;

% calculo das novas coordenadas aproximadas

            disp ('   ')

            if (max(abs(dX)) > CONV)
                for i=1:length(X);                
                    X(i,1)=X(i,1)+dX(i,1);
                 i=i+1;
                end
            else
                disp ('O processo iterativo convergiu.')
                disp '                                  '
                break;
            end

            if (iteracao > MAXIT)
                disp ('O processo iterativo nao convergiu ...')
                return
            end
        
            iteracao=iteracao+1;
        
        end
     
        fprintf('Numero de iterações:  %d\n', iteracao)

        disp ('   ')

        for i=1:length(X)/2; 
            fprintf('Coordenadas do ponto %s: (%9.3f, %9.3f)\n', ponto{i}, X(2*i-1), X(2*i));
            i=i+1;
        end

        disp ('   ')
       
        QX=inv(N+R*Rt)*N*inv(N+R*Rt);
       
%       disp 'Matriz cofactor das coordenadas ajustadas:'
        
        tracoQX=trace(QX);

        fprintf('Traco da matriz cofactor das coordenadas ajustadas:  %9.5f\n',tracoQX)

        disp ('   ')

%       vector dos residuos das observacoes

        v=f-B*dX;

        v_segundos=v*180/pi*3600;

        disp ('Vector dos residuos das observacoes ("): ')
        for i=1:6
            fprintf('   %6.3f\n', v_segundos(i))
            i=i+1;
        end

        Qv=Q-B*inv(B'*W*B)*B';
        
%        l_adj=l+v;

        Ql_adj=Q-Qv;
        
        s02=v'*W*v/r;

        disp ('   ')

        fprintf('Variancia a posteriori:  %9.5f\n',s02)

        disp ('   ')
       
        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_l_ajust=s02*Q_l_ajust   
            incerteza_global=trace(sqrt(sigma_l_ajust))
        end    
               
        disp('    ')
        ('Fim do programa.')     

% *************************************************************************
    case 3 % datum com traco da matriz cofactor dos parametros minimo
% *************************************************************************

    for iteracao=1:MAXIT

% actualizacao da matriz A

        if (iteracao>1)
            RL=rumo_lin(X(1),X(2),X(7),X(8));
            RL1=-rumo_lin(X(1),X(2),X(3),X(4));
            B(1,1)=RL(1,1)+RL1(1,1);
            B(1,2)=RL(2,1)+RL1(2,1);
            B(1,3)=RL1(3,1);
            B(1,4)=RL1(4,1);
            B(1,7)=RL(3,1);
            B(1,8)=RL(4,1);
            f(1,1)=-rumo_cal(X(1),X(2),X(7),X(8))+rumo_cal(X(1),X(2),X(3),X(4))-a412+2*pi;
            RL=rumo_lin(X(3),X(4),X(1),X(2));
            RL1=-rumo_lin(X(3),X(4),X(7),X(8));
            B(2,1)=RL(3,1);
            B(2,2)=RL(4,1);
            B(2,3)=RL(1,1)+RL1(1,1);
            B(2,4)=RL(2,1)+RL1(2,1);
            B(2,7)=RL1(3,1);
            B(2,8)=RL1(4,1);
            f(2,1)=-rumo_cal(X(3),X(4),X(1),X(2))+rumo_cal(X(3),X(4),X(7),X(8))-a124; 
            RL=rumo_lin(X(3),X(4),X(7),X(8));
            RL1=-rumo_lin(X(3),X(4),X(5),X(6));
            B(3,3)=RL(1,1)+RL1(1,1);
            B(3,4)=RL(2,1)+RL1(2,1);
            B(3,5)=RL1(3,1);
            B(3,6)=RL1(4,1);
            B(3,7)=RL(3,1);
            B(3,8)=RL(4,1);
            f(3,1)=-rumo_cal(X(3),X(4),X(7),X(8))+rumo_cal(X(3),X(4),X(5),X(6))-a423; 
            RL=rumo_lin(X(5),X(6),X(3),X(4));
            RL1=-rumo_lin(X(5),X(6),X(7),X(8));
            B(4,3)=RL(3,1);
            B(4,4)=RL(4,1);
            B(4,5)=RL(1,1)+RL1(1,1);
            B(4,6)=RL(2,1)+RL1(2,1);
            B(4,7)=RL1(3,1);
            B(4,8)=RL1(4,1);
            f(4,1)=-rumo_cal(X(5),X(6),X(3),X(4))+rumo_cal(X(5),X(6),X(7),X(8))-a234;
            RL=rumo_lin(X(7),X(8),X(5),X(6));
            RL1=-rumo_lin(X(7),X(8),X(3),X(4));
            B(5,3)=RL1(3,1);
            B(5,4)=RL1(4,1);
            B(5,5)=RL(3,1);
            B(5,6)=RL(4,1);
            B(5,7)=RL(1,1)+RL1(1,1);
            B(5,8)=RL(2,1)+RL1(2,1);
            f(5,1)=-rumo_cal(X(7),X(8),X(5),X(6))+rumo_cal(X(7),X(8),X(3),X(4))-a342;
            RL=rumo_lin(X(7),X(8),X(3),X(4));
            RL1=-rumo_lin(X(7),X(8),X(1),X(2));
            B(6,1)=RL1(3,1);
            B(6,2)=RL1(4,1);
            B(6,3)=RL(3,1);
            B(6,4)=RL(4,1);
            B(6,7)=RL(1,1)+RL1(1,1);
            B(6,8)=RL(2,1)+RL1(2,1);
            f(6,1)=-rumo_cal(X(7),X(8),X(3),X(4))+rumo_cal(X(7),X(8),X(1),X(2))-a241;
              
% calculo do acrescimo as coordenadas aproximadas

% [V,D] = eig(A) produces matrices of eigenvalues (D) and eigenvectors (V) of matrix A, so that A*V = V*D. 
% Matrix D is the canonical form of A - a diagonal matrix with A's eigenvalues on the main diagonal. 
% Matrix V is the modal matrix - its columns are the eigenvectors of A.

            [linhasN,colunasN]=size(N);

            [Vec,Val]=eig(N,'nobalance');

% matriz dos constrangimentos    
    
            for i=1:linhasN,
                for j=1:linhasN,
                    R(i,j)=Vec(i,j);
                end
            end

% calculo do vector dos parametros

            A=inv(N+R*R')*N*inv(N+R*R');

            dX=A*B'*W*f;
           
% calculo das novas coordenadas aproximadas

            if (max(abs(dX)) > CONV)
                for i=1:length(X); 
                    X(i,1)=X(i,1)+dX(i,1);
                    i=i+1;
                end
            else
                disp ('O processo iterativo convergiu ...')
                break;
            end
        
            if (iteracao > MAXIT)
                disp ('O processo iterativo nao convergiu ...')
                break;
            end
                    
            iteracao=iteracao+1;

        end
            
    end    

    disp ('   ')
   
    fprintf('Numero de iterações:  %d\n', iteracao)

    disp ('   ')

    for i=1:length(X)/2; 
        fprintf('Coordenadas do ponto %s: (%9.3f, %9.3f)\n', ponto{i}, X(2*i-1), X(2*i));
        i=i+1;
    end

    disp ('   ')
      
    QX=inv(N+R*R')*N*inv(N+R*R');
       
%   disp 'Matriz cofactor das coordenadas ajustadas:'
        
    tracoQX=trace(QX);

    fprintf('Traco da matriz cofactor das coordenadas ajustadas:  %9.5f\n',tracoQX)

    disp ('   ')

%   vector dos residuos das observacoes

    v=f-B*dX;

    v_segundos=v*180/pi*3600;

    disp ('Vector dos residuos das observacoes ("): ')
    for i=1:6
        fprintf('   %6.3f\n', v_segundos(i))
        i=i+1;
    end

    Qv=Q-B*inv(B'*W*B)*B';
        
%    l_adj=l+v;

    Ql_adj=Q-Qv;
        
    s02=v'*W*v/r;

    disp ('   ')

    fprintf('Variancia a posteriori:  %9.5f\n',s02)

    disp ('   ')
       
    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_l_ajust=s02*Q_l_ajust   
        incerteza_global=trace(sqrt(sigma_l_ajust))
    end    
               
    disp('    ')
    disp('Fim do programa.')     

% *************************************************************************
    case 4 % datum com traco parcial da matriz cofactor dos parametros minimo
% *************************************************************************

    for j=1:length(X)/2;
       if (j<10)
           str=['        ',num2str(j),' - ',char(ponto(j))];
        else
            str=['       ',num2str(j),' - ',char(ponto(j))];        
        end
        disp (str)
        j=j+1;
    end

    disp '                                                          '

    num=input ('Indique o numero de pontos do datum: ');
    disp '(cada ponto contribui com 2 parametros: total parametros>d)'
    if (2*num < nulidade+1)
        disp '                                                      '
        disp 'Numero de pontos insuficiente...'
        stop;
    end

    disp '                                                          '

    for i=1:num
        datum1=input ('Indique um ponto seleccionado para datum: ');
        datum(i)=datum1;
    end
  
    for i=1:num
        corta(2*i-1)=2*datum(i)-1;
        corta(2*i)=2*datum(i);
    end
    
    R=zeros(length(X),2*num);  
    A=zeros(length(X),length(X));
    S=zeros(length(X),2*num);

    % inicio do processo iterativo

    for iteracao=1:MAXIT
        RL=rumo_lin(X(1),X(2),X(7),X(8));
        RL1=-rumo_lin(X(1),X(2),X(3),X(4));
        B(1,1)=RL(1,1)+RL1(1,1);
        B(1,2)=RL(2,1)+RL1(2,1);
        B(1,3)=RL1(3,1);
        B(1,4)=RL1(4,1);
        B(1,7)=RL(3,1);
        B(1,8)=RL(4,1);
        f(1,1)=-rumo_cal(X(1),X(2),X(7),X(8))+rumo_cal(X(1),X(2),X(3),X(4))-a412+2*pi;
        RL=rumo_lin(X(3),X(4),X(1),X(2));
        RL1=-rumo_lin(X(3),X(4),X(7),X(8));
        B(2,1)=RL(3,1);
        B(2,2)=RL(4,1);
        B(2,3)=RL(1,1)+RL1(1,1);
        B(2,4)=RL(2,1)+RL1(2,1);
        B(2,7)=RL1(3,1);
        B(2,8)=RL1(4,1);
        f(2,1)=-rumo_cal(X(3),X(4),X(1),X(2))+rumo_cal(X(3),X(4),X(7),X(8))-a124; 
        RL=rumo_lin(X(3),X(4),X(7),X(8));
        RL1=-rumo_lin(X(3),X(4),X(5),X(6));
        B(3,3)=RL(1,1)+RL1(1,1);
        B(3,4)=RL(2,1)+RL1(2,1);
        B(3,5)=RL1(3,1);
        B(3,6)=RL1(4,1);
        B(3,7)=RL(3,1);
        B(3,8)=RL(4,1);
        f(3,1)=-rumo_cal(X(3),X(4),X(7),X(8))+rumo_cal(X(3),X(4),X(5),X(6))-a423; 
        RL=rumo_lin(X(5),X(6),X(3),X(4));
        RL1=-rumo_lin(X(5),X(6),X(7),X(8));
        B(4,3)=RL(3,1);
        B(4,4)=RL(4,1);
        B(4,5)=RL(1,1)+RL1(1,1);
        B(4,6)=RL(2,1)+RL1(2,1);
        B(4,7)=RL1(3,1);
        B(4,8)=RL1(4,1);
        f(4,1)=-rumo_cal(X(5),X(6),X(3),X(4))+rumo_cal(X(5),X(6),X(7),X(8))-a234;
        RL=rumo_lin(X(7),X(8),X(5),X(6));
        RL1=-rumo_lin(X(7),X(8),X(3),X(4));
        B(5,3)=RL1(3,1);
        B(5,4)=RL1(4,1);
        B(5,5)=RL(3,1);
        B(5,6)=RL(4,1);
        B(5,7)=RL(1,1)+RL1(1,1);
        B(5,8)=RL(2,1)+RL1(2,1);
        f(5,1)=-rumo_cal(X(7),X(8),X(5),X(6))+rumo_cal(X(7),X(8),X(3),X(4))-a342;
        RL=rumo_lin(X(7),X(8),X(3),X(4));
        RL1=-rumo_lin(X(7),X(8),X(1),X(2));
        B(6,1)=RL1(3,1);
        B(6,2)=RL1(4,1);
        B(6,3)=RL(3,1);
        B(6,4)=RL(4,1);
        B(6,7)=RL(1,1)+RL1(1,1);
        B(6,8)=RL(2,1)+RL1(2,1);
        f(6,1)=-rumo_cal(X(7),X(8),X(3),X(4))+rumo_cal(X(7),X(8),X(1),X(2))-a241;
     
% para eliminar uma coluna, utilizar o operador empty []

        B1=B;                   
        B1(:,corta)=[];
        B2=B(:,corta);
            
% calculo da matriz L tal que A1*L=A2 (A1_1 e a inversa generalizada de A1)

        B1_1=pinv(B1);
        L=B1_1*B2;
                
% calculo da matriz S
        
        for i=1:2*num
            S(corta(i),i)=-1;
        end
        
        if (num == 3)
            for i=1:(2*datum(1)-2)
                for j=1:2*num
                    S(i,j)=L(i,j);
                end
            end
            for i=(2*datum(1)+1):(2*datum(2)-2)
                for j=1:2*num
                    S(i,j)=L(i-2,j);
                end
            end
            for i=(2*datum(2)+1):(2*datum(3)-2)
                for j=1:2*num
                    S(i,j)=L(i-4,j);
                end
            end
            for i=(2*datum(3)+1):length(X)
                for j=1:2*num
                    S(i,j)=L(i-6,j);
                end
            end
        end

        if (num == 4)
            for i=1:(2*datum(1)-2)
                for j=1:2*num
                    S(i,j)=L(i,j);
                end
            end
            for i=(2*datum(1)+1):(2*datum(2)-2)
                for j=1:2*num
                    S(i,j)=L(i-2,j);
                end
            end
            for i=(2*datum(2)+1):(2*datum(3)-2)
                for j=1:2*num
                    S(i,j)=L(i-4,j);
                end
            end
            for i=(2*datum(3)+1):(2*datum(4)-2)
                for j=1:2*num
                    S(i,j)=L(i-6,j);
                end
            end            
            for i=(2*datum(4)+1):length(X)
                for j=1:2*num
                    S(i,j)=L(i-8,j);
                end
            end
        end
        
        if (num == 5)
            for i=1:(2*datum(1)-2)
                for j=1:2*num
                    S(i,j)=L(i,j);
                end
            end
            for i=(2*datum(1)+1):(2*datum(2)-2)
                for j=1:2*num
                    S(i,j)=L(i-2,j);
                end
            end
            for i=(2*datum(2)+1):(2*datum(3)-2)
                for j=1:2*num
                    S(i,j)=L(i-4,j);
                end
            end
            for i=(2*datum(3)+1):(2*datum(4)-2)
                for j=1:2*num
                    S(i,j)=L(i-6,j);
                end
            end  
            for i=(2*datum(4)+1):(2*datum(5)-2)
                for j=1:2*num
                    S(i,j)=L(i-8,j);
                end
            end             
            for i=(2*datum(5)+1):length(X)
                for j=1:2*num
                    S(i,j)=L(i-10,j);
                end
            end
        end
 
        if (num == 6)
            for i=1:(2*datum(1)-2)
                for j=1:2*num
                    S(i,j)=L(i,j);
                end
            end
            for i=(2*datum(1)+1):(2*datum(2)-2)
                for j=1:2*num
                    S(i,j)=L(i-2,j);
                end
            end
            for i=(2*datum(2)+1):(2*datum(3)-2)
                for j=1:2*num
                    S(i,j)=L(i-4,j);
                end
            end
            for i=(2*datum(3)+1):(2*datum(4)-2)
                for j=1:2*num
                    S(i,j)=L(i-6,j);
                end
            end  
            for i=(2*datum(4)+1):(2*datum(5)-2)
                for j=1:2*num
                    S(i,j)=L(i-8,j);
                end
            end       
            for i=(2*datum(5)+1):(2*datum(6)-2)
                for j=1:2*num
                    S(i,j)=L(i-10,j);
                end
            end            
            for i=(2*datum(6)+1):length(X)
                for j=1:2*num
                    S(i,j)=L(i-12,j);
                end
            end
        end
        
        if (num == 7)
            for i=1:(2*datum(1)-2)
                for j=1:2*num
                    S(i,j)=L(i,j);
                end
            end
            for i=(2*datum(1)+1):(2*datum(2)-2)
                for j=1:2*num
                    S(i,j)=L(i-2,j);
                end
            end
            for i=(2*datum(2)+1):(2*datum(3)-2)
                for j=1:2*num
                    S(i,j)=L(i-4,j);
                end
            end
            for i=(2*datum(3)+1):(2*datum(4)-2)
                for j=1:2*num
                    S(i,j)=L(i-6,j);
                end
            end  
            for i=(2*datum(4)+1):(2*datum(5)-2)
                for j=1:2*num
                    S(i,j)=L(i-8,j);
                end
            end       
            for i=(2*datum(5)+1):(2*datum(6)-2)
                for j=1:2*num
                    S(i,j)=L(i-10,j);
                end
            end
            for i=(2*datum(6)+1):(2*datum(7)-2)
                for j=1:2*num
                    S(i,j)=L(i-12,j);
                end
            end            
            for i=(2*datum(7)+1):length(X)
                for j=1:2*num
                    S(i,j)=L(i-14,j);
                end
            end
        end        
        
        if (num > 7)
            disp ' num demasiado grande'            
            break;
        end
         
% calculo da matriz A

        for i=1:2*num
            A(corta(i),corta(i))=-1;
        end     
        
% calculo da matriz R dos constrangimentos        
        
        R=A*S

% calculo do vector dos parametros

        A1=inv(N+R*R')*N*inv(N+R*R');

        dX=A1*B'*W*f;
           
% calculo das novas coordenadas aproximadas

        if (max(abs(dX)) > CONV)
            for i=1:length(X); 
                X(i,1)=X(i,1)+dX(i,1);
                i=i+1;
            end
        else
            disp ('O processo iterativo convergiu.')
            disp ('                                                    ')
            break;
        end
        
        if (iteracao > MAXIT)
            disp ('O processo iterativo nao convergiu ...')
            stop       
        end
            
        iteracao=iteracao+1;
    
    end
    
    disp ('   ')

    for i=1:length(X)/2; 
        fprintf('Coordenadas do ponto %s: (%9.3f, %9.3f)\n', ponto{i}, X(2*i-1), X(2*i));
        i=i+1;
    end

    disp ('   ')

    
    QX=inv(N+R*R')*N*inv(N+R*R');

    tracoQX=trace(QX);

    fprintf('Traco da matriz cofactor das coordenadas ajustadas:  %9.5f\n',tracoQX)

    disp ('   ')
    disp 'Vector dos residuos das observacoes:'
    v=B*dX-f

    Qv=Q-B*inv(B'*W*B)*B';

    %l_adj=l+v;
 
    Ql_adj=Q-Qv;

    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_l_ajust=s02*Q_l_ajust   
        incerteza_global=trace(sqrt(sigma_l_ajust))
    end    
               
    disp('    ')
    disp('Fim do programa.')     
    
end

