[Math] Converting from quaternion to angular velocity then back to quaternion

change-of-basisintegrationquaternions

I am trying to write a small physics simulator. I have q(t), the quaternion orientation in the inertial frame.

I would like to simulate the gyroscope input, so I need to get the moving frame angular velocity.

My first question is, how to calculate the temporal derivative of a quaternion by sampling q(t).

I thought about two solution $(q(t+dt) – q(t))/dt$ and $q(t+dt)*q(t)^{t}$ where the superscript t is for the conjugate. I found the second one by thinking that it would give me the change of $q(t)$ during $dt$ in the inertial reference.

The first one seems weird to me for a rotation. I am not sure if they are both correct or equivalent. If not, why ?

My second question is, assuming I have the correct $dq(t)/dt$, How do I get:

  • the body frame angular velocity (to simulate gyroscope input) $\omega_B$
  • And then from the angular velocity, how do I retrieve the body frame quaternion temporal derivative. $dq_B(t)/dt$
  • Then how to integrate that temporal derivative to get the new attitude $q_B(t+dt)$ with respect to the body frame
  • I assume that from there, I could retrieve $q(t+dt)$ by composing with the rotation $q(t)$: $q(t+dt) = q_B(t+dt)*q(t)$ ? Is that correct ?

I have made some attempt by using https://www.astro.rug.nl/software/kapteyn/_downloads/attitude.pdf . But I was very unsuccessful so far http://paste.awesom.eu/lzKh

Thank you in advance for your help

Best Answer

You have a curve $q\colon I \to \mathbb S^3$, where $I$ is a time interval and $\mathbb S^3\subseteq\mathbb R^4$ is the set of all unit quaternions. Here, we represent quaternions as 4-dimensional vectors. Note that if a quaternion should encode an orientation or rotation, it has to be a unit quaternion.

Since $q(t)\in\mathbb S^3\subseteq \mathbb R^4$ lives in the linear space $\mathbb R^4$ you can calculate its time derivative $\dot q(t) = \tfrac d{dt}q(t)$ by \begin{align}\tag{1} \dot q(t) = \tfrac d{dt}q(t) = \lim_{h\to0} \frac{q(t+h) - q(t)}h, \end{align} meaning that $\bigl(q(t+\delta)-q(t)\bigr)/\delta$ is an approximation to $\dot q(t)$ for small $\delta$. We can see that this approximation lives somewhere in $\mathbb R^4$. More specifically, it is an element of $T_{q(t)}\mathbb S^3$, the tangent space of the sphere at the element $q(t)\in\mathbb S^3$, because $q(\tau)\in\mathbb S^3$ for all $\tau\in I$.

Since $\mathbb S^3$ is a Lie group it is favourable to represent the velocity of $q(t)$ by a vector $\Omega(t)\in\mathbb R^3$ that fulfills $$\dot q(t) = \frac12 q(t) * \begin{bmatrix}0\\\Omega(t)\end{bmatrix}.$$ This can be thought of as mapping the tangent space $T_{q(t)}\mathbb S^3$ to the tangent space $T_e\mathbb S^3 = \{[0,x^T]^T\in\mathbb R^4\}$ with the neutral element $e=[1,0,0,0]^T$. The $\Omega(t)$ is actually the angular velocity in the body frame. We can calculate $\Omega(t)$ from $\dot q(t)$ by $$\tag{2} \Omega(t) = \operatorname{Im}\bigl(2 \overline{q(t)}*\dot q(t)\bigr), $$ where the overline represents quaternionic conjugation (which is actually the inversion on $\mathbb S^3$) and $\operatorname{Im}$ extracts the imaginary part, hence just drops the first component (which has to be zero here). Now we can put the limit expression for $\dot q(t)$ in here an get \begin{align*} \Omega(t) &= \operatorname{Im}\bigl(2 \overline{q(t)}*\dot q(t)\bigr)\\ &= \operatorname{Im}\left(2 \overline{q(t)}*\lim_{h\to0} \frac{q(t+h) - q(t)}h\right) \\ &= \operatorname{Im}\left(2 \lim_{h\to0} \frac{\overline{q(t)}*q(t+h) - \overline{q(t)}*q(t)}h\right) \\ &= \operatorname{Im}\left(2 \lim_{h\to0} \frac{\overline{q(t)}*q(t+h) - e}h\right) \\ &= \lim_{h\to0} 2\operatorname{Im}\frac{\overline{q(t)}*q(t+h)}h, \end{align*} since $\operatorname{Im}e = [0,0,0]^T$. This means that $2\operatorname{Im}\overline{q(t)}*q(t+\delta)/\delta$ is an approximation to $\Omega(t)$, the body frame angular velocity, for small $\delta$.

With equations (1) and (2) from above, you can transform the derivative $\dot q(t) = \tfrac d{dt}q(t)$ into the body frame angular velocity $\Omega(t)$ and back.

Note that $2\operatorname{Im}q(t+\delta)*\overline{q(t)}/\delta \approx \omega(t)$ gives an approximation to the angular velocity with respect to the inertial frame. It simply holds $\omega(t)=\Omega(t)^{q(t)}$, where $x^{q(t)}\in\mathbb R^3$ is the application of the rotation $q(t)$ to $x$ and is defined by $$ \begin{bmatrix}0\\ x^{q(t)}\end{bmatrix} = q(t)*\begin{bmatrix}0\\ x\end{bmatrix}*\overline{q(t)}. $$

The simplest way to numerically integrate the attitude is to use a forward Lie-group Euler method. If you somehow calculate $\dot q(t)$ or $\Omega(t)$ you can get the new attitude $q(t+\delta)$ by $$ q(t+\delta) = q(t)*\widetilde{\exp}(\delta\cdot \Omega(t)), $$ where $\widetilde{\exp}\colon\mathbb R^3\to\mathbb S^3$ is essentially a Lie group exponential function which is defined by a power series, but can, for unit quaternions, be written in closed form as $$ \widetilde{\exp}(v) = \cos(\tfrac12 \|v\|) + \frac{v}{\|v\|}\sin(\tfrac12 \|v\|) $$ for $v\in\mathbb R^3$, where $\|\bullet\|$ is the standard Euclidean norm.

If you're interested in more sophisticated Lie group time integration, you can check out one of my papers [Arnold, Hante 2016] or a nice paper (without quaternions though) of my supervisor [Arnold, Cardona, Brüls 2016] or the preliminary version.

Hope I could clear things up a little.