Welcome to OStack Knowledge Sharing Community for programmer and developer-Open, Learning and Share
Welcome To Ask or Share your Answers For Others

Categories

0 votes
408 views
in Technique[技术] by (71.8m points)

c - My quaternion method doesn't match my simple method for calculating yaw, pitch and roll

Objective: Using angular velocities to calculate YAW, PITCH and ROLL values using quaternions.

Problem: My quaternion method doesn't match my simple method for calculating YAW, PITCH and ROLL.

#define M_PI 3.14159265358979323846

//Angular Velocties in Radians
float angvx=(M_PI/2);
float angvy=0;
float angvz=0;        
// change in time
float dt=1;

int main()
{

    //"Simple" METHOD: yaw,pitch roll calc
    pitch=dt*angvx;
    roll=dt*angvy;
    yaw=dt*angvz;
    
    //Quaternion Method
    //ang velocity vector of all 3 
    omega = sqrt(pow(angvx,2)+pow(angvy,2)+pow(angvz,2)); 
    //length of angular velocity vector
    theta = omega*dt;
    
    printf("omega: %f
", omega);
    printf("theta: %f
", theta);
    
    v_x = angvx / omega;
    //normalized orientation of angular velocity vector
    v_y = angvy / omega;  
    v_z = angvz / omega;
    
    printf("v_x:%f v_y:%f v_z:%f
", v_x, v_y, v_z);
    
    
    qw=cos(theta/2);
    qx= v_x * sin(theta/2);
    qy= v_y * sin(theta/2);
    qz= v_z * sin(theta/2);
    
    printf("qw:%f qx:%f qy:%f qz:%f
", qw, qx, qy,qz);
    
    //formula from 
    //https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
    qyaw = atan2(2*qy*qw-2*qx*qz , 1 - 2*pow(qy,2) - 2*pow(qz,2));
    qpitch = asin(2*qx*qy + 2*qz*qw);
    qroll = atan2(2*qx*qw-2*qy*qz , 1 - 2*pow(qx,2) - 2*pow(qz,2));
    
    
    printf("qyaw:%f qpitch:%f qroll:%f
", qyaw, qpitch,qroll);
    printf("yaw:%f pitch:%f roll:%f
", yaw, pitch, roll);
    
    return 0;
}

When angvx = M_PI (pi) Example 1 Simple method yields pitch = 3.141593. Quaternion yields qroll:-3.141593

When angvz=M_PI (pi) Example 2 Simple method yields yaw= 3.141593. Quaternion yields **qroll:-3.141593 qyaw:3.141593 **

I understand that in qroll calculation qx is the only non-zero term in Example 1. I understand that in qroll & qyaw calculation qz is the only non-zero term in Example2.

However these formulas seem to have support from the community so I don't understand what I'm doing wrong?

Formula qroll, qyaw, qpitch sources:

https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/

Extracting Yaw from a Quaternion

Further Question

When I specify angvx, angvz, angvy at the same time the pitch, yaw & roll differ greatly from the simple method.

question from:https://stackoverflow.com/questions/65832606/my-quaternion-method-doesnt-match-my-simple-method-for-calculating-yaw-pitch-a

与恶龙缠斗过久,自身亦成为恶龙;凝视深渊过久,深渊将回以凝视…
Welcome To Ask or Share your Answers For Others

1 Answer

0 votes
by (71.8m points)
Waitting for answers

与恶龙缠斗过久,自身亦成为恶龙;凝视深渊过久,深渊将回以凝视…
Welcome to OStack Knowledge Sharing Community for programmer and developer-Open, Learning and Share
Click Here to Ask a Question

...