ADCS.helpers.math_helpers module

ADCS.helpers.math_helpers.Wmat(q)

Compute the quaternion kinematic matrix used in first-order quaternion propagation.

The quaternion derivative is expressed as

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

where \(\boldsymbol{\omega}\) is the angular velocity expressed in the body frame.

The matrix \(\mathbf{W}(\mathbf{q})\) is defined as

\[\begin{split}\mathbf{W}(\mathbf{q}) = \begin{bmatrix} -\mathbf{q}_v^\mathsf{T} \\ q_0\mathbf{I}_3 + [\mathbf{q}_v]_\times \end{bmatrix}\end{split}\]

with \([\cdot]_\times\) denoting the skew-symmetric operator.

Parameters:

q (numpy.ndarray) – Quaternion in Hamilton convention.

Returns:

Quaternion kinematic matrix.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.cayley_to_quat(cly)

Convert Cayley parameters to a quaternion.

Cayley parameters \(\mathbf{p}\) are related to the quaternion by

\[\begin{split}\mathbf{q} = \frac{1}{\sqrt{1+\|\mathbf{p}\|^2}} \begin{bmatrix} 1 \\ \mathbf{p} \end{bmatrix}\end{split}\]

This representation avoids trigonometric functions but is singular at rotations of \(\pm\pi\).

Parameters:

cly (numpy.ndarray) – Cayley parameter vector.

Returns:

Quaternion corresponding to the Cayley parameters.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.dcm_to_quat(R)

Convert a direction cosine matrix to a quaternion using a numerically stable algorithm.

The method evaluates the matrix trace

\[\mathrm{tr}(\mathbf{R}) = R_{11} + R_{22} + R_{33}\]

and selects the computation branch that maximizes numerical stability (Sheppard’s method).

The resulting quaternion follows the Hamilton convention with scalar-first ordering.

Parameters:

R (numpy.ndarray) – Direction cosine matrix.

Returns:

Unit quaternion corresponding to the rotation matrix.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.ddrotmatTvecdqdq(q, v)

Compute the Hessian of a rotated vector with respect to the quaternion.

This function evaluates the second derivative tensor

\[\frac{\partial^2}{\partial \mathbf{q}^2} \left( \mathbf{R}^\mathsf{T}(\mathbf{q})\,\mathbf{v} \right)\]

yielding a rank-3 tensor with dimensions \(4 \times 4 \times 3\).

This quantity is required in second-order optimization, uncertainty propagation, and higher-order filters.

Parameters:

• q (numpy.ndarray) – Quaternion.

• v (numpy.ndarray) – Vector to be rotated.

Returns:

Hessian tensor of the rotated vector with respect to the quaternion.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.drotmatTvecdq(q, v)

Compute the Jacobian of a rotated vector with respect to the quaternion.

This function evaluates

\[\frac{\partial}{\partial \mathbf{q}} \left( \mathbf{R}^\mathsf{T}(\mathbf{q})\,\mathbf{v} \right)\]

where \(\mathbf{R}(\mathbf{q})\) is the DCM associated with the quaternion.

The closed-form expression is

\[\begin{split}2 \begin{bmatrix} q_0\mathbf{v} - \mathbf{q}_v \times \mathbf{v} \\ (\mathbf{q}_v \cdot \mathbf{v})\mathbf{I} - \mathbf{q}_v\mathbf{v}^\mathsf{T} + \mathbf{v}\mathbf{q}_v^\mathsf{T} - q_0[\mathbf{v}]_\times \end{bmatrix}\end{split}\]

This Jacobian is frequently used in attitude estimation and disturbance torque linearization.

Parameters:

• q (numpy.ndarray) – Quaternion in Hamilton form.

• v (numpy.ndarray) – Vector expressed in the inertial frame.

Returns:

Jacobian of the rotated vector with respect to the quaternion.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.limit(u, umax)

Apply element-wise saturation limits to a control vector.

The saturation rule depends on the type of umax:

