Sync from remote server: 2025-10-12 18:56:41

Tilt/fqa.m

@@ -0,0 +1,107 @@
+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