alex/matlab-python
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
6d1bc7db4d7b485bfea7d656d590563f734878b2
matlab-python/Tilt/triax_HD.m

525 lines
18 KiB
Matlab
Executable File
Raw Blame History

%% Funzione che calcola gli spostamenti in modalità triassiale per i Tilt Link
function [X_HD,Y_HD,Z_HD,Xlocal_HD,Ylocal_HD,Zlocal_HD,HShift_HD,HShift_local_HD,...
AlfaX_HD,AlfaY_HD,Azimuth_HD,Speed_local_HD,Speed_HD,Acceleration_local_HD,...
Acceleration_HD,tempHD,ARRAYdateHD,ErrTiltLinkHD] = triax_HD(rHD,...
ACCdef_HD,ACCdefRisHD,MAGdef_HD,tempHD,SpeHD,PsHD,NodoTiltLinkHD,tolleranzaAcc,...
tolleranzaMag,DatiElabTiltLinkHD,Ndevst,Wdevst,ARRAYdateHD,Tmax,Tmin,NuovoZeroHD,...
NdatiMedia,Ndatidespike,ErrTiltLinkHD,margine,datainiHD,MEMS,IDcentralina,DTcatena,FileName)
%% Inizializzazione
fileID = fopen(FileName,'a');
fmt = '%s \r';
text = 'triax_HD function started';
fprintf(fileID,fmt,text);
if NuovoZeroHD == 1
if NdatiMedia > Ndatidespike
Ndati = NdatiMedia;
else
Ndati = Ndatidespike;
end
ini = round(Ndati/2)+1;
if rem(Ndati,2) == 0
ini = ini+1;
end
clear NDati
ini = ini + margine;
if ini < 6
ini = 6;
end
[letture,~]=size(DatiElabTiltLinkHD);
if ini > letture
ini = letture-1;
end
if Ndevst ~= 0 % Allora prendo tutti i dati e solo in seguito considero i nuovi, a valle della funzione filtro
ini = 1;
end
ACCdefRisHD = ACCdefRisHD(ini:end,:);
tempHD = tempHD(ini:end,:);
DatiElabTiltLinkHD = DatiElabTiltLinkHD(ini:end,:);
ARRAYdateHD = ARRAYdateHD(ini:end,1);
ErrTiltLinkHD = ErrTiltLinkHD(ini:end,:);
end
%% Controllo e correzione dei campi magnetici
[rA,cA] = size(ACCdef_HD); % righe -> date, colonna -> nodi
conf_acc = zeros(rA,cA);
contM = 0; % contatore correzioni campi magnetici
contT = rA*cA;
% Se le accelerazioni variano meno della confidenza, NON cambio i valori di
% cambio magnetico e prendo quelli della lettura precedente
for d = 2:rA
% seconda lettura e successive, confronto i risultati dei magnetometri
% e degli accelerometri
conf_acc(d,:) = abs(ACCdef_HD(d,:) - ACCdef_HD(d-1,:));
for dd = 1:cA
if abs(conf_acc(d,dd)) < tolleranzaMag % SOGLIA ACCELERAZIONE
MAGdef_HD(d,dd) = MAGdef_HD(d-1,dd);
contM = contM+1;
end
end
end
text = ['' num2str(contM) ' corrections of magnetometers executed on a total amount of ' num2str(contT)...
' data, which represent the ' num2str(contM*100/contT) '% for Tilt Link HD'];
fprintf(fileID,fmt,text);
if NuovoZeroHD == 1
ACCdef_HD = ACCdef_HD(ini:end,:);
MAGdef_TL2 = MAGdef_HD(ini:end,:);
else
MAGdef_TL2 = MAGdef_HD;
end
%% Definisco i dati
Nnodi = rHD;
[r,~] = size(ACCdef_HD); % Numero di dati
[Ndati,~] = size(ARRAYdateHD);
ax = zeros(r,Nnodi);
ay = zeros(r,Nnodi);
az = zeros(r,Nnodi);
mx = zeros(r,Nnodi);
my = zeros(r,Nnodi);
mz = zeros(r,Nnodi);
for i=1:Nnodi
ax(:,i) = ACCdef_HD(:,(i-1)*3+1); % ax
ay(:,i) = ACCdef_HD(:,(i-1)*3+2:(i-1)*3+2); % ay
az(:,i) = ACCdef_HD(:,(i-1)*3+3:(i-1)*3+3); % az
mx(:,i) = MAGdef_TL2(:,(i-1)*3+1); % mx
my(:,i) = MAGdef_TL2(:,(i-1)*3+2:(i-1)*3+2); % my
mz(:,i) = MAGdef_TL2(:,(i-1)*3+3:(i-1)*3+3); % mz
end
%% Costruzione delle matrici spostamento e rotazione
NordSud = zeros(Nnodi,Ndati); % in riga i nodi, in colonna le date
EstOvest = zeros(Nnodi,Ndati); % in riga i nodi, in colonna le date
Zlocal_HD = zeros(Nnodi,Ndati);
% parametri per il calcolo
SpeHD = SpeHD(2:end,1); % salto il segmento di pertinenza dell'ancora
c = 1;
% Inizio del ciclo di elaborazione
text = 'Triaxial Elaboration of Tilt Link HD started';
fprintf(fileID,fmt,text);
% Inizializzo le matrici
deltaNS = zeros(rHD,1);
deltaEO = zeros(rHD,1);
deltaZ = zeros(rHD,1);
BECCHEGGIO = zeros(rHD,1);
ROLLIO = zeros(rHD,1);
IMBARDATA = zeros(rHD,1);
qf = zeros(rHD,4);
qfy = zeros(rHD,4);
for jj = 1:Ndati
for jjj=1:rHD
axb = ax(jj,jjj)';
ayb = ay(jj,jjj)';
azb = az(jj,jjj)';
mxb = mx(jj,jjj)';
myb = my(jj,jjj)';
mzb = mz(jj,jjj)';
SP = SpeHD(jjj);
[DNS,DEO,Dz,becc,roll,imba,q,qy] = QuaternioniASE(axb,ayb,azb,mxb,myb,mzb,SP,MEMS,FileName);
deltaNS(jjj) = DNS;
deltaEO(jjj) = DEO;
deltaZ(jjj) = Dz;
BECCHEGGIO(jjj) = becc;
ROLLIO(jjj) = roll;
IMBARDATA(jjj) = imba;
qf(jjj,:) = q;
qfy(jjj,:) = qy;
dq = q-qy;
if any(dq>0.000001)
text = (['Cicle Number ' num2str(jj) ' during Quaternion Calculation']);
text2 = (['Node Number ' num2str(jjj)]);
fprintf(fileID,fmt,text);
fprintf(fileID,fmt,text2);
end
end
% Le seguenti matrici sono organizzate come segue:
% - ciascuna riga è relativa a un nodo (1a riga = fondo catena);
% - ciascuna colonna è relativa a una data.
NordSud(:,c) = deltaNS;
EstOvest(:,c) = deltaEO;
Zlocal_HD(:,c) = deltaZ;
c = c+1;
end
text = 'Quaternion calculation executed';
fprintf(fileID,fmt,text);
% dNS e dEO raccolgono i dati del singolo nodo nella singola data
dNS = diff(NordSud,1,2);
dEO = diff(EstOvest,1,2);
dZ = diff(Zlocal_HD,1,2);
%% Controllo delle risultanti di accelerazione e campo magnetico
clear r
clear rr
clear c
clear cc
ACCdefRisHD = ACCdefRisHD'; % Nodi in riga, date in colonna
[r,c] = size(ACCdefRisHD);
[rr,cc] = size(NordSud);
% controllo che le matrici con le risultanti delle accelerazioni e
% le matrici con i dati di spostamento abbiano le stesse dimensioni
if r~=rr
text = '---Warning! Number of row of displacement data do not correspond to the number of acceleration cosine vector!---';
fprintf(fileID,fmt,text);
end
if c~=cc
text = '---Warning! Number of column of displacement data do not correspond to the number of acceleration cosine vector!---';
fprintf(fileID,fmt,text);
end
clear i
clear j
cont = 1; % contatore
cont2 = 1; % contatore
cont3 = 1; % contatore
tempHD = tempHD';
textA = 'There are not correction of Tilt Link HD based on acceleration vectors filter';
textA2 = 'There are not correction of Tilt Link HD based on uncalibrated acceleration vectors';
textT = 'There are not correction of Tilt Link HD based on temperature filter';
for j = 2:c % Data
for i = 1:r % Nodo
% se il valore assoluto della differenza è maggiore della
% tolleranza, pongo gli spostamenti giornalieri pari a 0
if abs(ACCdefRisHD(i,j)-ACCdefRisHD(i,j-1)) > tolleranzaAcc
dNS(i,j-1) = 0;
dEO(i,j-1) = 0;
dZ(i,j-1) = 0;
textA = ['' num2str(cont) ' correction executed for Tilt Link HD - Acceleration vector filter!'];
cont = cont+1;
end
if ACCdefRisHD(i,j) < 0.9 || ACCdefRisHD(i,j) > 1.1 % Il nodo è fuori taratura!
dNS(i,j-1) = 0;
dEO(i,j-1) = 0;
dZ(i,j-1) = 0;
tempHD(i,j) = tempHD(i,j-1);
textA2 = ['' num2str(cont) ' correction executed for Tilt Link HD - uncalibrated Acceleration vector!'];
cont3 = cont3+1;
end
end
end
FileTemperature = ['' IDcentralina '-' DTcatena '-HD-Therm.csv'];
if isfile(FileTemperature) == 1
DatiRaw = csvread(FileTemperature);
[rDR,cDR] = size(DatiRaw);
DatiRaw(:,1) = DatiRaw(:,1) + 730000;
else
rDR = 1;
cDR = 1;
end
for b = 1:c % Data
for a = 1:r % Nodo
% NON considero i dati al di sopra dei 80 °C o al di sotto dei -30 °C!
if tempHD(a,b) > Tmax || tempHD(a,b) < Tmin
cont2 = cont2+1;
if b == 1
if isfile(FileTemperature) == 1
RawDate = find(DatiRaw(:,1)<=datenum(datainiHD));
if isempty(RawDate) == 1
cc = 2;
while cc <= c
if tempHD(a,cc) > Tmax || tempHD(a,cc) < Tmin
cc = cc+1;
else
break
end
end
tempHD(a,b) = tempHD(a,cc);
else
if isnan(DatiRaw(RawDate(end),a+1)) == 0
tempHD(a,b) = DatiRaw(RawDate(end),a+1);
ErrTiltLinkHD(b,a) = 0.5;
wardat = 'Temperature data of Tilt Link HD nodes corrected using Raw Data of reference Csv file.';
fprintf(fileID,fmt,wardat);
else
cc = 2;
while cc <= c
if tempHD(a,cc) > Tmax || tempHD(a,cc) < Tmin
cc = cc+1;
else
break
end
end
tempHD(a,b) = tempHD(a,cc);
end
end
else
cc = 2;
while cc <= c
if tempHD(a,cc) > Tmax || tempHD(a,cc) < Tmin
cc = cc+1;
else
break
end
end
tempHD(a,b) = tempHD(a,cc);
end
else
tempHD(a,b) = tempHD(a,b-1);
dNS(a,b-1) = 0;
dEO(a,b-1) = 0;
dZ(a,b-1) = 0;
ErrTiltLinkHD(b,a) = 0.5;
end
textT = ['' num2str(cont2) ' correction executed for Tilt Link HD - Temperature filter!'];
end
end
end
if rDR~=1 && cDR~=1 && isempty(DatiRaw) == 0
RawDate1 = find(DatiRaw(:,1)<=ARRAYdateHD(1));
if isempty(RawDate1) == 1
RawDate2 = 1;
elseif RawDate1(end) == rDR
RawDate2 = find(ARRAYdateHD(:,1)>DatiRaw(end,1));
else
RawDate2 = find(ARRAYdateHD(:,1)>DatiRaw(RawDate1(end)+1,1));
end
else
RawDate1 = [];
RawDate2 = 1;
end
if isempty(RawDate1) == 0 && isempty(RawDate2) == 0
Dati = [DatiRaw(1:RawDate1(end),:); ARRAYdateHD(RawDate2(1):end) tempHD(:,RawDate2(1):end)'];
elseif isempty(RawDate1) == 1 && isempty(RawDate2) == 0
Dati = [ARRAYdateHD tempHD'];
else
Dati = DatiRaw;
end
% Elimino appoggio più vecchio di un mese
RawDate3 = find(Dati(:,1)<now-30);
if isempty(RawDate3) == 0
[rDati,~] = size(Dati);
if RawDate3(end) == rDati
else
Dati = Dati(RawDate3(end)+1:end,:);
end
end
if isfile(FileTemperature) == 1
delete(FileTemperature);
end
Dati(:,1) = Dati(:,1) - 730000;
csvwrite(FileTemperature,Dati);
tempHD = tempHD';
fprintf(fileID,fmt,textA);
fprintf(fileID,fmt,textA2);
fprintf(fileID,fmt,textT);
fclose(fileID);
% azzeramenti di alcuni nodi in particolare
[dNS,dEO,dZ] = azzeramenti(IDcentralina,DTcatena,dNS,dEO,dZ,NodoTiltLinkHD,FileName);
% Filtro
[dNS,dEO,dZ] = filtro(dNS,dEO,dZ,Ndevst,Wdevst,FileName);
if NuovoZeroHD == 1 && Ndevst ~= 0
if NdatiMedia > Ndatidespike
NdatiF = NdatiMedia;
else
NdatiF = Ndatidespike;
end
ini = round(NdatiF/2)+1;
if rem(NdatiF,2) == 0
ini = ini+1;
end
clear NDatiF
iniST = round(Wdevst/2);
if rem(Wdevst,2) == 0
iniST = iniST+1;
end
iniST = iniST + margine;
if iniST > ini
ini = iniST;
end
if ini < 6
ini = 6;
end
dNS = dNS(:,ini:end);
dEO = dEO(:,ini:end);
dZ = dZ(:,ini:end);
tempHD = tempHD(ini:end,:);
DatiElabTiltLinkHD = DatiElabTiltLinkHD(ini:end,:);
ARRAYdateHD = ARRAYdateHD(ini:end,1);
ErrTiltLinkHD = ErrTiltLinkHD(ini:end,:);
end
%% Finalizzo i calcoli
[rNS,cNS] = size(dNS);
sommaX = zeros(rHD,1);
Xlocal_HD = zeros(rNS,cNS+1); % locale nello spazio, cumulato nel tempo
AlfaX_HD = zeros(rNS,cNS+1); % Angoli
sommaY = zeros(rHD,1);
Ylocal_HD = zeros(rNS,cNS+1);
AlfaY_HD = zeros(rNS,cNS+1); % Angoli
sommaZ = zeros(rNS,cNS);
Zlocal_HD = zeros(rNS,cNS+1);
X_HD = zeros(rNS,cNS+1); % cumulato nel tempo e nello spazio
Y_HD = zeros(rNS,cNS+1);
Z_HD = zeros(rNS,cNS+1);
HShift_HD = zeros(rNS,cNS+1); % massima pendenza cumulato nel tempo e nello spazio
HShift_local_HD = zeros(rNS,cNS+1); % massima pendenza locale
Azimuth_HD = zeros(rNS,cNS+1); % azimut
azim = zeros(rNS,cNS+1); % matrice di appoggio per il calcolo dell'azimuth
Speed_HD = zeros(rHD,cNS+1); % Velocità 2D Cumulata
Speed_local_HD = zeros(rHD,cNS+1); % Velocità 2D locale
Acceleration_HD = zeros(rHD,cNS+1); % Accelerazione 2D Cumulata
Acceleration_local_HD = zeros(rHD,cNS+1); % Accelerazione 2D Locale
% Recupero i dati già elaborati
if NuovoZeroHD == 1
[rE,cE] = size(DatiElabTiltLinkHD);
cont = 3;
n = 1;
while cont<=cE
sommaX(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont))';
Xlocal_HD(n,1) = sommaX(n,1);
X_HD(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont+3))';
sommaY(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont+1))';
Ylocal_HD(n,1) = sommaY(n,1);
Y_HD(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont+4))';
for j = 1:rHD
AlfaX_HD(j,1) = asind(Xlocal_HD(j,1)/SpeHD(j));
AlfaY_HD(j,1) = asind(Ylocal_HD(j,1)/SpeHD(j));
end
Zlocal_HD(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont+2))';
Z_HD(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont+5))';
HShift_HD(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont+6))';
HShift_local_HD(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont+7))';
Azimuth_HD(n,1) = cell2mat(DatiElabTiltLinkHD(1,cont+8))';
Speed_HD(n,1:rE) = cell2mat(DatiElabTiltLinkHD(:,cont+10))';
Speed_local_HD(n,1:rE) = cell2mat(DatiElabTiltLinkHD(:,cont+11))';
Acceleration_HD(n,1:rE) = cell2mat(DatiElabTiltLinkHD(:,cont+12))';
Acceleration_local_HD(n,1:rE) = cell2mat(DatiElabTiltLinkHD(:,cont+13))';
cont=cont+16;
n = n+1;
end
else
Zlocal_HD(:,1) = SpeHD;
Z_HD(:,1) = PsHD(2:end);
end
% elaboro i dati nuovi
for iii = 1:cNS
Xlocal_HD(:,iii+1) = sum(dNS(:,1:iii),2)+sommaX(:,1);
Ylocal_HD(:,iii+1) = sum(dEO(:,1:iii),2)+sommaY(:,1);
for j = 1:rHD
AlfaX_HD(j,iii+1) = asind(Xlocal_HD(j,iii+1)/SpeHD(j));
AlfaY_HD(j,iii+1) = asind(Ylocal_HD(j,iii+1)/SpeHD(j));
end
sommaZ(:,iii+1) = sum(dZ(:,1:iii),2);
X_HD(:,iii+1) = cumsum(Xlocal_HD(:,iii+1));
Y_HD(:,iii+1) = cumsum(Ylocal_HD(:,iii+1));
Z_HD(:,iii+1) = cumsum(sommaZ(:,iii+1))+ Z_HD(:,1);
HShift_HD(:,iii+1) = (X_HD(:,iii+1).^2+Y_HD(:,iii+1).^2).^0.5;
HShift_local_HD(:,iii+1) = (Xlocal_HD(:,iii+1).^2+Ylocal_HD(:,iii+1).^2).^0.5;
Zlocal_HD(:,iii+1) = sommaZ(:,iii+1) + SpeHD; % Zeta è il singolo abbassamento di quel nodo
for rr = 1:rHD
azim(rr,iii) = (acos(abs(X_HD(rr,iii))/HShift_HD(rr,iii)))*180/3.141592654; % Angolo Teta in gradi 0° - 90°
segnoNS = sign(X_HD(rr,iii));
segnoEO = sign(Y_HD(rr,iii));
% L'azimuth si calcola con NS = 0, positivo in senso orario
% (90° = Est)
if segnoNS == 1 && segnoEO == 1 % quadrante 1
Azimuth_HD(rr,iii+1) = azim(rr,iii); % Teta lo tengo come è (1 quadrante)
elseif segnoNS == -1 && segnoEO == 1 % quadrante 2
Azimuth_HD(rr,iii+1) = 180 - azim(rr,iii); % 180-teta
elseif segnoNS == -1 && segnoEO == -1 % quadrante 3
Azimuth_HD(rr,iii+1) = 180 + azim(rr,iii); % 180+teta
elseif segnoNS == 1 && segnoEO == -1 % quadrante 4
Azimuth_HD(rr,iii+1) = 360 - azim(rr,iii); % 360-teta
elseif segnoNS == 0 && segnoEO == -1 % 270°
Azimuth_HD(rr,iii+1) = 270;
elseif segnoNS == -1 && segnoEO == 0 % 180°
Azimuth_HD(rr,iii+1) = 180;
end
end
end
Azimuth_HD = real(Azimuth_HD);
%% Calcolo velocità di spostamento giornaliera
[numDate,~] = size(ARRAYdateHD); % numero di date
d = 1;
p = 1;
diffDate = 0;
n = 1;
period = 1; % calcolo giornaliero
ContSUP = [];
for dd = 1:numDate
while diffDate < period
d = d+1;
if d > numDate % Se d supera le date disponibili, esco dal ciclo while
break
end
diffDate = ARRAYdateHD(d) - ARRAYdateHD(p);
end
if d >numDate
break
end
ContSUP(n,1) = d; %#ok<*AGROW> % Creo matrice indici dell'estremo superiore della differenza
ContINF(n,1) = p; % Creo matrice indici dell'estremo inferiore della differenza
p = p+1; % passo alla data di partenza successiva
d = p; % resetto il conto di d
n = n+1;
diffDate = 0;
end
check = isempty(ContSUP);
if check == 0
[nDate,~] = size(ContSUP);
for s = 1:rHD
N = 1;
for dd = 1:nDate
Speed_HD(s,ContSUP(N,1)) = (HShift_HD(s,ContSUP(N,1))-HShift_HD(s,ContINF(N,1)))/(ARRAYdateHD(ContSUP(N,1))-ARRAYdateHD(ContINF(N,1)));
Speed_local_HD(s,ContSUP(N,1)) = (HShift_local_HD(s,ContSUP(N,1))-HShift_local_HD(s,ContINF(N,1)))/(ARRAYdateHD(ContSUP(N,1))-ARRAYdateHD(ContINF(N,1)));
Acceleration_HD(s,ContSUP(N,1)) = (Speed_HD(s,ContSUP(N,1))-Speed_HD(s,ContINF(N,1)))/(ARRAYdateHD(ContSUP(N,1))-ARRAYdateHD(ContINF(N,1)));
Acceleration_local_HD(s,ContSUP(N,1)) = (Speed_local_HD(s,ContSUP(N,1))-Speed_local_HD(s,ContINF(N,1)))/(ARRAYdateHD(ContSUP(N,1))-ARRAYdateHD(ContINF(N,1)));
N = N+1;
end
end
end
% Approssimo i dati con il corretto numero di cifre decimali
[X_HD,Y_HD,Z_HD,Xlocal_HD,Ylocal_HD,Zlocal_HD,HShift_HD,HShift_local_HD,Azimuth_HD,Speed_HD,...
Speed_local_HD,Acceleration_HD,Acceleration_local_HD,tempHD] = approx_HD(X_HD,Y_HD,Z_HD,...
Xlocal_HD,Ylocal_HD,Zlocal_HD,HShift_HD,HShift_local_HD,Azimuth_HD,Speed_HD,Speed_local_HD,...
Acceleration_HD,Acceleration_local_HD,tempHD,FileName);
% Riordino matrice errori
[r,~] = size(ErrTiltLinkHD);
Matrice_err = zeros(r,rHD);
for i = 1:r % date
d = 1;
for n = 1:rHD % nodi
j = 1;
err = ErrTiltLinkHD(i,d:d+6);
while j <= 7
if err(1,j) == 1
Matrice_err(i,n) = 1;
break
end
if err(1,j) == 0.5
Matrice_err(i,n) = 0.5;
end
j = j+1;
end
d = d+7;
end
end
ErrTiltLinkHD = Matrice_err';
text = 'Tilt Link HD triaxial calculation executed correctly. triax_HD function ended';
fileID = fopen(FileName,'a');
fmt = '%s \r';
fprintf(fileID,fmt,text);
fclose(fileID);
end
Reference in New Issue View Git Blame Copy Permalink