窗体顶端捷联系统的四元数法姿态算法算法输入物体的初始姿态3.docVIP

发新话题 发布投票 发布商品 发布悬赏 发布活动 发布辩论 发布视频 捷联系统的四元数法姿态算法算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；算法输出：物体的当前姿态。具体算法：1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A5.重复2～4部，即可连续更新姿态6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态核心算法1，欧拉角转四元数void Quaternion::FromEulerAngle(const EulerAngle ea){? ? ? ? float fCosHRoll = cos(ea.fRoll * .5f);? ? ? ? float fSinHRoll = sin(ea.fRoll * .5f);? ? ? ? float fCosHPitch = cos(ea.fPitch * .5f);? ? ? ?