% BASE DE CALIBRAÇÂO DO LNEC: 5 PILARES => 10 distancias

% ver "EDM zero correction calibration", Peter A. Steeves, univ. Toronto
%
% ver "ISO17123-4"
%
% ver "Diseno de bases da calibracion para instrumentos de medicion
% electro-opticas de distancias", J. Romano, P. Paus, J. Bergamini, R.
% Aldasoro, UNCA, ENIA

clc;

format long;

disp ('********************************************************************')
disp ('Segundo o procedimento descrito em "EDM zero correction calibration"')
disp ('********************************************************************')

% matriz B dos coeficientes dos parametros x2, x3, x4, x5 + ca (x1 esta fixo)
% ou seja as abcissas dos pilares 2, 3, 4, 5 + ca
% 
% equações de observação que relacionam os 5 parametros x2, x3, x4, x5, ca
% com as 10 observacoes x(1,2,), x(1,3), x(1,4), x(1,5), x(2,3), x(2,4),
% x(2,5), x(3,4), x(3,5), x(4,5)
%
% x2 - x1 - ca - x(1,2) - v(1,2) = 0
% x3 - x1 - ca - x(1,3) - v(1,3) = 0
% x4 - x1 - ca - x(1,4) - v(1,4) = 0
% x5 - x1 - ca - x(1,5) - v(1,5) = 0
% x3 - x2 - ca - x(2,3) - v(2,3) = 0
% x4 - x2 - ca - x(2,4) - v(2,4) = 0
% x5 - x2 - ca - x(2,5) - v(2,5) = 0
% x4 - x3 - ca - x(3,4) - v(3,4) = 0
% x5 - x3 - ca - x(3,5) - v(3,5) = 0
% x5 - x4 - ca - x(4,5) - v(4,5) = 0

% cada coluna da matriz B contém os coeficientes da cada parametro

B=zeros(10,5);
B(1,1)=1;
B(5,1)=-1;
B(6,1)=-1;
B(7,1)=-1;
B(2,2)=1;
B(5,2)=1;
B(8,2)=-1;
B(9,2)=-1;
B(3,3)=1;
B(6,3)=1;
B(8,3)=1;
B(10,3)=-1;
B(4,4)=1;
B(7,4)=1;
B(9,4)=1;
B(10,4)=1;
B(1,5)=-1;
B(2,5)=-1;
B(3,5)=-1;
B(4,5)=-1;
B(5,5)=-1;
B(6,5)=-1;
B(7,5)=-1;
B(8,5)=-1;
B(9,5)=-1;
B(10,5)=-1;

B

% distancias reduzidas ao horizonte observadas na base do LNEC consistindo em 5 pilares (Nov/2019) Leica TCA2003

x(1,2)=24.991178675542547190822346222923;
x(1,3)=100.61987467802062654542048820672;
x(1,4)=225.50696800339704269170757269645;
x(1,5)=275.21985877337414082604728546461;
x(2,3)=75.629288612685288950195180062908;
x(2,4)=200.51582701269918731985202227056;
x(2,5)=250.22899936588328367736697455954;
x(3,4)=124.88695045239437041301545260845;
x(3,5)=174.5993807899608620118751895305;
x(4,5)=49.712771769902247355223291360492;

% abcissas aproximadas dos pilares 
% valor aproximado da constante aditiva

x20=25.0;
x30=100.0;
x40=225.0;
x50=275.0;
ca=0.01;

% numero de graus de liberdade (nº observacoes - nº parametros)

df=10-5;

n=1;
while n<10

    n;
    
% vector dos termos independentes e constantes

    F0(1,1)=x20-x(1,2)-ca;
    F0(2,1)=x30-x(1,3)-ca;
    F0(3,1)=x40-x(1,4)-ca;
    F0(4,1)=x50-x(1,5)-ca;
    F0(5,1)=x30-x20-x(2,3)-ca;
    F0(6,1)=x40-x20-x(2,4)-ca;
    F0(7,1)=x50-x20-x(2,5)-ca;
    F0(8,1)=x40-x30-x(3,4)-ca;
    F0(9,1)=x50-x30-x(3,5)-ca;
    F0(10,1)=x50-x40-x(4,5)-ca;
    
% vector das correccoes aos parametros

    delta=-inv((transpose(B)*B))*(transpose(B)*F0);

% actualizacao dos parametros

    x20=x20+delta(1,1);
    x30=x30+delta(2,1);
    x40=x40+delta(3,1);
    x50=x50+delta(4,1);
    ca=ca+delta(5,1);

    n=n+1;

end

X=sprintf('Distancia ajustada P1-P2 = %f m',x20);
disp(X)
X=sprintf('Distancia ajustada P1-P3 = %f m',x30);
disp(X)
X=sprintf('Distancia ajustada P1-P4 = %f m',x40);
disp(X)
X=sprintf('Distancia ajustada P1-P5 = %f m',x50);
disp(X)
X=sprintf('Constante aditiva = %f m',ca);
disp(X)

% vector dos residuos das distancias observadas

v=B*delta+F0

% calculo da incerteza instrumental

