C语言实现惯导更新算法(机械编排)

四元数

四元数可用于描述刚体转动或姿态变换,与方向余弦阵相比,表达简洁。四元数是一个包含四个元素的列向量,列向量的第一个元素表示刚体围绕旋转轴转过的角度大小,其余3个元素代表了旋转轴的方向。
C语言实现惯导更新算法(机械编排)_第1张图片

姿态更新算法

本程序姿态的递推计算采用四元数来实现,姿态的变化采用等效旋转矢量表示
姿态更新时,利用k-1时刻的姿态四元数以及k-1至k时刻的两次陀螺仪角增量采样,计算方法如下:

在这里插入图片描述
程序中四元数的规范化在函数qmulrv()中进行,该函数作用为:利用旋转矢量更新姿态四元数。
C语言实现惯导更新算法(机械编排)_第2张图片
程序中双子样圆锥误差补偿在coningcom()函数中进行,该函数作用为:进行圆锥误差补偿,并输出补偿后旋转矢量。
C语言实现惯导更新算法(机械编排)_第3张图片

速度更新算法

你可能感兴趣的:(组合导航程序RTKLC代码集,c语言,算法,开发语言,组合导航,更新算法)