diff --git a/libs/kaptan-math/Cargo.toml b/libs/kaptan-math/Cargo.toml
new file mode 100644
index 0000000..863bc31
--- /dev/null
+++ b/libs/kaptan-math/Cargo.toml
@@ -0,0 +1,10 @@
+[package]
+name = "kaptan-math"
+edition.workspace = true
+license.workspace = true
+repository.workspace = true
+rust-version.workspace = true
+version.workspace = true
+
+[lints]
+workspace = true
diff --git a/libs/kaptan-math/src/lib.rs b/libs/kaptan-math/src/lib.rs
new file mode 100644
index 0000000..18b3ba0
--- /dev/null
+++ b/libs/kaptan-math/src/lib.rs
@@ -0,0 +1,898 @@
+//! Minimal `f32` math shared by Kaptan navigation and control.
+//!
+//! This crate deliberately exposes only fixed-size vectors and a unit
+//! quaternion. Frame markers carry no runtime data and prevent accidental
+//! body/local-vector interchange at public navigation and control boundaries.
+
+#![no_std]
+
+use core::marker::PhantomData;
+
+/// Failure reported by a checked mathematical operation.
+///
+/// The flight path must treat one of these as a rejected input or an estimator
+/// failure.  In particular, no quaternion constructor converts an error into
+/// the identity rotation.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum MathError {
+    /// An operand or intermediate value was not finite.
+    NonFinite,
+    /// A vector or quaternion did not have a representable non-zero norm.
+    ZeroNorm,
+    /// A small-angle quaternion was requested outside its reviewed domain.
+    RotationIncrementOutOfRange,
+    /// Rotation-matrix axes were not a finite, right-handed orthonormal basis.
+    NonOrthonormalAxes,
+}
+
+/// Local north-east-down navigation frame.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum Ned {}
+/// Aircraft forward-right-down body frame.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum Frd {}
+/// A physical sensor's locally declared coordinate frame.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum Sensor {}
+
+/// Three `f32` components expressed in one explicit frame.
+#[derive(Debug, PartialEq)]
+pub struct Vector3<Frame> {
+    x: f32,
+    y: f32,
+    z: f32,
+    _frame: PhantomData<Frame>,
+}
+
+impl<Frame> Copy for Vector3<Frame> {}
+impl<Frame> Clone for Vector3<Frame> {
+    fn clone(&self) -> Self {
+        *self
+    }
+}
+
+impl<Frame> Vector3<Frame> {
+    /// Creates a vector whose components use the caller's documented unit.
+    pub const fn new(x: f32, y: f32, z: f32) -> Self {
+        Self {
+            x,
+            y,
+            z,
+            _frame: PhantomData,
+        }
+    }
+
+    /// Returns the x component.
+    pub const fn x(self) -> f32 {
+        self.x
+    }
+    /// Returns the y component.
+    pub const fn y(self) -> f32 {
+        self.y
+    }
+    /// Returns the z component.
+    pub const fn z(self) -> f32 {
+        self.z
+    }
+
+    /// Returns the zero vector.
+    pub const fn zero() -> Self {
+        Self::new(0.0, 0.0, 0.0)
+    }
+
+    /// Returns whether every component is finite.
+    pub fn is_finite(self) -> bool {
+        self.x.is_finite() & self.y.is_finite() & self.z.is_finite()
+    }
+
+    /// Adds vectors in the same frame and unit.
+    pub const fn add(self, other: Self) -> Self {
+        Self::new(self.x + other.x, self.y + other.y, self.z + other.z)
+    }
+
+    /// Subtracts vectors in the same frame and unit.
+    pub const fn sub(self, other: Self) -> Self {
+        Self::new(self.x - other.x, self.y - other.y, self.z - other.z)
+    }
+
+    /// Scales a vector without changing its frame.
+    pub const fn scale(self, scalar: f32) -> Self {
+        Self::new(self.x * scalar, self.y * scalar, self.z * scalar)
+    }
+
+    /// Dot product of vectors in the same frame.
+    pub const fn dot(self, other: Self) -> f32 {
+        self.x * other.x + self.y * other.y + self.z * other.z
+    }
+
+    /// Cross product of vectors in the same frame.
+    pub const fn cross(self, other: Self) -> Self {
+        Self::new(
+            self.y * other.z - self.z * other.y,
+            self.z * other.x - self.x * other.z,
+            self.x * other.y - self.y * other.x,
+        )
+    }
+
+    /// Squared Euclidean norm.
+    pub const fn norm_squared(self) -> f32 {
+        self.dot(self)
+    }
+
+    /// Euclidean norm using a bounded Newton iteration.
+    pub fn norm(self) -> f32 {
+        sqrt(self.norm_squared())
+    }
+
+    /// Returns a unit vector, rejecting zero and non-finite input.
+    ///
+    /// Scaling before normalization prevents an otherwise finite, large vector
+    /// from overflowing while its direction is still perfectly representable.
+    pub fn normalized(self) -> Option<Self> {
+        if !self.is_finite() {
+            return None;
+        }
+        let scale = self.x.abs().max(self.y.abs()).max(self.z.abs());
+        if scale <= 1.0e-8 {
+            return None;
+        }
+        let scaled = self.scale(1.0 / scale);
+        let norm = sqrt(scaled.norm_squared());
+        // `scale` is a finite non-zero component, so `scaled` contains one
+        // component with magnitude one. Its squared norm is therefore finite
+        // and at least one; keep that proof local rather than carrying an
+        // unreachable fallback after the public input validation above.
+        debug_assert!(norm.is_finite());
+        debug_assert!(norm > 0.0);
+        let normalized = scaled.scale(1.0 / norm);
+        normalized.is_finite().then_some(normalized)
+    }
+}
+
+/// Two `f32` components expressed in one explicit frame.
+#[derive(Debug, PartialEq)]
+pub struct Vector2<Frame> {
+    x: f32,
+    y: f32,
+    _frame: PhantomData<Frame>,
+}
+
+impl<Frame> Copy for Vector2<Frame> {}
+impl<Frame> Clone for Vector2<Frame> {
+    fn clone(&self) -> Self {
+        *self
+    }
+}
+
+impl<Frame> Vector2<Frame> {
+    /// Creates a two-dimensional vector.
+    pub const fn new(x: f32, y: f32) -> Self {
+        Self {
+            x,
+            y,
+            _frame: PhantomData,
+        }
+    }
+    /// Returns the x component.
+    pub const fn x(self) -> f32 {
+        self.x
+    }
+    /// Returns the y component.
+    pub const fn y(self) -> f32 {
+        self.y
+    }
+    /// Returns whether both components are finite.
+    pub fn is_finite(self) -> bool {
+        self.x.is_finite() && self.y.is_finite()
+    }
+}
+
+/// A normalized Hamilton rotation from `From` into `To`.
+///
+/// Source and destination frames are zero-sized type parameters, so a caller
+/// cannot apply a valid quaternion to a vector in an unrelated frame. The
+/// navigation convention is body-to-NED.
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub struct UnitQuaternion<From, To> {
+    w: f32,
+    x: f32,
+    y: f32,
+    z: f32,
+    _frames: PhantomData<fn(From) -> To>,
+}
+
+impl<From, To> UnitQuaternion<From, To> {
+    /// Identity-component rotation. Calling this for different frame types is
+    /// an explicit assertion that their declared axes are aligned.
+    pub const fn identity() -> Self {
+        Self {
+            w: 1.0,
+            x: 0.0,
+            y: 0.0,
+            z: 0.0,
+            _frames: PhantomData,
+        }
+    }
+
+    /// Normalizes raw Hamilton components, rejecting non-finite or zero input.
+    ///
+    /// Values are scaled before the norm is evaluated, so valid directions
+    /// with very large finite components do not overflow to an identity-like
+    /// result.
+    pub fn from_components(w: f32, x: f32, y: f32, z: f32) -> Result<Self, MathError> {
+        if !w.is_finite() | !x.is_finite() | !y.is_finite() | !z.is_finite() {
+            return Err(MathError::NonFinite);
+        }
+        let scale = w.abs().max(x.abs()).max(y.abs()).max(z.abs());
+        if scale <= 1.0e-8 {
+            return Err(MathError::ZeroNorm);
+        }
+        let sw = w / scale;
+        let sx = x / scale;
+        let sy = y / scale;
+        let sz = z / scale;
+        let norm = sqrt(sw * sw + sx * sx + sy * sy + sz * sz);
+        // A finite non-zero maximum component makes one scaled component
+        // exactly +/-1. The normalization norm is consequently finite and
+        // strictly positive after the checks above.
+        debug_assert!(norm.is_finite());
+        debug_assert!(norm > 0.0);
+        Ok(Self {
+            w: sw / norm,
+            x: sx / norm,
+            y: sy / norm,
+            z: sz / norm,
+            _frames: PhantomData,
+        })
+    }
+
+    /// Returns the scalar component.
+    pub const fn w(self) -> f32 {
+        self.w
+    }
+    /// Returns the x imaginary component.
+    pub const fn x(self) -> f32 {
+        self.x
+    }
+    /// Returns the y imaginary component.
+    pub const fn y(self) -> f32 {
+        self.y
+    }
+    /// Returns the z imaginary component.
+    pub const fn z(self) -> f32 {
+        self.z
+    }
+
+    /// Returns whether components are finite and normalized within a small
+    /// floating-point tolerance.
+    pub fn is_normalized(self) -> bool {
+        let squared_norm_error =
+            (self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z - 1.0).abs();
+        self.w.is_finite()
+            & self.x.is_finite()
+            & self.y.is_finite()
+            & self.z.is_finite()
+            & (squared_norm_error < 2.0e-4)
+    }
+
+    /// Composes rotations: the returned rotation applies `right` then `self`.
+    pub fn compose<RightFrom>(
+        self,
+        right: UnitQuaternion<RightFrom, From>,
+    ) -> Result<UnitQuaternion<RightFrom, To>, MathError> {
+        UnitQuaternion::from_components(
+            self.w * right.w - self.x * right.x - self.y * right.y - self.z * right.z,
+            self.w * right.x + self.x * right.w + self.y * right.z - self.z * right.y,
+            self.w * right.y - self.x * right.z + self.y * right.w + self.z * right.x,
+            self.w * right.z + self.x * right.y - self.y * right.x + self.z * right.w,
+        )
+    }
+
+    /// Inverts a unit rotation.
+    pub const fn inverse(self) -> UnitQuaternion<To, From> {
+        UnitQuaternion {
+            w: self.w,
+            x: -self.x,
+            y: -self.y,
+            z: -self.z,
+            _frames: PhantomData,
+        }
+    }
+
+    /// Rotates a vector into this rotation's destination frame.
+    pub fn rotate(self, vector: Vector3<From>) -> Vector3<To> {
+        let q = Vector3::<To>::new(self.x, self.y, self.z);
+        let v = Vector3::<To>::new(vector.x, vector.y, vector.z);
+        let twice_cross = q.cross(v).scale(2.0);
+        v.add(twice_cross.scale(self.w)).add(q.cross(twice_cross))
+    }
+}
+
+impl<Frame> UnitQuaternion<Frame, Frame> {
+    /// Builds an incremental same-frame rotation from a rotation vector in
+    /// radians. The flight estimator admits increments no larger than two
+    /// radians before calling this bounded polynomial, avoiding a hidden
+    /// `libm` dependency in the rate path.
+    pub fn from_small_rotation_vector(rotation: Vector3<Frame>) -> Result<Self, MathError> {
+        if !rotation.is_finite() {
+            return Err(MathError::NonFinite);
+        }
+        let rotation_squared = rotation.norm_squared();
+        if !rotation_squared.is_finite() {
+            return Err(MathError::NonFinite);
+        }
+        if rotation_squared > 4.0 {
+            return Err(MathError::RotationIncrementOutOfRange);
+        }
+        let half_squared = 0.25 * rotation_squared;
+        let sin_over_angle = 0.5 * (1.0 - half_squared / 6.0 + half_squared * half_squared / 120.0);
+        let cosine = 1.0 - half_squared / 2.0 + half_squared * half_squared / 24.0
+            - half_squared * half_squared * half_squared / 720.0;
+        // The expression is close to unit length for its stated domain. A
+        // normalization removes accumulated polynomial/rounding error.
+        Self::from_components(
+            cosine,
+            rotation.x * sin_over_angle,
+            rotation.y * sin_over_angle,
+            rotation.z * sin_over_angle,
+        )
+    }
+}
+
+impl UnitQuaternion<Frd, Ned> {
+    /// Builds a body-to-NED rotation from orthonormal body x/y/z axes
+    /// expressed in NED.
+    pub fn from_body_axes_in_ned(
+        body_x: Vector3<Ned>,
+        body_y: Vector3<Ned>,
+        body_z: Vector3<Ned>,
+    ) -> Result<Self, MathError> {
+        if !body_x.is_finite() || !body_y.is_finite() || !body_z.is_finite() {
+            return Err(MathError::NonFinite);
+        }
+        const ORTHONORMAL_TOLERANCE: f32 = 2.0e-3;
+        let unit = |axis: Vector3<Ned>| (axis.norm_squared() - 1.0).abs() <= ORTHONORMAL_TOLERANCE;
+        let handedness = body_x.cross(body_y).dot(body_z);
+        if !unit(body_x)
+            || !unit(body_y)
+            || !unit(body_z)
+            || body_x.dot(body_y).abs() > ORTHONORMAL_TOLERANCE
+            || body_x.dot(body_z).abs() > ORTHONORMAL_TOLERANCE
+            || body_y.dot(body_z).abs() > ORTHONORMAL_TOLERANCE
+            || (handedness - 1.0).abs() > ORTHONORMAL_TOLERANCE
+        {
+            return Err(MathError::NonOrthonormalAxes);
+        }
+        let m00 = body_x.x;
+        let m01 = body_y.x;
+        let m02 = body_z.x;
+        let m10 = body_x.y;
+        let m11 = body_y.y;
+        let m12 = body_z.y;
+        let m20 = body_x.z;
+        let m21 = body_y.z;
+        let m22 = body_z.z;
+        let trace = m00 + m11 + m22;
+        let (w, x, y, z) = if trace > 0.0 {
+            let s = 2.0 * sqrt(trace + 1.0);
+            (0.25 * s, (m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s)
+        } else if m00 > m11 && m00 > m22 {
+            let s = 2.0 * sqrt(1.0 + m00 - m11 - m22);
+            ((m21 - m12) / s, 0.25 * s, (m01 + m10) / s, (m02 + m20) / s)
+        } else if m11 > m22 {
+            let s = 2.0 * sqrt(1.0 + m11 - m00 - m22);
+            ((m02 - m20) / s, (m01 + m10) / s, 0.25 * s, (m12 + m21) / s)
+        } else {
+            let s = 2.0 * sqrt(1.0 + m22 - m00 - m11);
+            ((m10 - m01) / s, (m02 + m20) / s, (m12 + m21) / s, 0.25 * s)
+        };
+        Self::from_components(w, x, y, z)
+    }
+
+    /// Rotates a body vector into NED using a body-to-NED quaternion.
+    pub fn body_to_ned(self, vector: Vector3<Frd>) -> Vector3<Ned> {
+        self.rotate(vector)
+    }
+
+    /// Rotates an NED vector into body using a body-to-NED quaternion.
+    pub fn ned_to_body(self, vector: Vector3<Ned>) -> Vector3<Frd> {
+        self.inverse().rotate(vector)
+    }
+}
+
+/// Bounded square root for positive finite `f32` values.
+///
+/// A scaled exponent estimate plus four Newton iterations provides bounded
+/// vector/quaternion normalization precision across the finite `f32` range.
+/// Negative/non-finite input is rejected as `NaN` rather than silently
+/// converted.
+pub fn sqrt(value: f32) -> f32 {
+    if !value.is_finite() || value < 0.0 {
+        return f32::NAN;
+    }
+    if value == 0.0 {
+        return value;
+    }
+    // The exponent shortcut below assumes a normal input.  Scale a
+    // subnormal by an exact power of two, then undo half that exponent after
+    // the same bounded iteration.  This keeps the smallest finite positive
+    // `f32` in the reviewed accuracy envelope without a libm dependency.
+    if value < f32::MIN_POSITIVE {
+        return sqrt(value * 16_777_216.0) / 4_096.0;
+    }
+    // A bit-level exponent estimate makes the fixed iteration count accurate
+    // for subnormal through maximum finite inputs.  It avoids linking a
+    // platform libm implementation in the embedded rate path.
+    let mut estimate = f32::from_bits((value.to_bits() >> 1) + 0x1fc0_0000);
+    let mut iteration = 0;
+    while iteration < 4 {
+        estimate = 0.5 * (estimate + value / estimate);
+        iteration += 1;
+    }
+    estimate
+}
+
+/// Applies the SO(3) right Jacobian to a vector in one frame.
+///
+/// The same bounded two-radian increment domain as
+/// [`UnitQuaternion::from_small_rotation_vector`] avoids a hidden `libm`
+/// dependency. The coefficients are fixed Taylor polynomials for
+/// `(1 - cos(theta)) / theta²` and `(theta - sin(theta)) / theta³`.
+pub fn right_jacobian_apply<Frame>(
+    rotation: Vector3<Frame>,
+    vector: Vector3<Frame>,
+) -> Result<Vector3<Frame>, MathError> {
+    jacobian_apply(rotation, vector, -1.0)
+}
+
+/// Applies the SO(3) left Jacobian to a vector in one frame.
+///
+/// For the same rotation, the left and right Jacobians differ only in the
+/// sign of the first cross-product term.
+pub fn left_jacobian_apply<Frame>(
+    rotation: Vector3<Frame>,
+    vector: Vector3<Frame>,
+) -> Result<Vector3<Frame>, MathError> {
+    jacobian_apply(rotation, vector, 1.0)
+}
+
+fn jacobian_apply<Frame>(
+    rotation: Vector3<Frame>,
+    vector: Vector3<Frame>,
+    first_cross_sign: f32,
+) -> Result<Vector3<Frame>, MathError> {
+    if !rotation.is_finite() | !vector.is_finite() {
+        return Err(MathError::NonFinite);
+    }
+    let theta_squared = rotation.norm_squared();
+    if !theta_squared.is_finite() {
+        return Err(MathError::NonFinite);
+    }
+    if theta_squared > 4.0 {
+        return Err(MathError::RotationIncrementOutOfRange);
+    }
+    let theta_fourth = theta_squared * theta_squared;
+    let theta_sixth = theta_fourth * theta_squared;
+    let theta_eighth = theta_fourth * theta_fourth;
+    let first_coefficient = 0.5 - theta_squared / 24.0 + theta_fourth / 720.0
+        - theta_sixth / 40_320.0
+        + theta_eighth / 3_628_800.0;
+    let second_coefficient = 1.0 / 6.0 - theta_squared / 120.0 + theta_fourth / 5_040.0
+        - theta_sixth / 362_880.0
+        + theta_eighth / 39_916_800.0;
+    let cross = rotation.cross(vector);
+    let result = vector
+        .add(cross.scale(first_cross_sign * first_coefficient))
+        .add(rotation.cross(cross).scale(second_coefficient));
+    result
+        .is_finite()
+        .then_some(result)
+        .ok_or(MathError::NonFinite)
+}
+
+#[cfg(test)]
+mod tests {
+    use core::marker::PhantomData;
+
+    use super::{
+        Frd, MathError, Ned, UnitQuaternion, Vector3, left_jacobian_apply, right_jacobian_apply,
+        sqrt,
+    };
+
+    #[test]
+    fn frame_rotation_preserves_norm() {
+        let q = UnitQuaternion::<Frd, Ned>::identity();
+        let vector = Vector3::<Frd>::new(3.0, -4.0, 2.0);
+        assert!((q.body_to_ned(vector).norm() - vector.norm()).abs() < 1.0e-3);
+    }
+
+    #[test]
+    fn composed_rotations_normalize() {
+        let delta =
+            UnitQuaternion::from_small_rotation_vector(Vector3::<Frd>::new(0.0, 0.0, 0.001))
+                .unwrap();
+        let mut q = UnitQuaternion::<Frd, Ned>::identity();
+        for _ in 0..10_000 {
+            q = q.compose(delta).unwrap();
+        }
+        assert!(q.is_normalized());
+        let forward = q.body_to_ned(Vector3::<Frd>::new(1.0, 0.0, 0.0));
+        assert!((forward.x() + 0.839).abs() < 0.02);
+    }
+
+    #[test]
+    fn matrix_identity() {
+        let q = UnitQuaternion::from_body_axes_in_ned(
+            Vector3::<Ned>::new(1.0, 0.0, 0.0),
+            Vector3::<Ned>::new(0.0, 1.0, 0.0),
+            Vector3::<Ned>::new(0.0, 0.0, 1.0),
+        )
+        .unwrap();
+        assert_eq!(q, UnitQuaternion::<Frd, Ned>::identity());
+    }
+
+    #[test]
+    fn sqrt_accuracy() {
+        assert!((sqrt(2.0) - core::f32::consts::SQRT_2).abs() < 1.0e-5);
+        for value in [
+            f32::from_bits(1),
+            f32::from_bits(2),
+            f32::from_bits(0x007f_ffff),
+        ] {
+            let root = sqrt(value);
+            assert!(root.is_finite());
+            assert!(root > 0.0);
+            assert!(((root * root - value) / value).abs() < 2.0e-5);
+        }
+    }
+
+    #[test]
+    fn so3_jacobians_match_their_bounded_series_reference() {
+        let rotation = Vector3::<Frd>::new(0.0, 0.0, 0.5);
+        let vector = Vector3::<Frd>::new(1.0, 0.0, 0.0);
+        let right = right_jacobian_apply(rotation, vector).unwrap();
+        let left = left_jacobian_apply(rotation, vector).unwrap();
+        let theta = 0.5_f32;
+        let expected_x = theta.sin() / theta;
+        let expected_y = (1.0 - theta.cos()) / theta;
+        assert!((right.x() - expected_x).abs() < 1.0e-6);
+        assert!((right.y() + expected_y).abs() < 1.0e-6);
+        assert!((left.x() - expected_x).abs() < 1.0e-6);
+        assert!((left.y() - expected_y).abs() < 1.0e-6);
+        assert_eq!(
+            right_jacobian_apply(Vector3::new(2.1, 0.0, 0.0), vector),
+            Err(MathError::RotationIncrementOutOfRange)
+        );
+        assert_eq!(
+            left_jacobian_apply(Vector3::new(f32::NAN, 0.0, 0.0), vector),
+            Err(MathError::NonFinite)
+        );
+        assert_eq!(
+            right_jacobian_apply(Vector3::new(f32::MAX, 0.0, 0.0), vector),
+            Err(MathError::NonFinite)
+        );
+    }
+
+    #[test]
+    fn quaternion_validation() {
+        assert_eq!(
+            UnitQuaternion::<Frd, Ned>::from_components(0.0, 0.0, 0.0, 0.0),
+            Err(MathError::ZeroNorm)
+        );
+        assert_eq!(
+            UnitQuaternion::<Frd, Ned>::from_components(f32::NAN, 0.0, 0.0, 0.0),
+            Err(MathError::NonFinite)
+        );
+        assert_eq!(
+            UnitQuaternion::<Frd, Frd>::from_small_rotation_vector(Vector3::new(2.01, 0.0, 0.0)),
+            Err(MathError::RotationIncrementOutOfRange)
+        );
+        assert_eq!(
+            UnitQuaternion::from_body_axes_in_ned(
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::<Ned>::new(0.0, 0.0, 1.0),
+            ),
+            Err(MathError::NonOrthonormalAxes)
+        );
+    }
+
+    #[test]
+    fn quaternion_known_answers() {
+        let half = core::f32::consts::FRAC_1_SQRT_2;
+        let q = UnitQuaternion::<Frd, Ned>::from_components(half, 0.0, 0.0, half).unwrap();
+        let east = q.body_to_ned(Vector3::new(1.0, 0.0, 0.0));
+        assert!(east.x().abs() < 2.0e-6);
+        assert!((east.y() - 1.0).abs() < 2.0e-6);
+
+        let huge =
+            UnitQuaternion::<Frd, Ned>::from_components(f32::MAX, -f32::MAX, f32::MAX, -f32::MAX)
+                .unwrap();
+        assert!(huge.is_normalized());
+        let recovered = q.ned_to_body(east);
+        assert!((recovered.x() - 1.0).abs() < 2.0e-6);
+        assert!(recovered.y().abs() < 2.0e-6);
+    }
+
+    #[test]
+    fn sqrt_rejects_invalid_input() {
+        assert!(sqrt(f32::NAN).is_nan());
+        assert!(sqrt(f32::INFINITY).is_nan());
+        assert!(sqrt(-1.0).is_nan());
+        assert_eq!(sqrt(-0.0).to_bits(), (-0.0_f32).to_bits());
+
+        for value in [
+            f32::from_bits(1),
+            f32::from_bits(0x007f_ffff),
+            f32::MIN_POSITIVE,
+            1.0e-20,
+            1.0e-6,
+            2.0,
+            123.456,
+            f32::MAX,
+        ] {
+            let reference = (value as f64).sqrt() as f32;
+            let actual = sqrt(value);
+            assert!(((actual - reference) / reference).abs() < 3.0e-6, "{value}");
+        }
+    }
+
+    #[test]
+    fn vector_validation() {
+        let vector = Vector3::<Frd>::new(3.0, 4.0, 0.0);
+        assert_eq!(vector.clone(), vector);
+        assert_eq!((vector.x(), vector.y(), vector.z()), (3.0, 4.0, 0.0));
+        assert_eq!(vector.normalized().unwrap(), Vector3::new(0.6, 0.8, 0.0));
+        assert!(
+            Vector3::<Frd>::new(f32::NAN, 0.0, 0.0)
+                .normalized()
+                .is_none()
+        );
+        assert!(Vector3::<Frd>::zero().normalized().is_none());
+        assert_eq!(
+            vector.sub(Vector3::new(1.0, -2.0, 3.0)),
+            Vector3::new(2.0, 6.0, -3.0)
+        );
+        for invalid in [
+            Vector3::<Frd>::new(f32::NAN, 0.0, 0.0),
+            Vector3::<Frd>::new(0.0, f32::INFINITY, 0.0),
+            Vector3::<Frd>::new(0.0, 0.0, f32::NEG_INFINITY),
+        ] {
+            assert!(!invalid.is_finite());
+        }
+
+        let two = super::Vector2::<Frd>::new(1.0, -2.0);
+        assert_eq!(two.clone(), two);
+        assert_eq!((two.x(), two.y()), (1.0, -2.0));
+        assert!(two.is_finite());
+        assert!(!super::Vector2::<Frd>::new(f32::NAN, 0.0).is_finite());
+    }
+
+    #[test]
+    fn quaternion_accessors() {
+        let quaternion = UnitQuaternion::<Frd, Ned>::from_components(1.0, 2.0, 3.0, 4.0).unwrap();
+        assert_eq!(quaternion.clone(), quaternion);
+        assert!(quaternion.w() > 0.0);
+        assert!(quaternion.x() > 0.0);
+        assert!(quaternion.y() > 0.0);
+        assert!(quaternion.z() > 0.0);
+        assert!(
+            UnitQuaternion::<Frd, Ned>::identity()
+                .inverse()
+                .is_normalized()
+        );
+        assert_eq!(
+            UnitQuaternion::<Frd, Frd>::from_small_rotation_vector(Vector3::new(
+                f32::INFINITY,
+                0.0,
+                0.0
+            )),
+            Err(MathError::NonFinite)
+        );
+        assert_eq!(
+            UnitQuaternion::<Frd, Frd>::from_small_rotation_vector(Vector3::new(
+                f32::MAX,
+                0.0,
+                0.0
+            )),
+            Err(MathError::NonFinite)
+        );
+        assert_eq!(
+            UnitQuaternion::<Frd, Ned>::from_body_axes_in_ned(
+                Vector3::new(f32::NAN, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+            ),
+            Err(MathError::NonFinite)
+        );
+    }
+
+    #[test]
+    fn matrix_diagonals() {
+        let axes = [
+            // 180 degree rotations about body x, y, and z respectively.
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.0, -1.0, 0.0),
+                Vector3::new(0.0, 0.0, -1.0),
+            ],
+            [
+                Vector3::<Ned>::new(-1.0, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(0.0, 0.0, -1.0),
+            ],
+            [
+                Vector3::<Ned>::new(-1.0, 0.0, 0.0),
+                Vector3::new(0.0, -1.0, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+            ],
+        ];
+        for [x, y, z] in axes {
+            assert!(
+                UnitQuaternion::from_body_axes_in_ned(x, y, z)
+                    .unwrap()
+                    .is_normalized()
+            );
+        }
+    }
+
+    #[test]
+    fn validation_operands() {
+        for vector in [
+            Vector3::<Frd>::new(f32::NAN, 0.0, 0.0),
+            Vector3::new(0.0, f32::NAN, 0.0),
+            Vector3::new(0.0, 0.0, f32::NAN),
+        ] {
+            assert!(!vector.is_finite());
+        }
+        for components in [
+            (f32::NAN, 1.0, 0.0, 0.0),
+            (1.0, f32::NAN, 0.0, 0.0),
+            (1.0, 0.0, f32::NAN, 0.0),
+            (1.0, 0.0, 0.0, f32::NAN),
+        ] {
+            assert_eq!(
+                UnitQuaternion::<Frd, Ned>::from_components(
+                    components.0,
+                    components.1,
+                    components.2,
+                    components.3,
+                ),
+                Err(MathError::NonFinite)
+            );
+        }
+        assert_eq!(
+            UnitQuaternion::<Frd, Ned>::from_components(f32::INFINITY, 0.0, 0.0, 0.0),
+            Err(MathError::NonFinite)
+        );
+        assert_eq!(
+            UnitQuaternion::<Frd, Ned>::from_components(1.0, f32::INFINITY, 0.0, 0.0),
+            Err(MathError::NonFinite)
+        );
+        assert_eq!(
+            UnitQuaternion::<Frd, Ned>::from_components(1.0, 0.0, f32::INFINITY, 0.0),
+            Err(MathError::NonFinite)
+        );
+        assert_eq!(
+            UnitQuaternion::<Frd, Ned>::from_components(1.0, 0.0, 0.0, f32::INFINITY),
+            Err(MathError::NonFinite)
+        );
+        for quaternion in [
+            UnitQuaternion::<Frd, Ned> {
+                w: f32::NAN,
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+                _frames: PhantomData,
+            },
+            UnitQuaternion::<Frd, Ned> {
+                w: 1.0,
+                x: f32::NAN,
+                y: 0.0,
+                z: 0.0,
+                _frames: PhantomData,
+            },
+            UnitQuaternion::<Frd, Ned> {
+                w: 1.0,
+                x: 0.0,
+                y: f32::NAN,
+                z: 0.0,
+                _frames: PhantomData,
+            },
+            UnitQuaternion::<Frd, Ned> {
+                w: 1.0,
+                x: 0.0,
+                y: 0.0,
+                z: f32::NAN,
+                _frames: PhantomData,
+            },
+            UnitQuaternion::<Frd, Ned> {
+                w: 2.0,
+                x: 0.0,
+                y: 0.0,
+                z: 0.0,
+                _frames: PhantomData,
+            },
+        ] {
+            assert!(!quaternion.is_normalized());
+        }
+        for axes in [
+            [
+                Vector3::<Ned>::new(2.0, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.0, 2.0, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(0.0, 0.0, 2.0),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.1, 1.0, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.1),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.1),
+                Vector3::new(0.0, 0.0, 1.0),
+            ],
+        ] {
+            assert_eq!(
+                UnitQuaternion::from_body_axes_in_ned(axes[0], axes[1], axes[2]),
+                Err(MathError::NonOrthonormalAxes)
+            );
+        }
+        for axes in [
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(f32::NAN, 0.0, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(f32::NAN, 0.0, 0.0),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.01, 0.999_95, 0.0),
+                Vector3::new(0.0, 0.0, 1.0),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(0.01, 0.0, 0.999_95),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(0.0, 0.01, 0.999_95),
+            ],
+            [
+                Vector3::<Ned>::new(1.0, 0.0, 0.0),
+                Vector3::new(0.0, 1.0, 0.0),
+                Vector3::new(0.0, 0.0, -1.0),
+            ],
+        ] {
+            assert!(UnitQuaternion::from_body_axes_in_ned(axes[0], axes[1], axes[2]).is_err());
+        }
+        let diagonal_tie = UnitQuaternion::from_body_axes_in_ned(
+            Vector3::<Ned>::new(0.0, 0.0, 1.0),
+            Vector3::new(0.0, -1.0, 0.0),
+            Vector3::new(1.0, 0.0, 0.0),
+        )
+        .unwrap();
+        assert!(diagonal_tie.is_normalized());
+    }
+}