quaternion

namespace saltro

namespace math

Typedefs

using Vec3 = Eigen::Vector3d

using Vec4 = Eigen::Vector4d

using Mat33 = Eigen::Matrix3d

using Mat43 = Eigen::Matrix<double, 4, 3>

using Mat44 = Eigen::Matrix<double, 4, 4>

Functions

Vec4 normalizeQuat(const Vec4 &q)

Normalize a quaternion to unit length.

Computes the normalized quaternion:

\[ \hat{\mathbf{q}} = \frac{\mathbf{q}}{\|\mathbf{q}\|} \]

Parameters:

q – Input quaternion (may be unnormalized).

Returns:

Normalized unit quaternion.

Mat33 rotationMatrix(const Vec4 &q)

Compute the rotation matrix (DCM) from a unit quaternion.

Converts a unit quaternion \(\mathbf{q} = [q_0, q_1, q_2, q_3]^T\) to a 3×3 direction cosine matrix (rotation matrix):

\[ \mathbf{C}(\mathbf{q}) = \mathbf{I} + 2q_0[\mathbf{q}_{1:3}]_\times + 2[\mathbf{q}_{1:3}]_\times^2 \]

where \([\cdot]_\times\) denotes the skew-symmetric cross-product matrix.

Parameters:

q – Unit quaternion (4D vector).

Returns:

3×3 rotation matrix.

Vec4 quatConj(const Vec4 &q)

Compute the quaternion conjugate.

For a unit quaternion, the conjugate equals the inverse:

\[ \mathbf{q}^* = [q_0, -q_1, -q_2, -q_3]^T \]

Parameters:

q – Input quaternion.

Returns:

Conjugate quaternion.

Vec4 quatMult(const Vec4 &q1, const Vec4 &q2)

Hamilton product of two quaternions.

Computes \(\mathbf{q}_1 \otimes \mathbf{q}_2\) using Hamilton’s convention, scalar-first ( \(q = [q_0, \mathbf{q}_v]^T\)). Equivalent to the left-multiplication matrix form \(L(\mathbf{q}_1) \mathbf{q}_2\) where \(L(\mathbf{q}_1) = [\mathbf{q}_1 \mid W(\mathbf{q}_1)]\) (4×4). Implemented via findWMat for the non-scalar columns.

Parameters:

• q1 – Left-hand quaternion.

• q2 – Right-hand quaternion.

Returns:

Product \(\mathbf{q}_1 \otimes \mathbf{q}_2\) (4-vector).

double quatAngle(const Vec4 &q_err)

Full SO(3) rotation angle from an error quaternion.

Given an error quaternion \(\mathbf{q}_e = \mathbf{q}_a^{-1} \otimes \mathbf{q}_b\), returns the full rotation angle in SO(3):

\[ \theta = 2 \arccos(|q_{e,0}|) \]

The absolute value handles the double cover so q and -q give the same angle.

NOTE: the attitude cost functions in Satellite::stageCost inline \(\arccos(|q_{goal} \cdot q|)\) — the half-angle — because their analytic gradient needs the raw dot product. This helper returns the full rotation angle, which is the standard convention for user-facing pointing error.

Parameters:

q_err – Error quaternion.

Returns:

Full rotation angle in radians, in [0, π].

Mat43 findWMat(const Vec4 &q)

Compute the W matrix for quaternion kinematics.

Returns the 4×3 matrix that relates angular velocity to quaternion rate:

\[ \dot{\mathbf{q}} = \frac{1}{2}\mathbf{W}(\mathbf{q})\boldsymbol{\omega} \]

where \(\boldsymbol{\omega}\) is the angular velocity in rad/s.

Parameters:

q – Unit quaternion.

Returns:

4×3 W matrix.

Mat43 quatNormJacobian(const Vec4 &q)

Compute the Jacobian of quaternion normalization.

Returns the 4×4 Jacobian matrix of the normalization operator:

\[ \mathbf{J}_{\text{norm}} = \frac{\partial \hat{\mathbf{q}}}{\partial \mathbf{q}} \]

Useful for error propagation when quaternions are normalized.

Parameters:

q – Input quaternion (typically unnormalized).

Returns:

4×4 Jacobian matrix.

Mat33 skewSymmetric(const Vec3 &v)

Compute the skew-symmetric (cross-product) matrix of a vector.

Given a vector \(\mathbf{v} = [v_1, v_2, v_3]^T\), returns the skew-symmetric matrix:

\[\begin{split} [\mathbf{v}]_\times = \begin{bmatrix} 0 & -v_3 & v_2 \\ v_3 & 0 & -v_1 \\ -v_2 & v_1 & 0 \end{bmatrix} \end{split}\]

Used for cross-product computation: \(\mathbf{v} \times \mathbf{w} = [\mathbf{v}]_\times \mathbf{w}\).

Parameters:

v – Input 3D vector.

Returns:

3×3 skew-symmetric matrix.

Mat43 drotmatTvecdq(const Vec4 &q, const Vec3 &v)

Compute the derivative of rotated vector with respect to quaternion.

Returns the 4×3 Jacobian of the rotated vector \(\mathbf{C}(\mathbf{q})\mathbf{v}\) with respect to the quaternion \(\mathbf{q}\):

\[ \frac{\partial \mathbf{C}(\mathbf{q})\mathbf{v}}{\partial \mathbf{q}} \]

Useful for computing derivatives in attitude dynamics and control.

Parameters:

• q – Unit quaternion.

• v – Fixed vector to be rotated.

Returns:

4×3 Jacobian matrix.

std::array<Mat44, 3> ddrotmatTvecdqdq(const Vec4 &q, const Vec3 &v)

Compute the second derivative of rotated vector (Hessian).

Returns an array of three 4×4 matrices representing the Hessian tensors for the derivative of \(\mathbf{C}(\mathbf{q})\mathbf{v}\) with respect to \(\mathbf{q}\):

\[ \frac{\partial^2 [\mathbf{C}(\mathbf{q})\mathbf{v}]_i}{\partial \mathbf{q} \partial \mathbf{q}^T} \]

Index \(i = 0,1,2\) corresponds to the \(x,y,z\) component.

Parameters:

• q – Unit quaternion.

• v – Fixed vector to be rotated.

Returns:

Array of three 4×4 Hessian matrices (one per component).