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.
-
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).
-
using Vec3 = Eigen::Vector3d#
-
namespace math