107 lines
2.5 KiB
Matlab
Executable File
107 lines
2.5 KiB
Matlab
Executable File
function [q, error, flag] = fqa(VaN,VmN)
|
|
% a is 3x1, m is 3x1
|
|
|
|
% Xiaoping Yun, May 7, 2008
|
|
% input a = 3-dim acceleration, m=3-dim local magnetic measurement
|
|
|
|
%Mref = [0.4943 0.0 0.8693];
|
|
|
|
epsilon = 0.10; % accuracy control constant
|
|
alpha = 30*pi/180; % offset angle
|
|
|
|
% x is 3x2, first col = magnetometer, second col = accelerometer.
|
|
|
|
a_bar = VaN;
|
|
m_bar = VmN;
|
|
% a_bar = VaN/norm(VaN); % make sure that it is normalized
|
|
% m_b = VmN/norm(VmN);
|
|
|
|
sin_th = a_bar(1);
|
|
cos_th = sqrt(1-sin_th^2);
|
|
|
|
% singularity avoidance algorithm
|
|
if (cos_th <= epsilon)
|
|
singular_flag = 1;
|
|
q_offset =cos(alpha/2)*[1 0 0 0]' + sin(alpha/2)*[0 0 1 0]';
|
|
|
|
a_bar_q = [0; a_bar];
|
|
m_bar_q = [0; m_bar];
|
|
a_q_offset = rotate_v_by_q(a_bar_q,q_offset);
|
|
m_q_offset = rotate_v_by_q(m_bar_q,q_offset);
|
|
|
|
a_bar = a_q_offset(2:4);
|
|
m_bar = m_q_offset(2:4);
|
|
|
|
else
|
|
% do not do anything other than setting the flag.
|
|
singular_flag = 0;
|
|
end
|
|
|
|
%% elevation quaternion
|
|
sin_th = a_bar(1);%h(1);
|
|
%cos_th = sqrt(1-sin_th^2);
|
|
cos_th = sqrt(a_bar(2)^2 + a_bar(3)^2); %J.C. 1/30/2009
|
|
|
|
% computing half-angle values
|
|
cos_half_th=sqrt((1+cos_th)/2);
|
|
if (cos_th<=-1) % this "if" is needed since sign(0) = 0.
|
|
sin_half_th = 1;
|
|
else
|
|
sin_half_th=sign(sin_th)*sqrt((1-cos_th)/2);
|
|
end
|
|
|
|
qe = cos_half_th*[1;0;0;0] + sin_half_th*[0;0;1;0];
|
|
|
|
%% Roll Quaternion
|
|
b = [a_bar(2) a_bar(3)];
|
|
c = b/norm(b);
|
|
sin_phi = -c(1);
|
|
cos_phi = -c(2);
|
|
|
|
cos_half_phi=sqrt((1+cos_phi)/2);
|
|
if (cos_phi<=-1)
|
|
sin_half_phi = 1;
|
|
else
|
|
sin_half_phi=sign(sin_phi)*sqrt((1-cos_phi)/2);
|
|
end
|
|
|
|
qr = cos_half_phi*[1;0;0;0] + sin_half_phi*[0;1;0;0];
|
|
|
|
%% Azimuth Quaternion
|
|
m_bar_q = [0; m_bar];
|
|
|
|
q_er = q_mult2(qe,qr);
|
|
q_er_inv =[q_er(1); -q_er(2); -q_er(3); -q_er(4)];
|
|
m_e = q_mult2(q_er,q_mult2(m_bar_q, q_er_inv));
|
|
|
|
M = [m_e(2),m_e(3)];
|
|
M = M/norm(M);
|
|
N = [1; 0];
|
|
tmp = [ M(1) M(2);
|
|
-M(2) M(1)]*N;
|
|
cos_psi = tmp(1);
|
|
sin_psi = tmp(2);
|
|
|
|
cos_half_psi=sqrt((1+cos_psi)/2);
|
|
if (cos_psi<=-1) %%%% IMPORTANT %%% if it is written as cos_psi==-1, it does not work.
|
|
%%%% cos_psi is potentially less than -1.
|
|
sin_half_psi = 1;
|
|
else
|
|
sin_half_psi=sign(sin_psi)*sqrt((1-cos_psi)/2);
|
|
end
|
|
|
|
qa = cos_half_psi*[1;0;0;0]+ sin_half_psi*[0;0;0;1];
|
|
|
|
q_tmp1 = q_mult2(qe,qr);
|
|
q_tmp = q_mult2(qa,q_tmp1);
|
|
|
|
if (singular_flag == 1)
|
|
q = q_mult2(q_tmp, q_offset);
|
|
else
|
|
q = q_tmp;
|
|
end
|
|
|
|
error = cos_th;
|
|
flag = singular_flag;
|
|
|
|
end |