umax type	Behavior
scalar	symmetric limit [-umax, umax]
array	symmetric element-wise limits
(umin, umax)	asymmetric limits

This function is typically used for actuator saturation modeling.

Parameters:

• u (array-like) – Input vector or scalar.

• umax (scalar, array-like, or tuple) – Saturation specification.

Returns:

Saturated output.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.matrix_row_norm(m)

Compute the Euclidean norm of each row of a matrix.

The output vector satisfies

\[n_i = \|\mathbf{m}_i\|\]

for each row \(\mathbf{m}_i\).

Parameters:

m (numpy.ndarray) – Two-dimensional input matrix.

Returns:

Vector of row norms.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.matrix_row_normalize(m)

Normalize each row of a matrix to unit Euclidean norm.

Each row \(\mathbf{m}_i\) is transformed as

\[\hat{\mathbf{m}}_i = \frac{\mathbf{m}_i}{\|\mathbf{m}_i\|}\]

This operation is commonly used for normalizing vector observations.

Parameters:

m (numpy.ndarray) – Input matrix.

Returns:

Row-normalized matrix.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.mrp_to_quat(mrp)

Convert Modified Rodrigues Parameters (MRP) to a quaternion.

The mapping is defined using the rotation angle

\[\theta = 4\arctan\left(\frac{\|\boldsymbol{\sigma}\|}{2}\right)\]

and unit rotation axis

\[\hat{\mathbf{u}} = \frac{\boldsymbol{\sigma}}{\|\boldsymbol{\sigma}\|}\]

yielding the quaternion

\[\begin{split}\mathbf{q} = \begin{bmatrix} \cos(\theta/2) \\ \hat{\mathbf{u}}\sin(\theta/2) \end{bmatrix}\end{split}\]

This formulation follows the standard aerospace definition (see NASA TR-19960035754).

Parameters:

mrp (numpy.ndarray) – Modified Rodrigues Parameters.

Returns:

Equivalent quaternion.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.njit(*args, **kws)

Equivalent to jit(nopython=True)

See documentation for jit function/decorator for full description.

ADCS.helpers.math_helpers.norm(v)

Compute the Euclidean norm of a vector.

Parameters:

v (numpy.ndarray) – Input vector.

Returns:

n – Euclidean norm (magnitude) of the vector.

Return type:

float

ADCS.helpers.math_helpers.normalize(v)

Normalize a vector to unit length.

Parameters:

v (ndarray) – Input vector \(\mathbf{v}\).

Returns:

Normalized vector \(\hat{\mathbf{v}} = \frac{\mathbf{v}}{\|\mathbf{v}\|}\). If \(\|\mathbf{v}\|=0\), the zero vector is returned unchanged.

Return type:

ndarray

ADCS.helpers.math_helpers.normed_vec_hess(v, dv=None, ddv=None)

Compute the Hessian of the normalized vector \(\hat{\mathbf{v}}\).

The second derivative tensor satisfies

\[\frac{\partial^2 \hat{\mathbf{v}}}{\partial v_i\,\partial v_j} = -\frac{v_i\,\mathbf{I}+v_j\,\mathbf{I}}{\|\mathbf{v}\|^3} + 3\,\frac{\mathbf{v}v_i v_j}{\|\mathbf{v}\|^5},\]

which is symmetrized across indices. When external derivatives dv and ddv are provided, the chain rule is applied to yield the composed Hessian.

Parameters:

• v (ndarray) – Vector \(\mathbf{v}\).

• dv (ndarray, optional) – Jacobian of \(\mathbf{v}\) with respect to higher variables.

• ddv (ndarray, optional) – Hessian of \(\mathbf{v}\) with respect to higher variables.

Returns:

The Hessian tensor \(\partial^2 \hat{\mathbf{v}}/\partial \mathbf{v}^2\) or its propagated form using dv/ddv.

Return type:

ndarray

ADCS.helpers.math_helpers.normed_vec_jac(v, dv=None)

