Sync from remote server: 2025-10-12 18:56:41
This commit is contained in:
525
Tilt/triax_HDVR.m
Executable file
525
Tilt/triax_HDVR.m
Executable file
@@ -0,0 +1,525 @@
|
||||
%% Funzione che calcola gli spostamenti in modalità triassiale per i Tilt Link
|
||||
function [X_HDVR,Y_HDVR,Z_HDVR,Xlocal_HDVR,Ylocal_HDVR,Zlocal_HDVR,HShift_HDVR,HShift_local_HDVR,...
|
||||
AlfaX_HDVR,AlfaY_HDVR,Azimuth_HDVR,Speed_local_HDVR,Speed_HDVR,Acceleration_local_HDVR,...
|
||||
Acceleration_HDVR,tempHDVR,ARRAYdateHDVR,ErrTiltLinkHDVR] = triax_HDVR(rHDVR,...
|
||||
ACCdef_HDVR,ACCdefRisHDVR,MAGdef_HDVR,tempHDVR,SpeHDVR,PsHDVR,NodoTiltLinkHDVR,tolleranzaAcc,...
|
||||
tolleranzaMag,DatiElabTiltLinkHDVR,Ndevst,Wdevst,ARRAYdateHDVR,Tmax,Tmin,NuovoZeroHDVR,...
|
||||
NdatiMedia,Ndatidespike,ErrTiltLinkHDVR,margine,datainiHDVR,MEMS,IDcentralina,DTcatena,FileName)
|
||||
|
||||
%% Inizializzazione
|
||||
fileID = fopen(FileName,'a');
|
||||
fmt = '%s \r';
|
||||
text = 'triax_HDVR function started';
|
||||
fprintf(fileID,fmt,text);
|
||||
|
||||
if NuovoZeroHDVR == 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(DatiElabTiltLinkHDVR);
|
||||
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
|
||||
ACCdefRisHDVR = ACCdefRisHDVR(ini:end,:);
|
||||
tempHDVR = tempHDVR(ini:end,:);
|
||||
DatiElabTiltLinkHDVR = DatiElabTiltLinkHDVR(ini:end,:);
|
||||
ARRAYdateHDVR = ARRAYdateHDVR(ini:end,1);
|
||||
ErrTiltLinkHDVR = ErrTiltLinkHDVR(ini:end,:);
|
||||
end
|
||||
|
||||
%% Controllo e correzione dei campi magnetici
|
||||
[rA,cA] = size(ACCdef_HDVR); % 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_HDVR(d,:) - ACCdef_HDVR(d-1,:));
|
||||
for dd = 1:cA
|
||||
if abs(conf_acc(d,dd)) < tolleranzaMag % SOGLIA ACCELERAZIONE
|
||||
MAGdef_HDVR(d,dd) = MAGdef_HDVR(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 VR'];
|
||||
fprintf(fileID,fmt,text);
|
||||
|
||||
if NuovoZeroHDVR == 1
|
||||
ACCdef_HDVR = ACCdef_HDVR(ini:end,:);
|
||||
MAGdef_TL2 = MAGdef_HDVR(ini:end,:);
|
||||
else
|
||||
MAGdef_TL2 = MAGdef_HDVR;
|
||||
end
|
||||
|
||||
%% Definisco i dati
|
||||
Nnodi = rHDVR;
|
||||
[r,~] = size(ACCdef_HDVR); % Numero di dati
|
||||
[Ndati,~] = size(ARRAYdateHDVR);
|
||||
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_HDVR(:,(i-1)*3+1); % ax
|
||||
ay(:,i) = ACCdef_HDVR(:,(i-1)*3+2:(i-1)*3+2); % ay
|
||||
az(:,i) = ACCdef_HDVR(:,(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_HDVR = zeros(Nnodi,Ndati);
|
||||
|
||||
% parametri per il calcolo
|
||||
SpeHDVR = SpeHDVR(2:end,1); % salto il segmento di pertinenza dell'ancora
|
||||
c = 1;
|
||||
|
||||
% Inizio del ciclo di elaborazione
|
||||
text = 'Triaxial Elaboration of Tilt Link HD VR started';
|
||||
fprintf(fileID,fmt,text);
|
||||
|
||||
% Inizializzo le matrici
|
||||
deltaNS = zeros(rHDVR,1);
|
||||
deltaEO = zeros(rHDVR,1);
|
||||
deltaZ = zeros(rHDVR,1);
|
||||
BECCHEGGIO = zeros(rHDVR,1);
|
||||
ROLLIO = zeros(rHDVR,1);
|
||||
IMBARDATA = zeros(rHDVR,1);
|
||||
qf = zeros(rHDVR,4);
|
||||
qfy = zeros(rHDVR,4);
|
||||
for jj = 1:Ndati
|
||||
for jjj=1:rHDVR
|
||||
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 = SpeHDVR(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_HDVR(:,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_HDVR,1,2);
|
||||
|
||||
%% Controllo delle risultanti di accelerazione e campo magnetico
|
||||
clear r
|
||||
clear rr
|
||||
clear c
|
||||
clear cc
|
||||
ACCdefRisHDVR = ACCdefRisHDVR'; % Nodi in riga, date in colonna
|
||||
[r,c] = size(ACCdefRisHDVR);
|
||||
[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
|
||||
tempHDVR = tempHDVR';
|
||||
textA = 'There are not correction of Tilt Link HD VR based on acceleration vectors filter';
|
||||
textA2 = 'There are not correction of Tilt Link HD VR based on uncalibrated acceleration vectors';
|
||||
textT = 'There are not correction of Tilt Link HD VR 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(ACCdefRisHDVR(i,j)-ACCdefRisHDVR(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 VR - Acceleration vector filter!'];
|
||||
cont = cont+1;
|
||||
end
|
||||
if ACCdefRisHDVR(i,j) < 0.9 || ACCdefRisHDVR(i,j) > 1.1 % Il nodo è fuori taratura!
|
||||
dNS(i,j-1) = 0;
|
||||
dEO(i,j-1) = 0;
|
||||
dZ(i,j-1) = 0;
|
||||
tempHDVR(i,j) = tempHDVR(i,j-1);
|
||||
textA2 = ['' num2str(cont) ' correction executed for Tilt Link HD VR - uncalibrated Acceleration vector!'];
|
||||
cont3 = cont3+1;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
FileTemperature = ['' IDcentralina '-' DTcatena '-HDVR-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 tempHDVR(a,b) > Tmax || tempHDVR(a,b) < Tmin
|
||||
cont2 = cont2+1;
|
||||
if b == 1
|
||||
if isfile(FileTemperature) == 1
|
||||
RawDate = find(DatiRaw(:,1)<=datenum(datainiHDVR));
|
||||
if isempty(RawDate) == 1
|
||||
cc = 2;
|
||||
while cc <= c
|
||||
if tempHDVR(a,cc) > Tmax || tempHDVR(a,cc) < Tmin
|
||||
cc = cc+1;
|
||||
else
|
||||
break
|
||||
end
|
||||
end
|
||||
tempHDVR(a,b) = tempHDVR(a,cc);
|
||||
else
|
||||
if isnan(DatiRaw(RawDate(end),a+1)) == 0
|
||||
tempHDVR(a,b) = DatiRaw(RawDate(end),a+1);
|
||||
ErrTiltLinkHDVR(b,a) = 0.5;
|
||||
wardat = 'Temperature data of Tilt Link HD VR nodes corrected using Raw Data of reference Csv file.';
|
||||
fprintf(fileID,fmt,wardat);
|
||||
else
|
||||
cc = 2;
|
||||
while cc <= c
|
||||
if tempHDVR(a,cc) > Tmax || tempHDVR(a,cc) < Tmin
|
||||
cc = cc+1;
|
||||
else
|
||||
break
|
||||
end
|
||||
end
|
||||
tempHDVR(a,b) = tempHDVR(a,cc);
|
||||
end
|
||||
end
|
||||
else
|
||||
cc = 2;
|
||||
while cc <= c
|
||||
if tempHDVR(a,cc) > Tmax || tempHDVR(a,cc) < Tmin
|
||||
cc = cc+1;
|
||||
else
|
||||
break
|
||||
end
|
||||
end
|
||||
tempHDVR(a,b) = tempHDVR(a,cc);
|
||||
end
|
||||
else
|
||||
tempHDVR(a,b) = tempHDVR(a,b-1);
|
||||
dNS(a,b-1) = 0;
|
||||
dEO(a,b-1) = 0;
|
||||
dZ(a,b-1) = 0;
|
||||
ErrTiltLinkHDVR(b,a) = 0.5;
|
||||
end
|
||||
textT = ['' num2str(cont2) ' correction executed for Tilt Link HD VR - Temperature filter!'];
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
if rDR~=1 && cDR~=1 && isempty(DatiRaw) == 0
|
||||
RawDate1 = find(DatiRaw(:,1)<=ARRAYdateHDVR(1));
|
||||
if isempty(RawDate1) == 1
|
||||
RawDate2 = 1;
|
||||
elseif RawDate1(end) == rDR
|
||||
RawDate2 = find(ARRAYdateHDVR(:,1)>DatiRaw(end,1));
|
||||
else
|
||||
RawDate2 = find(ARRAYdateHDVR(:,1)>DatiRaw(RawDate1(end)+1,1));
|
||||
end
|
||||
else
|
||||
RawDate1 = [];
|
||||
RawDate2 = 1;
|
||||
end
|
||||
if isempty(RawDate1) == 0 && isempty(RawDate2) == 0
|
||||
Dati = [DatiRaw(1:RawDate1(end),:); ARRAYdateHDVR(RawDate2(1):end) tempHDVR(:,RawDate2(1):end)'];
|
||||
elseif isempty(RawDate1) == 1 && isempty(RawDate2) == 0
|
||||
Dati = [ARRAYdateHDVR tempHDVR'];
|
||||
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);
|
||||
|
||||
tempHDVR = tempHDVR';
|
||||
|
||||
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,NodoTiltLinkHDVR,FileName);
|
||||
|
||||
% Filtro
|
||||
[dNS,dEO,dZ] = filtro(dNS,dEO,dZ,Ndevst,Wdevst,FileName);
|
||||
if NuovoZeroHDVR == 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);
|
||||
tempHDVR = tempHDVR(ini:end,:);
|
||||
DatiElabTiltLinkHDVR = DatiElabTiltLinkHDVR(ini:end,:);
|
||||
ARRAYdateHDVR = ARRAYdateHDVR(ini:end,1);
|
||||
ErrTiltLinkHDVR = ErrTiltLinkHDVR(ini:end,:);
|
||||
end
|
||||
|
||||
%% Finalizzo i calcoli
|
||||
[rNS,cNS] = size(dNS);
|
||||
sommaX = zeros(rHDVR,1);
|
||||
Xlocal_HDVR = zeros(rNS,cNS+1); % locale nello spazio, cumulato nel tempo
|
||||
AlfaX_HDVR = zeros(rNS,cNS+1); % Angoli
|
||||
sommaY = zeros(rHDVR,1);
|
||||
Ylocal_HDVR = zeros(rNS,cNS+1);
|
||||
AlfaY_HDVR = zeros(rNS,cNS+1); % Angoli
|
||||
sommaZ = zeros(rNS,cNS);
|
||||
Zlocal_HDVR = zeros(rNS,cNS+1);
|
||||
X_HDVR = zeros(rNS,cNS+1); % cumulato nel tempo e nello spazio
|
||||
Y_HDVR = zeros(rNS,cNS+1);
|
||||
Z_HDVR = zeros(rNS,cNS+1);
|
||||
HShift_HDVR = zeros(rNS,cNS+1); % massima pendenza cumulato nel tempo e nello spazio
|
||||
HShift_local_HDVR = zeros(rNS,cNS+1); % massima pendenza locale
|
||||
Azimuth_HDVR = zeros(rNS,cNS+1); % azimut
|
||||
azim = zeros(rNS,cNS+1); % matrice di appoggio per il calcolo dell'azimuth
|
||||
Speed_HDVR = zeros(rHDVR,cNS+1); % Velocità 2D Cumulata
|
||||
Speed_local_HDVR = zeros(rHDVR,cNS+1); % Velocità 2D locale
|
||||
Acceleration_HDVR = zeros(rHDVR,cNS+1); % Accelerazione 2D Cumulata
|
||||
Acceleration_local_HDVR = zeros(rHDVR,cNS+1); % Accelerazione 2D Locale
|
||||
|
||||
% Recupero i dati già elaborati
|
||||
if NuovoZeroHDVR == 1
|
||||
[rE,cE] = size(DatiElabTiltLinkHDVR);
|
||||
cont = 3;
|
||||
n = 1;
|
||||
while cont<=cE
|
||||
sommaX(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont))';
|
||||
Xlocal_HDVR(n,1) = sommaX(n,1);
|
||||
X_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+3))';
|
||||
sommaY(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+1))';
|
||||
Ylocal_HDVR(n,1) = sommaY(n,1);
|
||||
Y_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+4))';
|
||||
for j = 1:rHDVR
|
||||
AlfaX_HDVR(j,1) = asind(Xlocal_HDVR(j,1)/SpeHDVR(j));
|
||||
AlfaY_HDVR(j,1) = asind(Ylocal_HDVR(j,1)/SpeHDVR(j));
|
||||
end
|
||||
Zlocal_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+2))';
|
||||
Z_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+5))';
|
||||
HShift_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+6))';
|
||||
HShift_local_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+7))';
|
||||
Azimuth_HDVR(n,1) = cell2mat(DatiElabTiltLinkHDVR(1,cont+8))';
|
||||
Speed_HDVR(n,1:rE) = cell2mat(DatiElabTiltLinkHDVR(:,cont+10))';
|
||||
Speed_local_HDVR(n,1:rE) = cell2mat(DatiElabTiltLinkHDVR(:,cont+11))';
|
||||
Acceleration_HDVR(n,1:rE) = cell2mat(DatiElabTiltLinkHDVR(:,cont+12))';
|
||||
Acceleration_local_HDVR(n,1:rE) = cell2mat(DatiElabTiltLinkHDVR(:,cont+13))';
|
||||
cont=cont+16;
|
||||
n = n+1;
|
||||
end
|
||||
else
|
||||
Zlocal_HDVR(:,1) = SpeHDVR;
|
||||
Z_HDVR(:,1) = PsHDVR(2:end);
|
||||
end
|
||||
|
||||
% elaboro i dati nuovi
|
||||
for iii = 1:cNS
|
||||
Xlocal_HDVR(:,iii+1) = sum(dNS(:,1:iii),2)+sommaX(:,1);
|
||||
Ylocal_HDVR(:,iii+1) = sum(dEO(:,1:iii),2)+sommaY(:,1);
|
||||
for j = 1:rHDVR
|
||||
AlfaX_HDVR(j,iii+1) = asind(Xlocal_HDVR(j,iii+1)/SpeHDVR(j));
|
||||
AlfaY_HDVR(j,iii+1) = asind(Ylocal_HDVR(j,iii+1)/SpeHDVR(j));
|
||||
end
|
||||
sommaZ(:,iii+1) = sum(dZ(:,1:iii),2);
|
||||
X_HDVR(:,iii+1) = cumsum(Xlocal_HDVR(:,iii+1));
|
||||
Y_HDVR(:,iii+1) = cumsum(Ylocal_HDVR(:,iii+1));
|
||||
Z_HDVR(:,iii+1) = cumsum(sommaZ(:,iii+1))+ Z_HDVR(:,1);
|
||||
HShift_HDVR(:,iii+1) = (X_HDVR(:,iii+1).^2+Y_HDVR(:,iii+1).^2).^0.5;
|
||||
HShift_local_HDVR(:,iii+1) = (Xlocal_HDVR(:,iii+1).^2+Ylocal_HDVR(:,iii+1).^2).^0.5;
|
||||
Zlocal_HDVR(:,iii+1) = sommaZ(:,iii+1) + SpeHDVR; % Zeta è il singolo abbassamento di quel nodo
|
||||
for rr = 1:rHDVR
|
||||
azim(rr,iii) = (acos(abs(X_HDVR(rr,iii))/HShift_HDVR(rr,iii)))*180/3.141592654; % Angolo Teta in gradi 0° - 90°
|
||||
segnoNS = sign(X_HDVR(rr,iii));
|
||||
segnoEO = sign(Y_HDVR(rr,iii));
|
||||
% L'azimuth si calcola con NS = 0, positivo in senso orario
|
||||
% (90° = Est)
|
||||
if segnoNS == 1 && segnoEO == 1 % quadrante 1
|
||||
Azimuth_HDVR(rr,iii+1) = azim(rr,iii); % Teta lo tengo come è (1 quadrante)
|
||||
elseif segnoNS == -1 && segnoEO == 1 % quadrante 2
|
||||
Azimuth_HDVR(rr,iii+1) = 180 - azim(rr,iii); % 180-teta
|
||||
elseif segnoNS == -1 && segnoEO == -1 % quadrante 3
|
||||
Azimuth_HDVR(rr,iii+1) = 180 + azim(rr,iii); % 180+teta
|
||||
elseif segnoNS == 1 && segnoEO == -1 % quadrante 4
|
||||
Azimuth_HDVR(rr,iii+1) = 360 - azim(rr,iii); % 360-teta
|
||||
elseif segnoNS == 0 && segnoEO == -1 % 270°
|
||||
Azimuth_HDVR(rr,iii+1) = 270;
|
||||
elseif segnoNS == -1 && segnoEO == 0 % 180°
|
||||
Azimuth_HDVR(rr,iii+1) = 180;
|
||||
end
|
||||
end
|
||||
end
|
||||
Azimuth_HDVR = real(Azimuth_HDVR);
|
||||
|
||||
%% Calcolo velocità di spostamento giornaliera
|
||||
[numDate,~] = size(ARRAYdateHDVR); % 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 = ARRAYdateHDVR(d) - ARRAYdateHDVR(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:rHDVR
|
||||
N = 1;
|
||||
for dd = 1:nDate
|
||||
Speed_HDVR(s,ContSUP(N,1)) = (HShift_HDVR(s,ContSUP(N,1))-HShift_HDVR(s,ContINF(N,1)))/(ARRAYdateHDVR(ContSUP(N,1))-ARRAYdateHDVR(ContINF(N,1)));
|
||||
Speed_local_HDVR(s,ContSUP(N,1)) = (HShift_local_HDVR(s,ContSUP(N,1))-HShift_local_HDVR(s,ContINF(N,1)))/(ARRAYdateHDVR(ContSUP(N,1))-ARRAYdateHDVR(ContINF(N,1)));
|
||||
Acceleration_HDVR(s,ContSUP(N,1)) = (Speed_HDVR(s,ContSUP(N,1))-Speed_HDVR(s,ContINF(N,1)))/(ARRAYdateHDVR(ContSUP(N,1))-ARRAYdateHDVR(ContINF(N,1)));
|
||||
Acceleration_local_HDVR(s,ContSUP(N,1)) = (Speed_local_HDVR(s,ContSUP(N,1))-Speed_local_HDVR(s,ContINF(N,1)))/(ARRAYdateHDVR(ContSUP(N,1))-ARRAYdateHDVR(ContINF(N,1)));
|
||||
N = N+1;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
% Approssimo i dati con il corretto numero di cifre decimali
|
||||
[X_HDVR,Y_HDVR,Z_HDVR,Xlocal_HDVR,Ylocal_HDVR,Zlocal_HDVR,HShift_HDVR,HShift_local_HDVR,Azimuth_HDVR,Speed_HDVR,...
|
||||
Speed_local_HDVR,Acceleration_HDVR,Acceleration_local_HDVR,tempHDVR] = approx_HD(X_HDVR,Y_HDVR,Z_HDVR,...
|
||||
Xlocal_HDVR,Ylocal_HDVR,Zlocal_HDVR,HShift_HDVR,HShift_local_HDVR,Azimuth_HDVR,Speed_HDVR,Speed_local_HDVR,...
|
||||
Acceleration_HDVR,Acceleration_local_HDVR,tempHDVR,FileName);
|
||||
|
||||
% Riordino matrice errori
|
||||
[r,~] = size(ErrTiltLinkHDVR);
|
||||
Matrice_err = zeros(r,rHDVR);
|
||||
for i = 1:r % date
|
||||
d = 1;
|
||||
for n = 1:rHDVR % nodi
|
||||
j = 1;
|
||||
err = ErrTiltLinkHDVR(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
|
||||
ErrTiltLinkHDVR = Matrice_err';
|
||||
|
||||
text = 'Tilt Link HD VR triaxial calculation executed correctly. triax_HDVR function ended';
|
||||
fileID = fopen(FileName,'a');
|
||||
fmt = '%s \r';
|
||||
fprintf(fileID,fmt,text);
|
||||
fclose(fileID);
|
||||
|
||||
end
|
||||
Reference in New Issue
Block a user