crazyflie2.0中欧拉角和四元数之间转换的问题
2015-12-27 15:34
375 查看
crazyflie2.0中从四元数获取欧拉角的程序中,看到如下程序:
void sensfusion6GetEulerRPY(float* roll, float* pitch, float* yaw)
{
float gx, gy, gz; // estimated gravity direction
gx = 2 * (q1*q3 - q0*q2);
gy = 2 * (q0*q1 + q2*q3);
gz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
if (gx>1) gx=1;
if (gx<-1) gx=-1;
*yaw = atan2f(2*(q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180 / M_PI_F;
*pitch = asinf(gx) * 180 / M_PI_F; //Pitch seems to be inverted
*roll = atan2f(gy, gz) * 180 / M_PI_F;
}
但是在看邓正隆的惯性技术这本书中,计算出来的应该是如下公式,其中Z轴Yaw偏航,X轴Pitch俯仰,Y轴Roll翻滚;使用如下公式计算之后,从crazyflie PC client上看到X轴和Y轴交换了,很奇怪,不知道为什么?先记录下结果
void sensfusion6GetEulerRPY(float* roll, float* pitch, float* yaw)
{
float gx, gy, gz; // estimated gravity direction
gx = 2 * (q1*q3 - q0*q2);
gy = 2 * (q0*q1 + q2*q3);
gz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
if (gx>1) gx=1;
if (gx<-1) gx=-1;
*yaw = atan2f(2*(q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180 / M_PI_F;
*pitch = asinf(gx) * 180 / M_PI_F; //Pitch seems to be inverted
*roll = atan2f(gy, gz) * 180 / M_PI_F;
}
但是在看邓正隆的惯性技术这本书中,计算出来的应该是如下公式,其中Z轴Yaw偏航,X轴Pitch俯仰,Y轴Roll翻滚;使用如下公式计算之后,从crazyflie PC client上看到X轴和Y轴交换了,很奇怪,不知道为什么?先记录下结果
void sensfusion6GetEulerRPYNormal(float* roll, float* pitch, float* yaw) { *yaw = -atan2f(2*(-q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180 / M_PI_F; *pitch = asinf(2*q2*q3 + 2*q0*q1) * 180 / M_PI_F; //Pitch seems to be inverted *roll = -atan2f((2*q1*q3-2*q0*q2), q0*q0 + q3*q3 - q1*q1 - q2*q2) * 180 / M_PI_F; }
相关文章推荐
- Android之数据存储共享参数简单用户登录(一)
- springBoot jpa 多数据源的动态切换
- -----第一讲----第二节--------------什么是算法?-------------------------------------
- 批处理命令——choice
- uva227 - Puzzle
- 哈希表,HashTable
- 【Java并发编程实战】----- AQS(三):阻塞、唤醒:LockSupport
- thinkphp 跨模块调用
- 厦门Uber优步司机奖励政策(12月14日到12月20日)
- 补贴行动背后,火炬租房怎样实现“去中介化”?
- JavaHL(JNI) Not Available
- jdbc中java与mysql数据类型的映射
- C#中的internal关键字
- jQuery获取循环中的选中单选按钮radio的值
- C#使用StreamWriter类写入文件文件
- TableView编辑
- 解读Unity中的CG编写Shader系列1——初识CG
- LeetCode-18-4Sum(证明/KSum)-Medium
- 插入排序
- Android Studio开发文档doc显示问题