Compute the Jacobian of the normalized vector \(\hat{\mathbf{v}}\).

\[\frac{\partial \hat{\mathbf{v}}}{\partial \mathbf{v}} = \frac{1}{\|\mathbf{v}\|}\mathbf{I} - \frac{\mathbf{v}\mathbf{v}^\mathsf{T}}{\|\mathbf{v}\|^3}.\]

Parameters:

• v (ndarray) – Vector \(\mathbf{v}\) whose normalized derivative is taken.

• dv (ndarray, optional) – External Jacobian of \(\mathbf{v}\) to be post-multiplied by the above term, if provided.

Returns:

The Jacobian \(\partial \hat{\mathbf{v}}/\partial \mathbf{v}\) (or dv @ dndv if dv is supplied).

Return type:

ndarray

ADCS.helpers.math_helpers.quat_diff(q0, q1)

Compute the relative quaternion between two attitudes.

The quaternion error is defined as

\[\mathbf{q}_{\mathrm{err}} = \mathbf{q}_0^{-1} \otimes \mathbf{q}_1\]

The result is normalized and forced to have a non-negative scalar part, ensuring a unique minimal-angle representation.

Parameters:

• q0 (numpy.ndarray) – Reference quaternion.

• q1 (numpy.ndarray) – Target quaternion.

Returns:

Relative quaternion mapping q0 to q1.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.quat_inv(q)

Compute the inverse of a quaternion.

For a quaternion \(\mathbf{q}\), the inverse is

\[\mathbf{q}^{-1} = \frac{\mathbf{q}^*}{\|\mathbf{q}\|^2}\]

where \(\mathbf{q}^*\) denotes the conjugate quaternion.

For unit quaternions, this reduces to the conjugate.

Parameters:

q (numpy.ndarray) – Quaternion.

Returns:

Inverse quaternion.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.quat_mult(p, q, *extra)

Compute the Hamilton product of one or more quaternions.

Given two quaternions \(\mathbf{p}\) and \(\mathbf{q}\), the Hamilton product is defined as

\[\begin{split}\mathbf{p} \otimes \mathbf{q} = \begin{bmatrix} p_0 q_0 - \mathbf{p}_v^\mathsf{T}\mathbf{q}_v \\ p_0\mathbf{q}_v + q_0\mathbf{p}_v + \mathbf{p}_v \times \mathbf{q}_v \end{bmatrix}\end{split}\]

This operation composes rotations and is non-commutative.

Multiple quaternions may be supplied and are multiplied in sequence.

Parameters:

• p (numpy.ndarray) – First quaternion.

• q (numpy.ndarray) – Second quaternion.

• extra (tuple) – Additional quaternions to multiply sequentially.

Returns:

Resulting quaternion product.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.quat_to_cayley(quat)

Convert a quaternion to Cayley parameters.

\[\mathbf{p} = \frac{\boldsymbol{q}_v}{q_0}\]

Parameters:

quat (numpy.ndarray, shape (4,)) – Quaternion in Hamilton form.

Returns:

p – Cayley parameters.

Return type:

numpy.ndarray, shape (3,)

Notes

• Ensures nonzero scalar part by applying a numerical epsilon if needed.

ADCS.helpers.math_helpers.quat_to_euler(q)

Convert a quaternion to 3-2-1 (yaw-pitch-roll) Euler angles.

The conversion is performed via the corresponding direction cosine matrix and yields

\[\begin{split}\begin{aligned} \phi &= \arctan2(R_{32}, R_{33}) \\ heta &= -\arcsin(R_{31}) \\ \psi &= \arctan2(R_{21}, R_{11}) \end{aligned}\end{split}\]

The output angles are expressed in degrees.

Parameters:

q (numpy.ndarray) – Quaternion in Hamilton convention.

Returns:

Euler angles [roll, pitch, yaw] in degrees.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.quat_to_mrp(quat)

Convert a quaternion to Modified Rodrigues parameters (MRP).

\[\boldsymbol{\sigma} = \frac{2\boldsymbol{q}_v}{1 + q_0}\]