sigma_dist=sqrt(transpose(v)*v/5)

pause

% variancia a posteriori

s02=(transpose(v)*v)/df

% calculo da incerteza instrumental segundo a norma ISO17123-4

disp ('********************************************************************')
disp ('Segundo o procedimento descrito em "ISO17123-4"')
disp ('********************************************************************')

p=3;
while (p<=4)
    soma1=0.d0;
	q=1;
	while (q<=5-p)
        soma1=soma1+x(q,p+q);
		q=q+1;
    end
	soma2=0.d0;
	q=1;
	while (q<=p)
        soma2=soma2+x(q,5-p+q);
	    q=q+1;
    end
	a(p,1)=soma1-soma2;
	p=p+1;
end

p=1;
while (p<=5)
    b(p,1)=somatorio_p(p,x); 
	p=p+1;
end

p=1;
while (p<=4)
    q=p+1;
    while (q<=5)
        aux(p,q)=-(5+2*(p-q))/15.d0*ca;
		q=q+1;
    end
	p=p+1;
end

soma_res=0.d0;
p=1;
while (p<=4)
    q=p+1;
	while (q<=5)
        r(p,q)=(b(p,1)-b(q,1)-(5+2*(p-q))/5.d0*ca-x(p,q));
		soma_res=soma_res+r(p,q);
		q=q+1;
    end
	p=p+1;
end

%residuos

r 

soma_res2=0.d0;
p=1;
while (p<=4)
    q=p+1;
	while (q<=5)
		soma_res2=soma_res2+r(p,q)^2;
		q=q+1;
    end
	p=p+1;
end

sigma_dist=sqrt(soma_res2/5.d0)

delta=(x(1,5)-(x(1,2)+x(2,3)+x(3,4)+x(4,5)))/30

pause

% fim do calculo da incerteza instrumental segundo a norma ISO17123-4

% determinacao da unidade de medida U de desenho da base do LNEC

x12=24.991178675542547190822346222923;
x13=100.61987467802062654542048820672;
x14=225.50696800339704269170757269645;
x15=275.21985877337414082604728546461;
x23=75.629288612685288950195180062908;
x24=200.51582701269918731985202227056;
x25=250.22899936588328367736697455954;
x34=124.88695045239437041301545260845;
x35=174.5993807899608620118751895305;
x45=49.712771769902247355223291360492;

unidade_medida_U=[0.3,1.5,2.0,3.0,5.0,7.5,10.0,15.0,20.0,30.769,33.333];

