飞控姿态解算中,欧拉角与四元数之间的转换
Posted
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了飞控姿态解算中,欧拉角与四元数之间的转换相关的知识,希望对你有一定的参考价值。
//欧拉角转四元数
void cjx_from_euler(float roll, float pitch, float yaw)
{
float cr2 = cosf(roll*0.5f);
float cp2 = cosf(pitch*0.5f);
float cy2 = cosf(yaw*0.5f);
float sr2 = sinf(roll*0.5f);
float sp2 = sinf(pitch*0.5f);
float sy2 = sinf(yaw*0.5f);
q1 = cr2*cp2*cy2 + sr2*sp2*sy2;
q2 = sr2*cp2*cy2 - cr2*sp2*sy2;
q3 = cr2*sp2*cy2 + sr2*cp2*sy2;
q4 = cr2*cp2*sy2 - sr2*sp2*cy2;
}
#define RAD_TO_DEG 57.29577951f
//弧度转角度
float degrees(float rad)
{
return rad * RAD_TO_DEG;
}
//四元数转欧拉角
void cjx_to_euler(float *roll, float *pitch, float *yaw)
{
if (roll) {
*roll = degrees(atan2f(2.0f*(q1*q2 + q3*q4),1 - 2.0f*(q2*q2 + q3*q3)));
}
if (pitch) {
// 使用safe_asin()来处理pitch接近90/-90时的奇点
*pitch = degrees(safe_asin(2.0f*(q1*q3 - q2*q4)));
}
if (yaw) {
*yaw = degrees(atan2f(2.0f*(q2*q3 - q1*q4), 2.0f*(q1*q1 + q2*q2) - 1));
}
}
本文出自 “C小狼” 博客,请务必保留此出处http://caijinxun.blog.51cto.com/9101387/1872102
以上是关于飞控姿态解算中,欧拉角与四元数之间的转换的主要内容,如果未能解决你的问题,请参考以下文章