Parameters:

quat (numpy.ndarray, shape (4,)) – Quaternion in Hamilton form.

Returns:

sigma – Modified Rodrigues parameters.

Return type:

numpy.ndarray, shape (3,)

ADCS.helpers.math_helpers.quat_to_vec3(quat, mode=0)

Convert a quaternion to a 3D attitude parameter vector according to mode.

Parameters:

• quat (numpy.ndarray, shape (4,)) – Quaternion.

• mode (int) –

  Conversion mode:

  • 6 → 2×MRP (with positive scalar part)

  • 5 → 2×MRP

  • 4 → Vector component

  • 3 → Vector component (positive scalar)

  • 2 → Cayley parameters

  • 1 → MRP

  • 0 → Default: MRP (positive scalar)

Returns:

v – Equivalent attitude parameter vector.

Return type:

numpy.ndarray, shape (3,)

ADCS.helpers.math_helpers.random_n_unit_vec(n)

Generate a random unit-norm vector in \(\mathbb{R}^n\).

The components are sampled from a standard normal distribution and normalized:

\[\mathbf{v} = \frac{\mathbf{z}}{\|\mathbf{z}\|}, \qquad z_i \sim \mathcal{N}(0,1).\]

Parameters:

n (int) – Dimension of the vector space.

Returns:

A random unit vector \(\mathbf{v}\in\mathbb{R}^n\) with \(\|\mathbf{v}\|=1\).

Return type:

ndarray

ADCS.helpers.math_helpers.rot_exp(v)

Compute the exponential map \(\exp(\frac{\phi}{2}\hat{\mathbf{u}})\) to obtain a quaternion from a rotation vector.

Parameters:

v (numpy.ndarray, shape (3,)) – Rotation vector \(\boldsymbol{\phi}\) (axis × angle).

Returns:

q – Quaternion representing the rotation.

Return type:

numpy.ndarray, shape (4,)

Notes

• For \(\|\boldsymbol{\phi}\| = 0\), returns identity quaternion.

ADCS.helpers.math_helpers.rot_mat(q)

Construct the direction cosine matrix (DCM) corresponding to a quaternion using the Hamilton quaternion convention.

The returned matrix performs a rotation from the body frame to the inertial (ECI) frame, such that

\[\mathbf{v}_{\mathrm{ECI}} = \mathbf{R}(\mathbf{q})\,\mathbf{v}_{\mathrm{body}}\]

where the quaternion is defined as

\[\mathbf{q} = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^\mathsf{T}\]

with scalar-first ordering.

The rotation matrix is explicitly given by

\[\begin{split}\mathbf{R}(\mathbf{q}) = \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2) \\ 2(q_1q_2+q_0q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3-q_0q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_2q_3+q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix}\end{split}\]

Parameters:

q (numpy.ndarray) – Unit quaternion in Hamilton form [q0, q1, q2, q3].

Returns:

Direction cosine matrix mapping body-frame vectors to inertial frame.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.skewsym(v)

Construct the skew-symmetric matrix associated with a 3D vector.

The skew-symmetric operator satisfies

\[[\mathbf{v}]_\times \mathbf{a} = \mathbf{v} \times \mathbf{a}\]

and is defined as

\[\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}\]

Parameters:

v (numpy.ndarray) – Three-dimensional vector.

Returns:

Skew-symmetric cross-product matrix.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.square_mat_sections(mat, vals)

Extract a square submatrix from a matrix using index selection.

The returned matrix is formed by selecting rows and columns specified by vals:

\[\mathbf{M}_{\mathrm{sub}} = \mathbf{M}[\text{vals}, \text{vals}]\]

This utility is frequently used for covariance sub-block extraction.

Parameters:

• mat (numpy.ndarray) – Input matrix.

• vals (numpy.ndarray) – Index array defining the submatrix.

Returns:

Square submatrix.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.state_norm_jac(xk)

Construct the Jacobian of a state vector with quaternion normalization.