i=1;
while (i<=11)
    U=unidade_medida_U(i);
    switch U
        case 0.3
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            hold on
            grid on
            plot(U,resto12/U,'--r*')
            plot(U,resto13/U,'--r*')
            plot(U,resto14/U,'--r*')
            plot(U,resto15/U,'--r*')
            plot(U,resto23/U,'--r*')
            plot(U,resto24/U,'--r*')
            plot(U,resto25/U,'--r*')
            plot(U,resto34/U,'--r*')
            plot(U,resto35/U,'--r*')
            plot(U,resto45/U,'--r*')            
        case 1.5
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            hold on
            grid on
            plot(U,resto12/U,'--go')
            plot(U,resto13/U,'--go')
            plot(U,resto14/U,'--go')
            plot(U,resto15/U,'--go')
            plot(U,resto23/U,'--go')
            plot(U,resto24/U,'--go')
            plot(U,resto25/U,'--go')
            plot(U,resto34/U,'--go')
            plot(U,resto35/U,'--go')
            plot(U,resto45/U,'--go')
        case 2.0
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            hold on
            grid on
            plot(U,resto12/U,'--b*')
            plot(U,resto13/U,'--b*')
            plot(U,resto14/U,'--b*')
            plot(U,resto15/U,'--b*')
            plot(U,resto23/U,'--b*')
            plot(U,resto24/U,'--b*')
            plot(U,resto25/U,'--b*')
            plot(U,resto34/U,'--b*')
            plot(U,resto35/U,'--b*')
            plot(U,resto45/U,'--b*') 
        case 3.0
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            hold on
            grid on
            plot(U,resto12/U,'--go')
            plot(U,resto13/U,'--go')
            plot(U,resto14/U,'--go')
            plot(U,resto15/U,'--go')
            plot(U,resto23/U,'--go')
            plot(U,resto24/U,'--go')
            plot(U,resto25/U,'--go')
            plot(U,resto34/U,'--go')
            plot(U,resto35/U,'--go')
            plot(U,resto45/U,'--go')
        case 5.0
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            plot(U,resto12/U,'--yo')
            plot(U,resto13/U,'--yo')
            plot(U,resto14/U,'--yo')
            plot(U,resto15/U,'--yo')
            plot(U,resto23/U,'--yo')
            plot(U,resto24/U,'--yo')
            plot(U,resto25/U,'--yo')
            plot(U,resto34/U,'--yo')
            plot(U,resto35/U,'--yo')
            plot(U,resto45/U,'--yo')            
        case 7.5
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            hold on
            grid on
            plot(U,resto12/U,'--c*')
            plot(U,resto13/U,'--c*')
            plot(U,resto14/U,'--c*')
            plot(U,resto15/U,'--c*')
            plot(U,resto23/U,'--c*')
            plot(U,resto24/U,'--c*')
            plot(U,resto25/U,'--c*')
            plot(U,resto34/U,'--c*')
            plot(U,resto35/U,'--c*')
            plot(U,resto45/U,'--c*')            
        case 10.0
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            hold on
            grid on
            plot(U,resto12/U,'--mo')
            plot(U,resto13/U,'--mo')
            plot(U,resto14/U,'--mo')
            plot(U,resto15/U,'--mo')
            plot(U,resto23/U,'--mo')
            plot(U,resto24/U,'--mo')
            plot(U,resto25/U,'--mo')
            plot(U,resto34/U,'--mo')
            plot(U,resto35/U,'--mo')
            plot(U,resto45/U,'--mo')
        case 15.0
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            hold on
            grid on
            plot(U,resto12/U,'--r*')
            plot(U,resto13/U,'--r*')
            plot(U,resto14/U,'--r*')
            plot(U,resto15/U,'--r*')
            plot(U,resto23/U,'--r*')
            plot(U,resto24/U,'--r*')
            plot(U,resto25/U,'--r*')
            plot(U,resto34/U,'--r*')
            plot(U,resto35/U,'--r*')
            plot(U,resto45/U,'--r*')
        case 20.0
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            plot(U,resto12/U,'--go')
            plot(U,resto13/U,'--go')
            plot(U,resto14/U,'--go')
            plot(U,resto15/U,'--go')
            plot(U,resto23/U,'--go')
            plot(U,resto24/U,'--go')
            plot(U,resto25/U,'--go')
            plot(U,resto34/U,'--go')
            plot(U,resto35/U,'--go')
            plot(U,resto45/U,'--go')
        case 30.769
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            plot(U,resto12/U,'--b*')
            plot(U,resto13/U,'--b*')
            plot(U,resto14/U,'--b*')
            plot(U,resto15/U,'--b*')
            plot(U,resto23/U,'--b*')
            plot(U,resto24/U,'--b*')
            plot(U,resto25/U,'--b*')
            plot(U,resto34/U,'--b*')
            plot(U,resto35/U,'--b*')
            plot(U,resto45/U,'--b*')                       
        case 33.333
            U
            resto12=x12-fix(x12/U)*U
            resto13=x13-fix(x13/U)*U
            resto14=x14-fix(x14/U)*U
            resto15=x15-fix(x15/U)*U
            resto23=x23-fix(x23/U)*U
            resto24=x24-fix(x24/U)*U
            resto25=x25-fix(x25/U)*U
            resto34=x34-fix(x34/U)*U
            resto35=x35-fix(x35/U)*U
            resto45=x45-fix(x45/U)*U
            plot(U,resto12/U,'--yo')
            plot(U,resto13/U,'--yo')
            plot(U,resto14/U,'--yo')
            plot(U,resto15/U,'--yo')
            plot(U,resto23/U,'--yo')
            plot(U,resto24/U,'--yo')
            plot(U,resto25/U,'--yo')
            plot(U,resto34/U,'--yo')
            plot(U,resto35/U,'--yo')
            plot(U,resto45/U,'--yo')
    end
    i=i+1;
end

pause

hold off
        
% calculo do erro ciclico supondo U=1.5 m

%U=1.5;
U=3.0;
%U=30.769;
resto12=x12-fix(x12/U)*U;
resto13=x13-fix(x13/U)*U;
resto14=x14-fix(x14/U)*U;
resto15=x15-fix(x15/U)*U;
resto23=x23-fix(x23/U)*U;
resto24=x24-fix(x24/U)*U;
resto25=x25-fix(x25/U)*U;
resto34=x34-fix(x34/U)*U;
resto35=x35-fix(x35/U)*U;
resto45=x45-fix(x45/U)*U;

x=[resto12,resto13,resto14,resto15,resto23,resto24,resto25,resto34,resto35,resto45];

% ordenar componeentes de x por ordem crescente

x=sort(x) 

y=sin(2*pi*x)

yu=max(y);

yl=min(y);

yr=(yu-yl);

yz=y-yu+(yr/2);

zx=x(yz .*circshift(yz,[0 1]) <= 0);

per=2*mean(diff(zx));

ym=mean(y);

fit = @(b,x)  b(1).*(sin(2*pi*x./b(2) + 2*pi/b(3))) + b(4);    % Function to fit

fcn = @(b) sum((fit(b,x) - y).^2);                              % Least-Squares cost function

s = fminsearch(fcn, [yr;  per;  -1;  ym])                       % Minimise Least-Squares
%s = fminsearch(@(b) norm(y-fit(b,x)), [ym yr 1/per]) 

xp = linspace(min(x),max(x));

figure(1)

plot(x,y,'--*b', xp,fit(s,xp), 'r')

grid

%text(70,-0.18,sprintf('$y = %.3f + %.3f\\ sin(%.6f\\cdot x+1)$', s), 'Interpreter', 'latex')
