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