close all;
clear all;
clc;

char metodo;
char datum;

disp('****************************************************************')
disp('*                  Ajustamento de uma rede 1D                  *')
disp('****************************************************************')
disp(' ');

% vector l das 6 observacoes (desniveis) efectuadas (m)

l=[0.0012;0.0016;0.0017;0.0012;0.0021;0.0013]

rede_1d = imread('rede_1d_4pontos.png');
imshow(rede_1d)

pause

% redundancia=numero obs (6)-numero minimo de variaveis que determinam o modelo(4):1 cota+3 desniveis
% numero de parametros u=4
% numero de equacoes de observacao=u+r=4+2=6

redundancia=2;

% matriz sigma_obs das variancias-covariancias das observacoes (m**2): matriz diagonal

% a) observacoes nao correlacionadas de igual precisao

%sigma_obs=[0.001^2 0 0 0 0 0;0 0.001^2 0 0 0 0;0 0 0.001^2 0 0 0;0 0 0 0.001^2 0 0;0 0 0 0 0.001^2 0;0 0 0 0 0 0.001^2]

% b) observacoes nao correlacionadas de precisao diferente

%sigma_obs=[0.0011^2 0 0 0 0 0;0 0.0012^2 0 0 0 0;0 0 0.0013^2 0 0 0;0 0 0 0.0014^2 0 0;0 0 0 0 0.0015^2 0;0 0 0 0 0 0.0016^2]

sigma_obs=[0.0010^2 0 0 0 0 0;0 0.0015^2 0 0 0 0;0 0 0.0020^2 0 0 0;0 0 0 0.0025^2 0 0;0 0 0 0 0.0030^2 0;0 0 0 0 0 0.0035^2]

% variancia de referencia a priori

sigma02=1;

% matriz cofactor das observacoes

Qobs=1/sigma02*sigma_obs

% matriz P dos pesos das observacoes

P=inv(Qobs)

% matriz A dos coeficientes dos parametros (matriz de configuracao)

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

[linhasA,colunasA]=size(A);

linhasA

colunasA

caracteristicaA=rank(A);

% matriz normal

N=A'*P*A

inv(N)

[linhasN,colunasN]=size(N);

linhasN=linhasN
colunasN=colunasN

caracteristicaN=rank(N);

caracteristicaN=caracteristicaN

% nulidade ou deficit de caracteristica da matriz N

nulidadeN=linhasN-caracteristicaN;

nulidadeN

if (nulidadeN>=0)
  disp(' ');
  disp('  Matriz normal singular.')   
  disp(' ');
end

disp('****************************************************************')
disp(' ');
disp('  Metodos de estabelecimento do datum');
disp(' ');
disp('  1: datum com variancia zero');
disp('  2: datum com constrangimentos');
disp('  3: datum com traco minimo da matriz var/covar dos parametros (variancia minima)');
disp('  4: datum com traco minimo da matriz var/covar dos parametros com utilizacao de inversas generalizadas');
disp('  5: datum com traco parcial minimo da matriz var/covar dos parametros');
disp('  6: mudanca de datum');
disp(' ');
disp('****************************************************************')
disp(' ');

metodo=input ('Indique o metodo de ajustamento da rede a utilizar: ');
disp(' ');

switch metodo
    
  case 1 

%inicio opcao1: datum com variancia zero *******************************************
 
    datum=input ('Indique o ponto seleccionado para datum: ');
    disp(' ');

    switch datum
      case 1
% ponto P1 = datum
        cota=input ('Indique a cota do ponto seleccionado para datum: ');
        x2(1,1)=cota;
% combinacao linear entre as 3 colunas restantes para igualarem a coluna eliminada
        L=[-1;-1;-1]
% matriz de configuracao restante
        A1=[1 0 0;0 1 0;0 0 1;-1 1 0;-1 0 1;0 -1 1]            
      case 2
% ponto P2 = datum
        cota=input ('Indique a cota do ponto seleccionado para datum: ');
        x2(1,1)=cota
% combinacao linear entre as 3 colunas restantes para igualarem a coluna eliminada
        L=[-1;-1;-1]
% matriz de configuracao restante
        A1=[-1 0 0;-1 1 0;-1 0 1;0 1 0;0 0 1;0 -1 1]          
      case 3
% ponto P3 = datum
        cota=input ('Indique a cota do ponto seleccionado para datum: ');
        x2(1,1)=cota
