Sync from remote server: 2025-10-12 18:56:41
This commit is contained in:
175
Tilt/QuaternioniASE.m
Executable file
175
Tilt/QuaternioniASE.m
Executable file
@@ -0,0 +1,175 @@
|
||||
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
|
||||
Reference in New Issue
Block a user