四元数 四元数可用于描述刚体转动或姿态变换,与方向余弦阵相比,表达简洁。四元数是一个包含四个元素的列向量,列向量的第一个元素表示刚体围绕旋转轴转过的角度大小,其余3个元素代表了旋转轴的方向。 姿态更新算法 本程序姿态的递推计算采用四元数来实现,姿态的变化采用等效旋转矢量表示 姿态更新时,利用k-1时刻的姿态四元数以及k-1至k时刻的两次陀螺仪角增量采样,计算方法如下: 程序中四元数的规范化在函数qmulrv()中进行,该函数作用为:利用旋转矢量更新姿态四元数。 程序中双子样圆锥误差补偿在coningcom()函数中进行,该函数作用为:进行圆锥误差补偿,并输出补偿后旋转矢量。 速度更新算法