Quaternion rotation ignoring yaw

Question

I'am working with Quaternion and one LSM6DSO32 captor gyro + accel. So I fused datas coming from my captor and after that I have a Quaternion, everything works well.

Now I'd like to detect if my Quaternion has rotated more than 90° about a initial quaternion, here is what I do, first I have q1 is my initial quaternion, q2 is the Quaternion coming from my fusion data, to detect if q2 has rotated more than 90° from q1 I do :

q_conj = conjugateQuaternion(q2);
q_mulitply = multiplyQuaternion(q1, q_conj);

float angle = (2 * acos(q_mulitply.element.w)) * RAD_TO_DEG;

if(angle > 90.0f)
   do something

this is works very well I can detect if q2 has rotated more than 90°. But my "problem" is I also detect 90° rotation in yaw, and I don't want integrate yaw in my test. Is it possible to nullify yaw (z component in my quaternion) without modify w, x and y component ?

My final objective is to detect a rotation more than 90° but without caring yaw, and I don't want to use Euler angle because I want avoid Gimbal lock

Edit : I want to calculate the magnitude between q1and q2 and don't care about yaw

Answer 1

The "yaw" of a quaternion generally means q_yaw in a quaternion formed by q_roll * q_pitch * q_yaw. So that quaternion without its yaw would be q_roll * q_pitch. If you have the pitch and roll values at hand, the easiest thing to do is just to reconstruct the quaternion while ignoring q_yaw.

However, if we are really dealing with a completely arbitrary quaternion, we'll have to get from q_roll * q_pitch * q_yaw to q_roll * q_pitch.

We can do it by appending the opposite transformation at the end of the equation: q_roll * q_pitch * q_yaw * conj(q_yaw). q_yaw * conj(q_yaw) is guaranteed to be the identity quaternion as long as we are only dealing with normalized quaternions. And since we are dealing with rotations, that's a safe-enough assumption.

In other words, removing the "Yaw" of a quaternion would involve:

1. Find the yaw of the quaternion
2. Multiply the quaternion by the conjugate of that.

So we need to find the yaw of the quaternion, which is how much the forward vector is rotated around the up axis by that quaternion.

The simplest way to do that is to just try it out, and measure the result:

1. Transform a reference forward vector (on the ground plane) by the quaternion
2. Take that and project it back on the ground plane.
3. Get the angle between this projection and the reference vector.
4. Form a "Yaw" quaternion with that angle around the Up axis.

Putting all this together, and assuming you are using a Y=up system of coordinates, it would look roughly like this:

quat remove_yaw(quat q) {
  vec3 forward{0, 0, -1};
  vec3 up{0, 1, 0};

  vec3 transformed = q.rotate(forward);

  vec3 projected = transformed.project_on_plane(up);
  if( length(projected) < epsilon ) {
    // TODO: unsolvable, what should happen here?
  }

  float theta = acos(dot(normalize(projected), forward));

  quat yaw_quat = quat.from_axis_angle(up, theta);

  return multiply(q, conjugate(yaw_quat));
}

This can be simplified a bit, obviously. For example, the conjugate of a axis-angle quaternion is the same thing as a quaternion of the negative angle around the same axis, and I'm sure there are other possible simplifications here. However, I wanted to illustrate the principle as clearly as possible.

There's also a singularity when the pitch is exactly ±90°. In these cases the yaw is gimbal-locked into being indistinguishable from roll, so you'll have to figure out what you want to do when length(projected) < epsilon.