% combinacao linear entre as 3 colunas restantes para igualarem a coluna eliminada
        L=[-1;-1;-1]
% matriz de configuracao restante
        A1=[-1 1 0;-1 0 0;-1 0 1;0 -1 0;0 -1 1;0 0 1]           
      case 4
% ponto P4 = datum
        cota=input ('Indique a cota do ponto seleccionado para datum: ');
        x2(1,1)=cota
% combinacao linear entre as 3 colunas restantes para igualarem a coluna eliminada
        L=[-1;-1;-1]
% matriz de configuracao restante
        A1=[-1 1 0;-1 0 1;-1 0 0;0 -1 1;0 -1 0;0 0 -1]       
      case 5
% pontos P3 e P4 = datum (rede constrangida, isto e, fixam-se mais pontos do que os
% estritamente necessarios para definir o datum)
        A1=[-1 1;-1 0;-1 0;0 -1;0 -1;0 0] 
        A2=[0 0;1 0;0 1;1 0;0 1;-1 1] 
        cota=input ('Indique a cota do 1 ponto seleccionado para datum: ');
        cota1=input ('Indique a cota do 2 ponto seleccionado para datum: ');
        x2(1,1)=cota
        x2(2,1)=cota1
      otherwise
        fprintf ('Esse ponto nao existe!');  
        %break
    end
     
