function [DNS,DEO,Dz,becc,roll,imba,q,qy]=QuaternioniASE(axb,ayb,azb,mxb,myb,mzb,SP,MEMS,FileName) % OPERAZIONI PRELIMINARI % calcola la norma dell'accelerazione e del campo magnetico Na = (axb^2+ayb^2+azb^2)^0.5; Nm = (mxb^2+myb^2+mzb^2)^0.5; % normalizzo l'accelerazione axbN = axb/Na; aybN = ayb/Na; azbN = azb/Na; % normalizzo il campo magnetico mxbN = mxb/Nm; mybN = myb/Nm; mzbN = mzb/Nm; if MEMS == 1 % Correggo i segni degli assi % correggo la direzione degli assi del sistema di coordinate % aggiornato al 15 febbraio 2016 axsN = axbN; aysN = aybN; azsN = -azbN; % anche per il campo magnetico... mxsN = -mxbN; mysN = -mybN; mzsN = mzbN; elseif MEMS == 2 % correggo la direzione degli assi del sistema di coordinate % aggiornato al 09 febbraio 2017 axsN = axbN; aysN = aybN; azsN = -azbN; % anche per il campo magnetico... mxsN = -mxbN; mysN = -mybN; mzsN = mzbN; elseif MEMS == 3 % correggo la direzione degli assi del sistema di coordinate % aggiornato al 10 ottobre 2022 axsN = axbN; aysN = aybN; azsN = azbN; % anche per il campo magnetico... mxsN = mybN; mysN = mxbN; mzsN = -mzbN; end % trovo i vettori dei campi gravitazionale e magnetico VaN = [axsN aysN azsN]; VmN = [mxsN mysN mzsN]; % CONTI SECONDO YUN [qy,~,~] = fqa(VaN',VmN'); qy = qy'; % determino i relativi quaternioni qaN = [0 VaN]; qmN = [0 VmN]; % calcolo il coseno del beccheggio % gamma = acos(azsN); gamma non viene mai usato COSteta = (1-axsN^2)^0.5; % VERIFICA DELLA SINGOLARITA' % impongo un epsilon piccolo a piacere epsilon = 0.1; if COSteta <= epsilon % 20 deg espresso in radianti alpha = 0.3491; else alpha = 0; end SENalphamezzi = sign(sin(alpha))*((1-cos(alpha))/2)^0.5; COSalphamezzi = ((1+cos(alpha))/2)^0.5; %% calcolo il quaternione qalpha e il suo coniugato qalpha = [COSalphamezzi 0 SENalphamezzi 0]; qalphaC = quatconj(qalpha); % modifico il vettore accelerazione... qacc = quatmultiply(qalpha,qaN); qacc = quatmultiply(qacc,qalphaC); % ...e quello campo magnetico qmag = quatmultiply(qalpha,qmN); qmag = quatmultiply(qmag,qalphaC); %% CALCOLA IL QUATERNIONE ELEVAZIONE % NB: il valore di COSteta sarà riscritto SENteta = qacc(2); COSteta = (1-SENteta^2)^0.5; SENtetamezzi = sign(SENteta)*((1-COSteta)/2)^0.5; COStetamezzi = ((1+COSteta)/2)^0.5; qe = [COStetamezzi 0 SENtetamezzi 0]; %% CALCOLA IL QUATERNIONE ROLLIO SENphi = -qacc(3)/COSteta; COSphi = -qacc(4)/COSteta; % caso in cui l'asse x sia orientato verticalmente if COSteta == 0 SENphi = 0; COSphi = 1; end % determinazione degli elementi di qr if COSphi == -1 SENphimezzi = 1*((1-COSphi)/2)^0.5; COSphimezzi = 0; else SENphimezzi = sign(SENphi)*((1-COSphi)/2)^0.5; COSphimezzi = ((1+COSphi)/2)^0.5; if isreal(SENphimezzi) elseif imag(SENphimezzi)<0.001 SENphimezzi=0; text = 'SENphi/2 corrected'; fileID = fopen(FileName,'a'); fmt = '%s \r'; fprintf(fileID,fmt,text); fclose(fileID); end end % scrittura di qr qr = [COSphimezzi SENphimezzi 0 0]; %% CALCOLA IL QUATERNIONE AZIMUTALE % Campo magnetico nel sistema di coordinate Earth qaus = quatmultiply(qe,qr); qauC = quatconj(qaus); qEm = quatmultiply(qaus,qmag); qEm = quatmultiply(qEm,qauC); % Campo magnetico di riferimento Nx = 1; Ny = 0; % Normalizzazione delle componenti orizzontali del campo magnetico misurato mxmearot = qEm(2); mymearot = qEm(3); Mx = (1/(mxmearot^2+mymearot^2)^0.5)*mxmearot; My = (1/(mxmearot^2+mymearot^2)^0.5)*mymearot; % determino coseno e seno dell'angolo di imbardata COSpsi = Mx*Nx+My*Ny; SENpsi = -My*Nx+Mx*Ny; % determinazione degli elementi di qa SENpsimezzi = sign(SENpsi)*((1-COSpsi)/2)^0.5; COSpsimezzi = ((1+COSpsi)/2)^0.5; % scrittura di qa qa = [COSpsimezzi 0 0 SENpsimezzi]; % QUATERNIONE FINALE q = quatmultiply(qa,qe); q = quatmultiply(q,qr); qINV = [q(:,1) q(:,2).*-1 q(:,3).*-1 q(:,4).*-1]; % RIPERCORRE AL CONTRARIO LA ROTAZIONE IMPOSTA PER EVITARE LA SINGOLARITA' q = quatmultiply(q,qalpha); Q0 = q(1); Q1 = q(2); Q2 = q(3); Q3 = q(4); % CALCOLO GLI ANGOLI DAI QUATERNIONI roll = atan2(2*(Q0*Q1+Q2*Q3),(1-2*(Q1^2+Q2^2))); becc = asin(2*(Q0*Q2-Q3*Q1)); imba = atan2(2*(Q0*Q3+Q1*Q2),(1-2*(Q2^2+Q3^2))); % Calcolo degli spostamenti relativi al nodo qVett = [0 0 0 SP]; qVettR = quatmultiply(q,qVett); qVettR = quatmultiply(qVettR,qINV); DNS = qVettR(2); DEO = qVettR(3); Dz = qVettR(4); end