The Jacobian is identity except for the quaternion components, which are replaced by the quaternion normalization Jacobian

\[\frac{\partial \hat{\mathbf{q}}}{\partial \mathbf{q}}\]

This is commonly used in extended Kalman filters with quaternion states.

Parameters:

xk (numpy.ndarray) – State vector containing a quaternion in indices [3:7].

Returns:

State normalization Jacobian.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.vec3_to_quat(v3, mode)

Convert a 3-element attitude parameter vector into a quaternion according to a specified representation mode.

This function implements the inverse mapping of quat_to_vec3(), supporting Modified Rodrigues Parameters (MRP), Cayley parameters, direct vector-part representations, and scaled variants.

Depending on mode, the conversion is defined as:

\[\begin{split}\mathbf{q} = \begin{cases} \text{MRP}^{-1}(\tfrac{1}{2}\mathbf{v}), & \text{mode}=5,6 \\ \begin{bmatrix}\sqrt{1-\|\mathbf{v}\|^2} \\ \mathbf{v}\end{bmatrix}, & \text{mode}=3,4 \\ \text{Cayley}^{-1}(\mathbf{v}), & \text{mode}=2 \\ \text{MRP}^{-1}(\mathbf{v}), & \text{mode}=0,1 \end{cases}\end{split}\]

Sign conventions are enforced where applicable to ensure a positive scalar component.

Parameters:

• v3 (numpy.ndarray) – 3-element attitude parameter vector.

• mode (int) – Conversion mode selector.

Returns:

Quaternion in Hamilton convention.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.vec_norm_hess(v, dv=None, ddv=None)

Compute the Hessian of the Euclidean norm of a vector.

The second derivative of \(\|\mathbf{v}\|\) is

\[\frac{\partial^2 \|\mathbf{v}\|}{\partial \mathbf{v}^2} = \frac{1}{\|\mathbf{v}\|}\mathbf{I} - \frac{\mathbf{v}\mathbf{v}^\mathsf{T}}{\|\mathbf{v}\|^3}\]

External Jacobians and Hessians are propagated using the chain rule.

Parameters:

• v (numpy.ndarray) – Input vector.

• dv (numpy.ndarray or None) – Jacobian of the vector.

• ddv (numpy.ndarray or None) – Hessian of the vector.

Returns:

Hessian of the vector norm.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.vec_norm_jac(v, dv=None)

Compute the Jacobian of the Euclidean norm of a vector.

For \(n = \|\mathbf{v}\|\), the gradient is

\[\frac{\partial n}{\partial \mathbf{v}} = \frac{\mathbf{v}}{\|\mathbf{v}\|}\]

When an external Jacobian dv is provided, the chain rule is applied.

Parameters:

• v (numpy.ndarray) – Input vector.

• dv (numpy.ndarray or None) – Optional external Jacobian.

Returns:

Jacobian of the vector norm.

Return type:

numpy.ndarray

ADCS.helpers.math_helpers.wahbas_svd(weights, body, inertial)

Solve Wahba’s attitude determination problem using singular value decomposition.

Wahba’s problem minimizes the cost function

\[J(\mathbf{R}) = \frac{1}{2} \sum_{i=1}^N w_i \left\| \mathbf{b}_i - \mathbf{R}\mathbf{r}_i \right\|^2\]

where \(\mathbf{b}_i\) are body-frame measurements and \(\mathbf{r}_i\) are inertial-frame reference vectors.

The optimal rotation matrix is obtained via the SVD of the attitude profile matrix

\[\mathbf{B} = \sum_i w_i\,\mathbf{b}_i\mathbf{r}_i^\mathsf{T}\]

and subsequently converted to a quaternion.

Parameters:

• weights (iterable) – Scalar weights associated with each vector observation.

• body (iterable) – Body-frame measurement vectors.

• inertial (iterable) – Inertial-frame reference vectors.

Returns:

Optimal attitude quaternion.

Return type:

numpy.ndarray