基于MEMS惯性地磁测量组件的人体运动跟踪与捕捉方法(7)
对式（2-41）、式（2-42）和式（2-45）求转置并合并，可得
因此，可得（2-47）
由于陀螺仪误差较大，因此不用陀螺仪的信号进行初始对准，而直接采用加速度计的信号来进行水平姿态角的对准。
由于 ，将式（2-46）代入式（2-42），得
(2-48)
因此可计算2个水平姿态角为
（2-49）
2.2.5 四元数算法整体流程
综上所述，四元数算法流程为:首先进行初对准得到初始姿态信息。利用陀螺仪测量载体相对于惯性空间的角速度 ，然后借助空间几何关系由陀螺仪的输出求 ，其中要用到 。解决这个问题要用到上一时刻的 进行迭代，进而求出当前时刻的 。因此， 的求解是一个循环迭代过程。从姿态阵的元素中提取载体的姿态信息求得 、 、 。如下图,
图2.1 四元数算法流程图
2.3 基于FQA算法的姿态捕捉方法
因式分解的四元数算法，即FQA算法，此算法输出一组四元数代表载体的姿态。通过基于半角公式的推导并且由于使用了四元数使此算法计算过程避免了三角函数[2]。下面介绍此算法的计算过程。
将一个传感器模块作为惯性测量单元（IMU）附属于需姿态捕捉的载体上。为便于分析，首先定义：地球固定坐标系xeyeze遵循东-北-天（NED）公约，即：xe指向东，ye指向北，ze指向天。身体坐标系xbybzb附属于被测姿态的载体。传感器模块有它自己的坐标系xsyszs与三个正交安装的加速度计/磁传感器轴一致。因为传感器模块固定在物体上，身体坐标系xbybzb与传感器坐标系统xsyszs有一个常偏置量。为方便讨论，假设身体坐标系统与传感器坐标系统相一致。
姿态角的定义为：
（1）关于ｚ轴旋转一个角度 为方位角，其范围为：a- ；
（2）关于y轴旋转一个角度 为俯仰角，其范围为−π/2 ≤ θ ≤ π/2；
（3）关于x轴旋转一个角度 为横滚角，其范围为−π < ≤ π。
2.3.1 俯仰角解算方法
当一个物体以恒定的速率向固定的方向运动时，x轴加速度计垂直于重力，因此零加速度。y轴加速度计也为零，z轴加速度计为-g。如果物体按z轴旋转一个角度，则无论方位角为多少度x轴加速度计始终为零。如果刚体以一个角度θ上抛,x轴加速度计为
(2-50)z轴加速度计为 (2-51)
设重力加速度为g=9.81m/s2。且身体坐标系统测量的加速度矢量为 (2-52)
为了方便，加速度计和磁传感器从传感器模块输出的是标准的单位向量。即 (2-53)
|a|是加速度矢量a的模。有式（2-57） 的值可以表示为(2-54)
可通过下式计算出来(2-55)
再利用半角公式求出 和
则俯仰角四元数为 (2-57)
2.3.2 橫滚角解算方法
当物体做橫滚运动时，y轴加速度计为
cos 的值由式(2-55)决定。若cos 不等于零，则sin 和cos 可由下式决定
如果cosθ等于零,则意着身体坐标的x轴垂直定位方向。 的半角值可以用式(2-56)计算。橫滚四元数为
(2-64)
2.3.3 方位角解算方法
因为方位角不受角速度计的影响，因此FQA算法是思路是首先计算方位角与橫滚角，然后计算方位角。把身体坐标系 (2-65)
投影到地面坐标系的旋转操作为 (2-66)
式中 、 代表三文矢量中的纯矢量四元数，例如，bm=（0bmxbｍｙbｍｙ）。 应该与已知的当地的正规化磁场矢量n=［ｎｘｎｙｎｚ］T一致,即： 。且