matlab-python/Tilt/QuaternioniASE.m

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