基于MEMS惯性地磁测量组件的人体运动跟踪与捕捉方法(6)
(2-23)
—地理坐标系相对于地球坐标系的转动角速率在地理系上的投影
(2-24)
式中 —载体高度。人体站在原地时肢体的运动姿态解算中人体运动速度与位置可设为常数。
4.四元数与方向余弦的关系
将三文空间矢量扩展成四文，则地理坐标系的三文矢量 和载体坐标系的三文矢量 表示为
（2-25）
可以证明，存在一个四元数 ，使
（2-26）
式中， ，为 的共轭四元数。
写成矩阵形式为
（2-27）
将上式展开后，得
（2-28）
上式确定了四元数和姿态转换矩阵的关系，下面再确定四元数与姿态角之间的关系。根据航向角、俯仰角、橫滚角的转动顺序，从地理系到载体系的变换四元数为
（2-29）
5.四元数运动学微分方程
设矢量 为四元数形式，其与 对应的四元数 有如下微分关系
（2-30）
矩阵矢量形式为
（2-31）
四元数微分方程的求解，类似矩阵微分方程，可用毕卡逼近法求解，得解析式表达式
（2-32）
在对指数积分取近似的情况下，定义
（2-33）
将 代入（2-32）式，可得四元数的解析式
（2-34）
其中 （2-35）
6.等效转动矢量修正法
四元数算法的解析表达式(2-34)用到了角速度矢量的积分，即
（2-36）
根据刚体转动特性，当刚体运动不是定轴转动时，即矢量 的方向在空间变化时，上式是不成立的。因此，当采用角速度矢量积分时，计算产生了误差，称做转动不可交换性误差。只有当积分区间很小时，近似认为角速度矢量的方向不变，上式才近似成立。
根据1971年，Bortz提出的等效转动矢量概念，转动矢量表示为
T (2-37)
它可以唯一确定刚体在t时刻的运动姿态，即以 为轴，转动角度大小等于旋转矢量 的幅值 。当以旋转矢量描述机体运动姿态时，旋转矢量微分方程可以表示为
（2-38）
式中， 是载体角速度矢量。
考虑旋转矢量很小，对旋转矢量微分方程取二阶近似，则上式可近似为
略去高次项，则可进一步简化为
2.2.4 四元数姿态算法中初始姿态的确定
初始对准的主要主要目的是精确确定姿态矩阵 和四元数算法的初始姿态。在载体静止条件下， 和 均为已知，且有 。而在不考虑惯性传感器误差的情况下，陀螺仪和加速度计的输出为
定义一个新的矢量 ，即有
有矩阵的相似变化可得 （2-45）