% calculo do vector dos parametros         
    if datum == 1
      x1=inv(A1'*P*A1)*A1'*P*(l-A1*L*x2)
    end
    if datum == 2
      x1=inv(A1'*P*A1)*A1'*P*(l-A1*L*x2)
    end
    if datum == 3
      x1=inv(A1'*P*A1)*A1'*P*(l-A1*L*x2)
    end
    if datum == 4
      x1=inv(A1'*P*A1)*A1'*P*(l-A1*L*x2)
    end 
    if datum == 5
      x1=inv(A1'*P*A1)*A1'*P*(l-A2*x2) 
    end
 
% calculo da matriz cofactor dos parametros

    Qpar=inv(A1'*P*A1)

    traco=Qpar(1,1)+Qpar(2,2)+Qpar(3,3)

% calculo do vector dos residuos

    if datum ==1
      res=A1*x1+A1*L*x2-l
    end
    if datum ==2
      res=A1*x1+A1*L*x2-l
    end
    if datum ==3
      res=A1*x1+A1*L*x2-l
    end
    if datum ==4
      res=A1*x1+A1*L*x2-l
    end
    if datum == 5
      res=A1*x1+A2*x2-l
    end

% variancia de referencia a posteriori    
    
    s02=res'*P*res/redundancia
    
% teste de hipoteses do qui-quadrado para aceitacao/rejeicao do ajustamento
% admitindo um nivel de significancia igual a 5% (s02 estatisticamente
% igual a sigma02)

    if (redundancia*s02/sigma02<12.838)
        disp('***** aceitar ajustamento *********************************')
    else
        disp('***** rejeitar ajustamento ********************************')
    end 
    
% calculo da matriz cofactor dos residuos

    Qres=Qobs-A1*inv(A1'*P*A1)*A1'

% calculo da matriz cofactor das observacoes ajustadas

    Qobs_ajust=A1*inv(A1'*P*A1)*A1'

% calculo das observacoes ajustadas

    lajust=l+res
    
    disp('Fim do ajustamento.');
    
    %break;

%inicio opcao2: datum com constrangimentos *****************************************
 
  case 2 
 
    datum=input ('Indique o ponto seleccionado para datum: ');
    disp(' ');
    
    switch datum
        
      case 1
% ponto P1 = datum
        cota=input ('Indique a cota do ponto seleccionado para datum: ');
        c(1,1)=cota
        R=[1; 0; 0; 0]
      case 2
% ponto P2 = datum
        cota=input ('Indique a cota do ponto seleccionado para datum: ');
        c(1,1)=cota  
        R=[0; 1; 0; 0]
      case 3
% ponto P3 = datum
        cota=input ('Indique a cota do ponto seleccionado para datum: ');
        c(1,1)=cota
        R=[0; 0; 1; 0]
      case 4
% ponto P4 = datum
        cota=input ('Indique a cota do ponto seleccionado para datum: ');
        c(1,1)=cota
        R=[0; 0; 0; 1]
      case 5
% datum fixando varios pontos (rede constrangida): R consiste num vector que nao seja combinacao linear das linhas de A,
% como e o caso de R=[1; -1; 0; 0], que nao e suficiente para a definicao do datum (a diferenca de cotas entre P1 e P2 e constante)
        cota=input ('Indique a cota do 1 ponto seleccionado para datum: ');
        cota1=input ('Indique a cota do 2 ponto seleccionado para datum: ');
        c(1,1) = cota+cota1
        R=[1; 1; 0; 0]   
      otherwise
        fprintf ('Esse ponto nao existe!');  
        %break
    end
       
% calculo do vector dos parametros         
            
    x=inv(N+R*R')*N*inv(N+R*R')*A'*P*l + inv(N+R*R')*R*c

% calculo da matriz cofactor dos parametros

    Qpar=inv(N+R*R')*N*inv(N+R*R')

    traco=Qpar(1,1)+Qpar(2,2)+Qpar(3,3)+Qpar(4,4)

% calculo do vector dos residuos das observacoes

    res=A*x-l

% variancia de referencia a posteriori    
    
    s02=res'*P*res/redundancia
    
% teste de hipoteses do qui-quadrado para aceitacao/rejeicao do ajustamento
% admitindo um nivel de significancia igual a 5% (s02 estatisticamente
% igual a sigma02)

    if (redundancia*s02/sigma02<12.838)
        disp('***** aceitar ajustamento *********************************')
    else
        disp('***** rejeitar ajustamento ********************************')
    end     
    
% calculo da matriz cofactor dos residuos

    Qres=Qobs-A*Qpar*A'

% calculo da matriz cofactor das observacoes ajustadas

    Qobs_ajust=A*Qpar*A'

% calculo das observacoes ajustadas

    lajust=l+res
    
    disp('Fim do ajustamento.');
  
%inicio opcao3: datum de traco minimo (variancia minima)****************************

  case 3

% [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.
 
    [Vec,Val]=eig(N,'nobalance');
    
    Val;

    Vec;

    lambda=eig(N);
       
% escolher os vectores proprios associados aos valores proprios nulos, que 
% vao formar a matriz S(u,d)

    S=zeros(linhasN,nulidadeN);     

    for j=1:linhasN
        if lambda(j)<0.0001;
            for i=1:linhasN
                S(i,j)=Vec(i,j);
                i=i+1;
            end
        end
        j=j+1;
    end
    
    S
    
% matriz B(u,u)    
    
    B=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
    
% matriz dos constrangimentos R(u,d)

    R=B*S;

% calculo do vector dos parametros

    x=inv(N+R*R')*N*inv(N+R*R')*A'*P*l
    
% calculo do vector dos residuos das observacoes

    res=A*x-l

% variancia de referencia a posteriori    
    
    s02=res'*P*res/redundancia
    
% teste de hipoteses do qui-quadrado para aceitacao/rejeicao do ajustamento
% admitindo um nivel de significancia igual a 5% (s02 estatisticamente
% igual a sigma02)

    if (redundancia*s02/sigma02<12.838)
        disp('***** aceitar ajustamento *********************************')
    else
        disp('***** rejeitar ajustamento ********************************')
    end        

    Qpar=inv(N+R*R')*N*inv(N+R*R')

    traco=Qpar(1,1)+Qpar(2,2)+Qpar(3,3)+Qpar(4,4)
    
    disp('Fim do ajustamento.');

%inicio opcao4: utilizacao de inversas generalizadas *******************************

  case 4

    Ng=pinv(N);
    
    Ng
    
    x=Ng*A'*l;
    
    x
    
    Qpar=Ng;
    
    Ng

    traco=Qpar(1,1)+Qpar(2,2)+Qpar(3,3)+Qpar(4,4);
    
    traco
    
    residuos=A*x-l;
    
    residuos

    disp('Fim do ajustamento.');
    
%inicio opcao5: datum de traco parcial minimo ****************************

  case 5

% [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.
 
    [Vec,Val]=eig(N,'nobalance');
    
    Val;

    Vec;

    lambda=eig(N);
       
% escolher os vectores proprios associados aos valores proprios nulos, que 
% vao formar a matriz S(u,d)

    S=zeros(linhasN,nulidadeN);     

    for j=1:linhasN
        if lambda(j)<0.0001;
            for i=1:linhasN
                S(i,j)=Vec(i,j);
                i=i+1;
            end
        end
        j=j+1;
    end
    
    S;

% matriz dos constrangimentos    

    disp(' ');
    parcial=input ('Quantos pontos definem o datum (2,3)? ');
    disp(' ');

    if (parcial==2)
        disp(' ');
        Ponto1=input('Ponto 1: ');
        Ponto2=input('Ponto 2: ');
        if (Ponto1==1 & Ponto2==2)
              B=[1 0 0 0;0 1 0 0;0 0 0 0;0 0 0 0];
        end
        if (Ponto1==1 & Ponto2==3)
              B=[1 0 0 0;0 0 0 0;0 0 1 0;0 0 0 0];
        end
        if (Ponto1==1 & Ponto2==4)
              B=[1 0 0 0;0 0 0 0;0 0 0 0;0 0 0 1];
        end
        if (Ponto1==2 & Ponto2==3)
              B=[0 0 0 0;0 1 0 0;0 0 1 0;0 0 0 0];
        end
        if (Ponto2==1 & Ponto2==4)
              B=[1 0 0 0;0 0 0 0;0 0 0 0;0 0 0 1];
        end
        if (Ponto1==3 & Ponto2==4)
              B=[0 0 0 0;0 0 0 0;0 0 1 0;0 0 0 1];
        end
    end
    
    if (parcial==3)
        disp(' ');
        Ponto1=input('Ponto 1:');
        Ponto2=input('Ponto 2:');
        Ponto3=input('Ponto 3:');
        if (Ponto1==1 & Ponto2==2 & Ponto3==3)
              B=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 0];
        end
        if (Ponto1==1 & Ponto2==2 & Ponto3==4)
              B=[1 0 0 0;0 1 0 0;0 0 0 0;0 0 0 1];
        end
        if (Ponto1==1 & Ponto2==3 & Ponto3==4)
              B=[1 0 0 0;0 0 0 0;0 0 1 0;0 0 0 1];
        end
        if (Ponto1==2 & Ponto2==3 & Ponto3==4)
              B=[0 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];
        end
    end
    
    if (parcial<2 & parcial>3)
        disp('Datum mal definido.');
        %break;
    end
    
    B;
    S;

% matriz dos constrangimentos

    R=B*S

% calculo do vector dos parametros

    x=inv(N+R*R')*N*inv(N+R*R')*A'*P*l
    
% calculo do vector dos residuos das observacoes

    res=A*x-l

% variancia de referencia a posteriori    
    
    s02=res'*P*res/redundancia
    
% teste de hipoteses do qui-quadrado para aceitacao/rejeicao do ajustamento
% admitindo um nivel de significancia igual a 5% (s02 estatisticamente
% igual a sigma02)

    if (redundancia*s02/sigma02<12.838)
        disp('***** aceitar ajustamento *********************************')
    else
        disp('***** rejeitar ajustamento ********************************')
    end        

    Qpar=inv(N+R*R')*N*inv(N+R*R')

    traco=Qpar(2,2)+Qpar(3,3)
    
    disp('Fim do ajustamento.');

%inicio opcao6: mudanca de datum  *****************************************

  case 6
    
% por hipotese, sao conhecidos o vector dos parametros e a matriz de var/covar
% num dado datum (datum com variancia nula, datum ni ponto 1, cota 0)

    x_datum1=[0.0000;0.0006;0.0015;0.0024];

    Qx_datum1=1.0e-006*[0.0000 0.0000 0.0000 0.0000;0.0000 0.5000 0.2500 0.2500;0.0000 0.2500 0.5000 0.2500;0.0000 0.2500 0.2500 0.5000]
    
% por hipotese, pretende-se efectuar uma mudanca de datum para o caso
% do datum de variancia nula:

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

    Vec;

    lambda=eig(N);
       
% escolher os vectores proprios associados aos valores proprios nulos, que 
% vao formar a matriz S(u,d)

    S=zeros(linhasN,nulidadeN);     

    for j=1:linhasN
        if lambda(j)<0.0001;
            for i=1:linhasN
                S(i,j)=Vec(i,j);
                i=i+1;
            end
        end
        j=j+1;
    end
    
    S;

% matriz dos constrangimentos    
    
    B=[1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];

    R=B*S;

% calculo da matriz da trasformacao Kt

    I4=eye(4)
   
    Kt=I4-S*inv(R'*S)*R'
    
    x_datum2=Kt*x_datum1
    
    Qx_datum2=Kt*Qx_datum1*Kt'
    
end    

