I am trying to extract YAW, PITCH and ROLL information from quaternions

196 Views Asked by At

I am trying to extract YAW, PITCH and ROLL information from quaternions using angular velocities. this method. This method seems to work if only one axis is non-zero at at a time. Example: when angvx= pi/2 and angvy & angvz are equal to 0, we get qroll = 1.570451 qpitch=0 and qyaw =0.

But when angvy, angvz & angvx are all equal to (M_PI/2). We get qyaw:-2.171378 qpitch:1.062277 qroll:-2.171378. Compared to the simple method which gives. qyaw:-2.171378 qpitch:1.062277 qroll:-2.171378.

How do I correctly extract YAW, PITCH and ROLL values from quaternions.

        #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
   roll+=dt*angvx;
   yaw+=dt*angvy;
   pitch+=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;
    
    v_x = angvx / omega;
    //normalized orientation of angular velocity vector
    v_y = angvy / omega;  
    v_z = angvz / omega;
    
   
    qw=cos(theta/2);
    qx= v_x * sin(theta/2);
    qy= v_y * sin(theta/2);
    qz= v_z * sin(theta/2);
    
   
    
    //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));
    
    
    return 0;
}