alex/matlab-python
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
876ef073fce86490a3d03f9d9b612e40a9a06c19
matlab-python/Tilt/biax_TLH.m

426 lines
15 KiB
Matlab
Executable File
Raw Blame History

% Questa elaborazione semplificata NON usa i quaternioni e i dati di campo
% magnetico
function [Y_TLH,Z_TLH,Xlocal_TLH,Ylocal_TLH,Zlocal_TLH,AlfaX_TLH,AlfaY_TLH,TempDef_TLH,...
Speed_TLH,Speed_local_TLH,Acceleration_TLH,Acceleration_local_TLH,ARRAYdateTLH,...
ErrTiltLinkH] = biax_TLH(IDcentralina,DTcatena,rTLH,ACCdef_TLH,ACCdefRis_TLH,...
TempDef_TLH,SpeTLH,PsTLH,tolleranzaAcc,DatiElabTiltLinkH,Ndevst,Wdevst,ARRAYdateTLH,...
NuovoZeroTLH,Tmax,Tmin,NodoTiltLinkH,NdatiMedia,Ndatidespike,Traversine,ErrTiltLinkH,...
margine,FileName)
fileID = fopen(FileName,'a');
fmt = '%s \r';
text = 'biax_TLH function started';
fprintf(fileID,fmt,text);
if NuovoZeroTLH == 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
if Ndevst ~= 0 % Allora prendo tutti i dati e solo in seguito considero i nuovi, a valle della funzione filtro
ini = 1;
end
ACCdef_TLH = ACCdef_TLH(ini:end,:);
ACCdefRis_TLH = ACCdefRis_TLH(ini:end,:);
TempDef_TLH = TempDef_TLH(ini:end,:);
DatiElabTiltLinkH = DatiElabTiltLinkH(ini:end,:);
ARRAYdateTLH = ARRAYdateTLH(ini:end,1);
ErrTiltLinkH = ErrTiltLinkH(ini:end,:);
end
%% Definisco i dati
Nnodi = rTLH;
% Spacchetto
[r,~] = size(ACCdefRis_TLH); % Numero di dati
[Ndati,~] = size(ARRAYdateTLH);
ax = zeros(r,Nnodi);
ay = zeros(r,Nnodi);
for i=1:Nnodi
ax(:,i) = ACCdef_TLH(:,(i-1)*3+1); % ax
ay(:,i) = ACCdef_TLH(:,(i-1)*3+2); % ay
end
ax = ax'; % riga nodi, colonna date
ay = ay'; % riga nodi, colonna date
% Dati locali
asseX_TLH = zeros(Nnodi,Ndati); % Raccoglie i dati planari locali nella direzione perpendicolare allo sviluppo della catena
asseY_TLH = zeros(Nnodi,Ndati); % Raccoglie i dati planari locali nella direzione di sviluppo della catena
asseZ_TLH = zeros(Nnodi,Ndati); % Raccoglie i dati planari locali nella direzione di sviluppo della catena, basso->alto
% Dati cumulati
Y_TLH = zeros(Nnodi,Ndati); % Raccoglie i dati cumulati nella direzione di sviluppo della catena
Z_TLH = zeros(Nnodi,Ndati); % Raccoglie i dati cumulati nella direzione di sviluppo della catena, basso->alto
% Angoli
AlfaX_TLH = zeros(Nnodi,Ndati);
AlfaY_TLH = zeros(Nnodi,Ndati);
% parametri per il calcolo
SpeTLH = SpeTLH(2:end,1); % valori segmenti di pertinenza
PsTLH = PsTLH(2:end,1); % valori posizioni punti di calcolo
% Inizio del ciclo di elaborazione
text = 'Elaboration of Tilt Link H started';
fprintf(fileID,fmt,text);
% Nodi nuovi (lo devo sapere per le convenzioni)
% I due assi sono a 90° di cui asse Y a 90° verso sinistra rispetto allo
% sviluppo della catena, asse X a 0° coincidente con lo sviluppo della catena.
% L'asse Z è verso il basso.
for jj = 1:Ndati
for ii = 1:Nnodi
[Y_TLH,Z_TLH,asseX_TLH,asseY_TLH,asseZ_TLH,AlfaX_TLH,AlfaY_TLH] = CalcoloBiax_TLH(...
SpeTLH,PsTLH,ii,jj,ax,ay,asseX_TLH,asseY_TLH,asseZ_TLH,Y_TLH,Z_TLH,AlfaX_TLH,...
AlfaY_TLH,Traversine);
end
end
dX_TLH = diff(asseX_TLH,1,2);
dY_TLH = diff(asseY_TLH,1,2);
dZ_TLH = diff(asseZ_TLH,1,2);
%% Controllo delle risultanti di accelerazione e campo magnetico
clear r
clear rr
clear c
clear cc
ACCdefRis_TLH = ACCdefRis_TLH'; % Nodi in riga, date in colonna
[r,c] = size(ACCdefRis_TLH);
[rr,cc] = size(asseZ_TLH);
% 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
TempDef_TLH = TempDef_TLH';
textA = 'There are not correction of Tilt Link H based on acceleration vectors filter';
textA2 = 'There are not correction of Tilt Link H based on uncalibrated acceleration vectors';
textT = 'There are not correction of Tilt Link H 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(ACCdefRis_TLH(i,j)-ACCdefRis_TLH(i,j-1)) > tolleranzaAcc
dX_TLH(i,j-1) = 0;
dY_TLH(i,j-1) = 0;
dZ_TLH(i,j-1) = 0;
textA = ['' num2str(cont) ' correction executed for Tilt Link H - Acceleration vector filter!'];
cont = cont+1;
end
if ACCdefRis_TLH(i,j) < 0.9 || ACCdefRis_TLH(i,j) > 1.1 % Il nodo è fuori taratura!
dX_TLH(i,j-1) = 0;
dY_TLH(i,j-1) = 0;
dZ_TLH(i,j-1) = 0;
TempDef_TLH(i,j) = TempDef_TLH(i,j-1);
textA2 = ['' num2str(cont3) ' correction executed for Tilt Link H - uncalibrated Acceleration vector!'];
cont3 = cont3+1;
end
end
end
FileTemperature = ['' IDcentralina '-' DTcatena '-TLH-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 TempDef_TLH(a,b) > Tmax || TempDef_TLH(a,b) < Tmin
cont2 = cont2+1;
if b == 1
if isfile(FileTemperature) == 1
DatiRaw = csvread(RawDataFile);
[rDR,cDR] = size(DatiRaw);
RawDate = find(DatiRaw(:,1)<=datenum(datainiTLH));
if isempty(RawDate) == 1
cc = 2;
while cc <= c
if TempDef_TLH(a,cc) > Tmax || TempDef_TLH(a,cc) < Tmin
cc = cc+1;
else
break
end
end
TempDef_TLH(a,b) = TempDef_TLH(a,cc);
else
if isnan(DatiRaw(RawDate(end),a+1)) == 0
TempDef_TLH(a,b) = DatiRaw(RawDate(end),a+1);
ErrTiltLinkH(1,a) = 0.5;
wardat = 'Temperature data of Tilt Link H nodes corrected using Raw Data of reference Csv file.';
fprintf(fileID,fmt,wardat);
else
cc = 2;
while cc <= c
if TempDef_TLH(a,cc) > Tmax || TempDef_TLH(a,cc) < Tmin
cc = cc+1;
else
break
end
end
TempDef_TLH(a,b) = TempDef_TLH(a,cc);
end
end
else
cc = 2;
while cc <= c
if TempDef_TLH(a,cc) > Tmax || TempDef_TLH(a,cc) < Tmin
cc = cc+1;
else
break
end
end
TempDef_TLH(a,b) = TempDef_TLH(a,cc);
end
else
TempDef_TLH(a,b) = TempDef_TLH(a,b-1);
dY_TLH(i,j-1) = 0;
dZ_TLH(i,j-1) = 0;
ErrTiltLinkH(1,a) = 0.5;
end
textT = ['' num2str(cont2) ' correction executed for Tilt Link H - Temperature filter!'];
end
end
end
if rDR~=1 && cDR~=1 && isempty(DatiRaw) == 0
RawDate1 = find(DatiRaw(:,1)<=ARRAYdateTLH(1));
if isempty(RawDate1) == 1
RawDate2 = 1;
elseif RawDate1(end) == rDR
RawDate2 = find(ARRAYdateTLH(:,1)>DatiRaw(end,1));
else
RawDate2 = find(ARRAYdateTLH(:,1)>DatiRaw(RawDate1(end)+1,1));
end
else
RawDate1 = [];
RawDate2 = 1;
end
if isempty(RawDate1) == 0 && isempty(RawDate2) == 0
Dati = [DatiRaw(1:RawDate1(end),:); ARRAYdateTLH(RawDate2(1):end) TempDef_TLH(:,RawDate2(1):end)'];
elseif isempty(RawDate1) == 1 && isempty(RawDate2) == 0
Dati = [ARRAYdateTLH TempDef_TLH'];
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);
TempDef_TLH = TempDef_TLH';
fprintf(fileID,fmt,textA);
fprintf(fileID,fmt,textA2);
fprintf(fileID,fmt,textT);
fclose(fileID);
[dX_TLH,dY_TLH,dZ_TLH] = azzeramenti_TLH(IDcentralina,DTcatena,dX_TLH,...
dY_TLH,dZ_TLH,NodoTiltLinkH,FileName);
%% Filtro
[dX_TLH,dY_TLH,dZ_TLH] = filtro_TLH(dX_TLH,dY_TLH,dZ_TLH,Ndevst,Wdevst,FileName);
fileID = fopen(FileName,'a');
fprintf(fileID,fmt,text);
if NuovoZeroTLH == 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
dX_TLH = dX_TLH(:,ini:end);
dY_TLH = dY_TLH(:,ini:end);
dZ_TLH = dZ_TLH(:,ini:end);
TempDef_TLH = TempDef_TLH(ini:end,:);
DatiElabTiltLinkH = DatiElabTiltLinkH(ini:end,:);
ARRAYdateTLH = ARRAYdateTLH(ini:end,1);
ErrTiltLinkH = ErrTiltLinkH(ini:end,:);
end
%% Finalizzo i calcoli
[rx,cx] = size(dZ_TLH);
% totale del singolo nodo data per data
sommaX_TLH = zeros(rx,cx+1);
sommaY_TLH = zeros(rx,cx+1);
sommaZ_TLH = zeros(rx,cx+1);
% cumulata del singolo nodo data per data in posizione assoluta
% il primo valore è quello di posizione assoluta iniziale
Y_TLH = zeros(rx,cx+1);
Z_TLH = zeros(rx,cx+1);
% cumulata del singolo nodo data per data in posizione relativa
% il primo valore è quello di posizione assoluta iniziale
Xlocal_TLH = zeros(Nnodi,Ndati);
Ylocal_TLH = zeros(Nnodi,Ndati);
Zlocal_TLH = zeros(Nnodi,Ndati);
Speed_TLH = zeros(Nnodi,Ndati);
Speed_local_TLH = zeros(Nnodi,Ndati);
Acceleration_TLH = zeros(Nnodi,Ndati);
Acceleration_local_TLH = zeros(Nnodi,Ndati);
if NuovoZeroTLH == 1
[rE,cE] = size(DatiElabTiltLinkH);
cont = 4;
n = 1;
while cont<=cE
sommaX_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont-1))';
Xlocal_TLH(n,1) = sommaX_TLH(n,1);
sommaY_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont))';
Ylocal_TLH(n,1) = sommaY_TLH(n,1);
sommaZ_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont+1))';
Zlocal_TLH(n,1) = sommaZ_TLH(n,1);
Y_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont+3))';
Z_TLH(n,1) = cell2mat(DatiElabTiltLinkH(1,cont+4))';
Speed_local_TLH(n,1:rE) = cell2mat(DatiElabTiltLinkH(:,cont+7))';
Speed_TLH(n,1:rE) = cell2mat(DatiElabTiltLinkH(:,cont+6))';
Acceleration_local_TLH(n,1:rE) = cell2mat(DatiElabTiltLinkH(:,cont+9))';
Acceleration_TLH(n,1:rE) = cell2mat(DatiElabTiltLinkH(:,cont+8))';
cont = cont+13;
n = n+1;
end
else
% Primo dato
Y_TLH(:,1) = PsTLH;
Z_TLH(:,1) = 0;
end
for iii = 1:cx % date
% somma singoli spostamenti del singolo nodo
sommaX_TLH(:,iii+1) = sum(dX_TLH(:,1:iii),2)+sommaX_TLH(:,1);
sommaY_TLH(:,iii+1) = sum(dY_TLH(:,1:iii),2)+sommaY_TLH(:,1);
sommaZ_TLH(:,iii+1) = sum(dZ_TLH(:,1:iii),2)+sommaZ_TLH(:,1);
% spostamenti locali riferiti allo 0
Xlocal_TLH(:,iii+1) = sommaX_TLH(:,iii+1);
Ylocal_TLH(:,iii+1) = sommaY_TLH(:,iii+1);
Zlocal_TLH(:,iii+1) = sommaZ_TLH(:,iii+1);
% cumulata spostamenti
Y_TLH(:,iii+1) = Y_TLH(:,1) + cumsum(sommaY_TLH(:,iii+1)) - cumsum(sommaY_TLH(:,1));
Z_TLH(:,iii+1) = Z_TLH(:,1) + cumsum(sommaZ_TLH(:,iii+1)) - cumsum(sommaZ_TLH(:,1));
end
%% Calcolo velocità di spostamento giornaliera
[numDate,~] = size(ARRAYdateTLH); % 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 = ARRAYdateTLH(d) - ARRAYdateTLH(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:rTLH
N = 1;
for dd = 1:nDate
Speed_TLH(s,ContSUP(N,1)) = (Z_TLH(s,ContSUP(N,1))-Z_TLH(s,ContINF(N,1)))/(ARRAYdateTLH(ContSUP(N,1))-ARRAYdateTLH(ContINF(N,1)));
Speed_local_TLH(s,ContSUP(N,1)) = (Zlocal_TLH(s,ContSUP(N,1))-Zlocal_TLH(s,ContINF(N,1)))/(ARRAYdateTLH(ContSUP(N,1))-ARRAYdateTLH(ContINF(N,1)));
Acceleration_TLH(s,ContSUP(N,1)) = (Speed_TLH(s,ContSUP(N,1))-Speed_TLH(s,ContINF(N,1)))/(ARRAYdateTLH(ContSUP(N,1))-ARRAYdateTLH(ContINF(N,1)));
Acceleration_local_TLH(s,ContSUP(N,1)) = (Speed_local_TLH(s,ContSUP(N,1))-Speed_local_TLH(s,ContINF(N,1)))/(ARRAYdateTLH(ContSUP(N,1))-ARRAYdateTLH(ContINF(N,1)));
N = N+1;
end
end
end
fclose(fileID);
% Approssimo i dati con il corretto numero di cifre decimali
[Y_TLH,Z_TLH,Ylocal_TLH,Zlocal_TLH,Speed_TLH,Speed_local_TLH,...
Acceleration_TLH,Acceleration_local_TLH,TempDef_TLH] = approx_TLH(...
Y_TLH,Z_TLH,Ylocal_TLH,Zlocal_TLH,Speed_TLH,Speed_local_TLH,...
Acceleration_TLH,Acceleration_local_TLH,TempDef_TLH,FileName);
% Riordino matrice errori
[r,~] = size(ErrTiltLinkH);
Matrice_err = zeros(r,rTLH);
for i = 1:r % date
d = 1;
for n = 1:rTLH % nodi
j = 1;
err = ErrTiltLinkH(i,d:d+3);
while j <= 4
if err(1,j) == 1
Matrice_err(i,n) = 1;
end
j = j+1;
end
d = d+4;
end
end
ErrTiltLinkH = Matrice_err';
text = 'Tilt Link H calculation executed correctly';
fileID = fopen(FileName,'a');
fmt = '%s \r';
fprintf(fileID,fmt,text);
fclose(fileID);
end
Reference in New Issue View Git Blame Copy Permalink