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;
}