clc; clear all; close all;

% variancia a priori
sigma_priori_2=1

% vector do nome dos parametros
nome={'   P7';'    C';'    P';'    B';'    A';' AUX1';'   P8';' AUX2';' AUX3';'   P3';' AUX4';'   P1';' AUX5'}

% matriz dos coeficientes dos parametros B(14,13)
B=[-1 0 0 0 0 0 0 0 0 0 0 0 0;
    1 -1 0 0 0 0 0 0 0 0 0 0 0;
    0 1 -1 0 0 0 0 0 0 0 0 0 0;
    0 0 1 -1 0 0 0 0 0 0 0 0 0;
    0 0 0 1 -1 0 0 0 0 0 0 0 0;
    0 0 0 0 1 -1 0 0 0 0 0 0 0;
    0 0 0 0 0 1 -1 0 0 0 0 0 0;
    0 0 0 0 0 0 1 -1 0 0 0 0 0
    0 0 0 0 0 0 0 1 -1 0 0 0 0
    0 0 0 0 0 0 0 0 1 -1 0 0 0
    0 0 0 0 0 0 0 0 0 1 -1 0 0
    0 0 0 0 0 0 0 0 0 0 1 -1 0
    0 0 0 0 0 0 0 0 0 0 0 1 -1
    0 0 0 0 0 0 0 0 0 0 0 0 1]

% vector das constantes (cotas conhecidas) d(14,1)
d=[80.108;0;0;0;0;0;0;0;0;0;0;0;0;-80.562]; %m

% vector das observacoes l(14,1)
l=[-1.854;-0.051;-0.215;0.582;-0.193;-0.747;0.103;-0.543;-0.452;-0.071;2.790;0.790;0.090;0.216] %m

% matriz das distancias entre miras (soma da distancia atras e distancia a frente) dist(14,14)
dist=[54.30 0 0 0 0 0 0 0 0 0 0 0 0 0;
      0 40.40 0 0 0 0 0 0 0 0 0 0 0 0;
      0 0 30.60 0 0 0 0 0 0 0 0 0 0 0;
      0 0 0 38.20 0 0 0 0 0 0 0 0 0 0;
      0 0 0 0 57.50 0 0 0 0 0 0 0 0 0;
      0 0 0 0 0 38.00 0 0 0 0 0 0 0 0;
      0 0 0 0 0 0 46.70 0 0 0 0 0 0 0;
      0 0 0 0 0 0 0 53.60 0 0 0 0 0 0;
      0 0 0 0 0 0 0 0 49.60 0 0 0 0 0;
      0 0 0 0 0 0 0 0 0 30.90 0 0 0 0;
      0 0 0 0 0 0 0 0 0 0 40.50 0 0 0;
      0 0 0 0 0 0 0 0 0 0 0 38.00 0 0;
      0 0 0 0 0 0 0 0 0 0 0 0 44.40 0;
      0 0 0 0 0 0 0 0 0 0 0 0 0 27.90] %m

f=-d-l;

% incerteza do aparelho (0.7mm/km nivelamento duplo)
s_Wild_NA2=0.0007 %m

% incerteza de cada desnivel
s_desniveis_2=(s_Wild_NA2*sqrt(2*dist/1000))^2

% pesos de cada desnivel
pesos=zeros(14,14);

pesos(1,1)=1/s_desniveis_2(1,1);
pesos(2,2)=1/s_desniveis_2(2,2);
pesos(3,3)=1/s_desniveis_2(3,3);
pesos(4,4)=1/s_desniveis_2(4,4);
pesos(5,5)=1/s_desniveis_2(5,5);
pesos(6,6)=1/s_desniveis_2(6,6);
pesos(7,7)=1/s_desniveis_2(7,7);
pesos(8,8)=1/s_desniveis_2(8,8);
pesos(9,9)=1/s_desniveis_2(9,9);
pesos(10,10)=1/s_desniveis_2(10,10);
pesos(11,11)=1/s_desniveis_2(11,11);
pesos(12,12)=1/s_desniveis_2(12,12);
pesos(13,13)=1/s_desniveis_2(13,13);
pesos(14,14)=1/s_desniveis_2(14,14);

pesos

% ajustamento por minimos quadrados

N=B'*pesos*B
t=B'*pesos*f

% calculo dos parametros
delta=inv(N)*t

% calculo da matriz cofactor dos parametros
Q_delta=inv(N)

% calculo dos residuos
v=f-B*delta

% observacoes ajustadas
l_ajustadas=l+v

% calculo da matriz cofactor das observacoes ajustadas
Q_l_ajustadas=B*inv(N)*B'

% redundancia ou graus de liberdade (n. observacoes-n.parametros)
r=14-13;

% calculo da variancia a posteriori
sigma_posteriori_2=v'*pesos*v/r

% calculo da matriz de variancias-covariancias dos parametros
sigma_delta=sigma_posteriori_2*Q_delta

for i=1:13
    fprintf('Cota ajustada do ponto %s %s %5.3f %s %5.3f\n',nome{i},' = ',delta(i),'±',sqrt(sigma_delta(i,i)));
end

% calculo da matriz de variancias-covariancias das observacoes ajustadas
sigma_l_ajustadas=sigma_posteriori_2*Q_l_ajustadas

% distribuicao do qui-quadrado a 0.05%
qui=icdf('chi2',1-0.05,r);

% teste da razao de variancias

if (sigma_posteriori_2*r/sigma_priori_2 < qui)
% sigma02 e s02 estatisticamente semelhantes    
    'Aceitar o ajustamento.'
else
    'Rejeitar o ajustamento.'
end
