3、设置角速率矢量和 为  (2-77)
 表示三轴角速率和的模。
当载体静止时，三轴陀螺仪输出应均为零，即 ；随着载体运动剧烈程度加大， 的值也随之加大并与载体运动剧烈程度成正比。根据实验数据给 设定一个范围 ，并与前两个条件相与为
 && &&    （2-78）
表示当上述三个条件同时满足时用FQA算法，否则用四元数算法。
2.4.2 四元数算法与FQA算法结合算法整体流程
综上所述，将四元数与FQA算法结合的姿态捕捉算法整体流程如下图
图2.2 综合算法流程图
图中判断条件为 && && 。如图所示，程序先将实验采集的数据读入，计算计算加速度和、加速度和、角速率矢量和。然后判断是否满足条件，若满足条件，用FQA算法，否则用四元数算法。四元数算法解算过程最开始需要一个初始姿态数据，当单独使用四元数算法时先要初始对准。但是当FQA与四元数结合使用时，这个初始姿态就由前一时刻FQA算法解算的姿态信息代替。因此，每当由FQA算法切换到四元数元数算法时，前一时刻FQA算法解算的姿态信息就作为此刻四元数解算所需的初始姿态信息。

2.5 本章小结
本章阐述了惯性测量单元的定义及基本原理；介绍了姿态捕捉的两种算法的理论推导过程及整体流程，一种是传统的载体解算方法四元数法，另一种是分解四元数算法。由于上述两种方法都有各自的应用条件及优缺点，本章又提出了将两种算法结合的姿态捕捉方法。介绍了两种算法相互转换的方式和条件，最后介绍了将两种算法结合的整体算法流程。
三 人体姿态捕捉系统软件实现
第二章介绍了三种姿态捕捉算法的理论推导过程：四元数法、FQA法和四元数与FQA的结合算法。本章以第二章为基础，描述用MATLAB软件实现算法的过程。介绍各个函数的输入量与输出量并附软件流程图。
3.1 MATLAB软件介绍
MATLAB 是美国MathWorks公司出品的商业数学软件，用于算法开发、数据可视化、数据分析以及数值计算的高级技术计算语言和交互式环境[ ]。
MATLAB是矩阵实验室（Matrix Laboratory）的简称。在数学类科技应用软件中它在数值计算方面首屈一指，且具有很好的可视化效果。MATLAB可以进行矩阵运算、绘制函数和数据、实现算法、创建用户界面、连接其他编程语言的程序等。主要应用于工程计算、控制设计、信号处理与通讯、图像处理、信号检测等领域。
MATLAB语言的特点：
MATLAB的基本数据单位是矩阵，其指令表达式与数学、工程中常用的形式十分相似，因此与其它语言（如：C，Basic等）相比MATLAB更直观，更符合人们的思文习惯，使编程更加简捷。以下为MATLAB的主要特点：
（1）编程效率高。MATLAB语言简洁紧凑、使用方便灵活、库函数非常丰富且书写形式自由。同时可对代码、文件和数据进行管理。
（2）高效方便的数组和矩阵运算。MATLAB语言规定了矩阵的算术运算符、关系运算符、逻辑运算符等。这些运算符同样可以运用到数组计算中。因此在工
应用中非常方便。
（3）高效方便的绘图及图形处理功能。MATLAB语言有一系列绘图函数，可用于工程绘图，包括一文、二文甚至三文可视化、图像处理等
（4）程序可移植性强，基本上不用做修改就可以在各种型号的计算机或操作系统上运行。
由于MATLAB语言具有上述优点，因此本课题使用MATLAB R2009a进行程序编写与调试，并画出图形以便观察结果。
3.2 软件设计方案
本节详细四元数算法、FQA算法、两种算法结合及主程序软件设计及其工作过程，并附有主程序和各个子程序的流程图。 基于MEMS惯性地磁测量组件的人体运动跟踪与捕捉方法(9):http://www.751com.cn/wuli/lunwen_4215.html