# James Gregson's Website

## Quaternion Notes

These are some helpful quaternion identities, all summarized from Kok et al., Using Inertial Sensors for Position and Orientation Estimation although I have put the scalar component last.

A quaternion $$q$$ is represented as a 4-tuple, or an imaginary vector portion $$q_v=[q_x,q_y,q_z]^T$$ and a scalar value $$q_s$$ giving $$q = [q_v^T, q_s]^T$$. The norm of a quaternion is $$\|q\|_2 = (q_v^T q_v + q_s^2)^{\frac{1}{2}}$$, the conjugate of a quaternion is $$q^* = [-q_v^T, q_s]^T$$ and the inverse of a quaternion is $$q^{-1} = \frac{q^*}{\|q\|_2^2}$$.

Unit quaternions have $$\|q\|_2 = 1$$, in which case $$q^* = q^{-1}$$. Unit quaternions also represent spatial rotations/orientations of $$\theta$$ radians around a unit axis $$a$$ via $$q = [a^T \sin\left(\frac{\theta}{2}\right), \cos\left(\frac{\theta}{2}\right) ]^T$$. Both $$q$$ and $$-q = [-q_v, -q_s]^T$$ represent the same orientation, although the one with positive scalar component takes the shortest rotation to achieve that orientation since $$\cos^{-1}(|x|) \leq \cos^{-1}(-|x|)$$.

The multiplication of two quaternions is $$p \otimes q = [ (p_s q_v + q_s p_v + p_v \times q_v)^T, p_s q_s - p_v \times q_v ]^T$$. The presence of the cross-product means that $$p\otimes q \neq q \otimes p$$. When $$p, q$$ are unit quaternions, $$p\otimes q$$ is equivalent to chaining the two rotations. The rotation matrix corresponding to a unit quaternion is:

\begin{equation*} R(q) = \begin{bmatrix} 1 - 2 q_y^2 - 2*q_z^2 & 2 q_x q_y - 2 q_z q_s & 2 q_x q_z + 2 q_y q_s \\ 2 q_x q_y + 2 q_z q_s & 1 - 2 q_x^2 - 2 q_z^2 & 2 q_y q_z - 2 q_x q_s \\ 2 q_x q_z - 2 q_y q_s & 2 q_y q_z + 2 q_x q_s & 1 - 2 q_x^2 - 2 q_y^2 \end{bmatrix} \end{equation*}

or in python:

def quat2mat( q: np.ndarray ) -> np.ndarray:
assert q.ndim == 1 and q.size == 4
qx,qy,qx,qs = q
return np.ndarray((
( 1 - 2*qy**2 - 2*qz**2,     2*qx*qy - 2*qz*qs,     2*qx*qz + 2*qy*qs ),
(     2*qx*qy + 2*qz*qs, 1 - 2*qx**2 - 2*qz**2,     2*qy*qz - 2*qx*qs ),
(     2*qx*qz - 2*qy*qs,     2*qy*qz + 2*qx*qs, 1 - 2*qx**2 - 2*qy**2 )
))


The, usually non-unit, quaternion representation of a vector $$v^\wedge = [ v^T, 0]^T$$. The vector can be rotated by the unit-quaternion $$q$$ via $$R(q) v = (q\otimes v^\wedge \otimes q^*)_v$$ where only the vector portion is retained.

The quaternion-quaternion product can be represented by matrix multiplication in either the same, or reversed order, i.e.:

\begin{align*} \newcommand{\pd}[2]{\frac{\partial #1}{\partial #2}} p \otimes q &= p^L q = q^R p \hspace{1cm} & \mbox{where} \\ p^L &= \begin{bmatrix} p_s I + \lfloor p_v \rfloor_\times & p_v \\ -p_v^T & p_s \end{bmatrix} & \\ q^R &= \begin{bmatrix} q_s I - \lfloor q_v \rfloor_\times & q_v \\ -q_v^T & q_s \end{bmatrix} & \end{align*}

where $$\lfloor m \rfloor_\times$$ is the matrix representation of the cross-product. This is extremely useful when differentiating quaternion expressions since $$p^L = \pd{}{q}(p\otimes q)$$ and $$q^R = \pd{}{p}(p\otimes q)$$.

A Rodrigues rotation vector $$\eta = \theta a$$ where $$\|a\|_2 = 1$$ can be mapped to a quaternion via the exponential map:

\begin{align*} \exp_q(\eta) =& q = \begin{pmatrix} a \sin\left(\frac{\theta}{2}\right) \\ \cos\left(\frac{\theta}{2}\right) \end{pmatrix} \\ \log_q(q) =& \eta = 2 q_v \end{align*}

Under the assumption that $$\theta << 1$$, the following approximations can be made:

\begin{align*} \exp_q(\eta) \approx& \begin{pmatrix} \frac{\eta}{2} \\ 1 \end{pmatrix} \\ \pd{\exp_q(\eta)}{\eta} \approx& \begin{pmatrix} \frac{1}{2} I_{3\times3} \\ 0 \end{pmatrix} \\ \pd{\log_q(q)}{q} \approx& \begin{pmatrix} 2 I_{3\times3} & 0_{3\times1} \end{pmatrix} \end{align*}

The above are also very helpful in manifold optimization over quaternion states where the Newton update is generally small which results in linear Jacobians with respect to the update variables.