- 上一篇：主从方式无线传输的弹体飞行时间测量系统
- 下一篇：μVISION高精度三维磁场测量电路设计与实现+源代码
式中 是方位角。而在实际应用中，在存在磁干扰和测量噪声的情况下，其长度会有误差。为了弥补这种影响，将式(2-67)两边的向量标准化。即
并且磁传感器测量的相应的数量是
则cos 和sin 的值可以从下式解出(2-71)
方向角的范围为- 。由式（2-56）给出的半角公式又可以用来计算 的正弦和余弦。方向四元数给出为 (2-72)
得到三个旋转四元数，用四元数估计的载体方向最终表示为 （2-73）
综上所述，FQA算法先由三轴加速度计测量值计算俯仰角和橫滚角四元数，再由磁传感器测量的数据计算航向角四元数，然后把三个四元数相乘得到载体姿态四元数。
2.4 四元数算法与FQA算法结合的姿态捕捉方法
前两节介绍了四元数算法与FQA算法的理论推导，在实际应用中这两种算法分别有各自的优缺点。四元数算法是一个比较成熟的算法，由式（2-34）可以看出，此方法可以全姿态工作；同时微分方程只有四文，相比其他姿态解算方法（如：方向余弦法）计算量降低，且性能优于其他方法。因此，四元数法成为目前惯性系统求解姿态的主要方法。但是，因为此算法为迭代过程，角速率在积分过程中会将误差累计，所以长时间应用四元数法误差会随时间的增长而逐渐增加。
FQA算法是一种简化四元数算法，顾名思义FQA算法与四元数算法相比计算量小很多。但是，FQA算法只能解算静态或缓慢移动的载体姿态，并不适用于相对较大的线性加速度。(以上两种算法的优缺点将在第四章中根据实验数据进行详细分析与比较。)
因此提出改进方案，将四元数算法与FQA算法结合使用，当人体移动缓慢时使用FQA算法，当人体运动剧烈程度加剧时自动转换为用四元数算法。这样就在一定程度上克服了四元数方法误差累计的缺点且兼备了两种算法的优点。
为实现FQA算法与四元数算法的相互转换要设计一个判断条件，用来判断人体运动的剧烈程度。
2.4.1 四元数算法与FQA算法转换条件
1、 设加速度矢量和 为（2-74）
表示三轴加速度和的模。
当载体静止时 应为当地的重力加速度，当载体运动时 的值会随之改变。则以当地重力加速度为参照，根据具体实验数据选择适当阈值可以判断载体的运动状况。给 设定一个范围 ，使得在此范围内系统使用FQA算法解算，否则使用四元数算法解算。
但是经试验验证，只有这一个判断条件时解算结果仍然不理想，误差较大。经分析，这些误差点的加速度测量值本应在阈值以外，理论上应该用四元数算法计算但是实际用的FQA算法。可能是由于传感器有随机误差使系统产生误判。因此一个判断条件是不够的。
2、 设加速度和 为 (2-75)
表示三轴加速度的和。
载体静止时 的数值为参照，认为当三轴加速度计中任意轴加速度计值增大过减小，即 值发生变化时，载体在运动。因此可以根据实验数据选择适当的阈值来判断载体是做静止或缓慢运动，还是做剧烈运动。给 设定一个范围 ，并将两个条件相与，即：
&& （2-76）
表示当这两个条件同时满足时使用FQA算法，否则应用四元数算法。
但是实验证明，用上述条件进行判断时，解算结果虽然比只用 理想，但是还会有个别明显误差点。分析其原因，可能是因为只涉及到了加速度计测量值的判断。当载体做剧烈运动时，不仅线加速度会变化，角速率也会发生变化。所以只用加速度信息做为判断依据是不合理的，应加入角速率信息。