Transforming a quaternion between two coordinate systems

248 Views Asked by At

Background

I am trying to convert an orientation quaternion from a head-mounted IMU into a quaternion which axes would correspond to the pitch/yaw/roll of said head. Since I am a quaternion novice, I will try to describe my process very clearly.

Initially, the axes of the IMU quaternion, $\textbf{Q}_I$, are related to the sensor itself – which could be situated arbitrarily on the head.

In order to convert $\textbf{Q}_I$ to a forward-facing orientation in software, I first produce a calibration quaternion, $ \textbf{Q}_C $, by taking $$ \textbf{Q}_C = \textbf{Q}_I^{'-1}$$ where $ \textbf{Q}_I^{'} $ denotes $ \textbf{Q}_I $ when the head is looking directly forward.

I then produce a software quaternion $\textbf{Q}_S$ via

$$ \textbf{Q}_S = \textbf{Q}_I \textbf{Q}_C$$

A $\textbf{Q}_S^{'}$ (looking straight ahead) would thus be $\textbf{Q}_S^{'} = \{1,0,0,0\}$.

If the IMU placement on the head is perfectly parallell, or orthogonal to IMU's own axes, this $\textbf{Q}_S$ is perfectly usable (with some negating of $a$, $b$, $c$, and/or $d$ of $\textbf{Q}_S$, depending on how it is mounted). However, I cannot assume that that condition will hold.

So, working from an arbitrary placement, what $\textbf{Q}_S$ does provide is a perfect yaw axis: when converting its $b$, $c$ and $d$-component to a normalized vector, $\textbf{V}_y$; and strictly rotating around the head's up-axis, the $z$-component of $\textbf{V}_y ≈ 1$. I am not entirely certain if this follows by necessity, or if this comes from how $\textbf{Q}_I$ is set up; in any case, it is very useful as I now only should need to determine one other axis, either pitch or roll, and then take the cross product of it and $\textbf{V}_y$.

I do exactly this by recording a quaternion when strictly pitching the head forward, $\textbf{Q}_{p1}$, and one backwards $\textbf{Q}_{p2}$; and then extracting the pitch axis, $\textbf{V}_{p}$, by averaging each of the absolute values of $b$, $c$ and $d$-components from $\textbf{Q}_{p1}$ and $\textbf{Q}_{p2}$, while preserving the sign from $\textbf{Q}_{p1}$.

Lastly I compute the roll axis, $\textbf{V}_{r}$, by

$$ \textbf{V}_{r} = \textbf{V}_{p} \times \textbf{V}_{y} $$

I am able to verify that these axes are correct by finding which one of them has the shortest absolute distance to $\textbf{Q}_S$ when strictly pitching, rolling, or yawing.

Question

So to my question (thank you for bearing with me), how do I transform $\textbf{Q}_S$ into a quaternion, $\textbf{Q}_{pry}$, with coordinate axes corresponding to $\textbf{V}_{p}$, $\textbf{V}_{r}$, and $\textbf{V}_{y}$? i.e. if converting the $b$, $c$ and $d$-components of $\textbf{Q}_{pry}$ to a normalized vector, $\textbf{V}_{pry}$, strictly pitching should yield an $x$-component of $\textbf{V}_{pry} ≈ 1$; strictly rolling an $y$-component $≈1$; and strictly yawing a $z$-component $≈1$.

Any help is much appreciated!

Attempt

With inspiration from the answer of this question, I tried to build a rotation matrix with each column being $\textbf{V}_{p}$, $\textbf{V}_{r}$, and $\textbf{V}_{y}$, followed by converting it to a quaternion, $\textbf{Q}_{T}$, (I am using the GLM library for all matrix and quaternion maths) – thinking that I could retry the logic from $\textbf{Q}_{S}$ and $\textbf{Q}_{C}$ above, I attempt to produce another calibration quaternion, $\textbf{Q}_{C2}$:

$$ \textbf{Q}_T = \textbf{Q}_S^{'} \textbf{Q}_{C2} $$ $$ \textbf{Q}_S^{'-1}\textbf{Q}_T = \textbf{Q}_{C2} $$ $$ \textbf{Q}_{pry} = \textbf{Q}_S\textbf{Q}_{C2} $$

However, as noted above $ \textbf{Q}_S^{'-1} = \{1, 0, 0, 0 \} $, which leads to

$$ \textbf{Q}_T = \textbf{Q}_{C2} $$ and $$ \textbf{Q}_{pry} = \textbf{Q}_S\textbf{Q}_{T} $$

Yet, such a $\textbf{Q}_{pry}$ does not produce the desired result – strictly yawing is still around the head up-axis, but neither roll or pitch align with head front/back or left/right, respectively. (I have attempted this with all orders of $\textbf{V}_{p}$, $\textbf{V}_{r}$, and $\textbf{V}_{y}$ when constructing $\textbf{Q}_{T}$, only those where $\textbf{V}_{y}$ is placed in the third column produce the correct yaw axis).