将陀螺仪数据转换为仰角与水平旋转角

突然想起一年多前，需要将手机空间姿态，同步到一个设备上，而这个设备只能水平旋转或者改变仰角

首先，先读取手机初始状态四元数，记为$q_0$,因为这个空间姿态的变化相对于手机来说是相对的，然后再读取每一时刻的四元数，记为$q$,记相对初始状态的相对四元数为$q_1$，根据四元数旋转的复合，那么有

$$\begin{aligned} q&=q_0q_1\\ 即q_1&=q_0^{-1}q \end{aligned}$$

那么到此，我们已经得到了相对四元数$q_1$。

为了获得使手机的朝向，根据下图，我们可以构造一个3D向量$(0,1,0)$，代表手机指向的方向，记为$p_0$

将$q_1$旋转作用于$p_0$，可得

$$p_1=q_1p_0q_1^{-1}$$

显然，$p_1$的坐标即代表着手机朝向，将其记为$(x,y,z)$

记与$x$轴的夹角为$\alpha \in(-\pi,\pi]$,与$y$轴的夹角为$\beta\in[-\frac{\pi}{2},\frac{\pi}{2}]$，可得

$$\begin{aligned} \cos \alpha&=\frac{x}{\sqrt{x^2+y^2}}\\ \tan \beta &=\frac{z}{\sqrt{x^2+y^2}} \end{aligned}$$

易得

$$\alpha =\left\{ \begin{array}{l} \arccos \frac{x}{\sqrt{x^2+y^2}}&{y \geq 0}\\ -\arccos \frac{x}{\sqrt{x^2+y^2}}&{y<0} \end{array} \right.$$

$$\beta =\left\{ \begin{array}{l} |\arctan \frac{z}{\sqrt{x^2+y^2}}|&{z \geq 0}\\ -|\arctan \frac{z}{\sqrt{x^2+y^2}}|&{z<0} \end{array} \right.$$

注：上文出现的三维向量与纯四元数的转换均为隐式转换，即$(a,b,c)=\{0,a,b,c\}$

代码实现
注意：代码实现中的变量命名与上文不相同,水平旋转角的参考系也有些差别

参考资料

四元数与三维旋转

Microsoft Docs