mrp#

namespace saltro
namespace math

Functions

Eigen::Vector4d quatError(const Eigen::Vector4d &q_goal, const Eigen::Vector4d &q)#

Compute the quaternion error: q_err = q_goal^{-1} ⊗ q.

Returns the relative rotation from q to q_goal. If q == q_goal, the result is [1, 0, 0, 0] (identity quaternion).

Convention: scalar-first [q0, q1, q2, q3].

Parameters:

  • q_goal – Goal quaternion (unit).

  • q – Current quaternion (unit).

Returns:

Quaternion error (unit), sign-chosen so q0 >= 0.

Eigen::Vector3d quatToMRP(const Eigen::Vector4d &q_err)#

Convert a small quaternion error to Modified Rodrigues Parameters.

Given q_err = [q0, q1, q2, q3], the MRP is:

\[ \boldsymbol{\sigma} = \frac{2 \, \mathbf{q}_{1:3}}{1 + q_0} \]

For small rotations, MRP ≈ rotation vector (angle × axis).

Parameters:

q_err – Quaternion error (unit, q0 >= 0).

Returns:

3D MRP vector.

Eigen::MatrixXd findGMat(const Eigen::Vector4d &q, int nRW)#

Build the G projection matrix from full state to reduced state.

The full state is [ω(3), q(4), h_rw(nRW)] with dimension 7+nRW. The reduced state is [ω(3), δθ(3), h_rw(nRW)] with dimension 6+nRW.

The G matrix maps full-state perturbations to reduced-state perturbations:

\[\begin{split} G = \begin{bmatrix} I_3 & 0 & 0 \\ 0 & W^T(q) & 0 \\ 0 & 0 & I_{nRW} \end{bmatrix} \end{split}\]
where W(q) is the 4×3 quaternion kinematics matrix, so W^T is 3×4.

G has shape (6+nRW) × (7+nRW).

Parameters:

  • q – Unit quaternion at the linearization point.

  • nRW – Number of reaction wheels.

Returns:

G matrix of size (6+nRW) × (7+nRW).