Fix yaw gradient bug

src/smsfusion/_ins/_common.py

@@ -8,10 +8,22 @@
 @njit  # type: ignore[misc]
 def _yaw_gradient(q: NDArray[np.float64]) -> NDArray[np.float64]:
     """
-    Compute yaw/heading angle gradient wrt to the unit quaternion.
+    Compute the yaw/heading angle gradient wrt. the attitude error state.
 
-    Defined in terms of scaled Gibbs vector in ref [1]_, but implemented in terms of
-    unit quaternion here to avoid singularities.
+    The attitude error state, ``a``, is a small body-frame rotation parameterized as
+    a scaled (2x) Gibbs vector, applied multiplicatively as ``q_true = q ⊗ dq(a)``
+    (see :func:`_update_quaternion_with_gibbs2`). The gradient of the yaw angle (as
+    given by :func:`_yaw_from_quaternion`) wrt. this error is the bottom row of the
+    Euler angle rate transformation matrix::
+
+        d(yaw)/da = [0, sin(roll) / cos(pitch), cos(roll) / cos(pitch)]
+
+    Implemented in terms of the unit quaternion to avoid computing the Euler angles.
+    The x-component is identically zero: the yaw angle is ``atan2(R_nb[1, 0],
+    R_nb[0, 0])``, i.e. a function of the body x-axis alone, which a rotation about
+    that same axis leaves unchanged.
+
+    Singular at ``cos(pitch) = 0``, as the yaw angle itself is.
 
     Parameters
     ----------
@@ -22,26 +34,23 @@ def _yaw_gradient(q: NDArray[np.float64]) -> NDArray[np.float64]:
     -------
     numpy.ndarray, shape (3,)
         Yaw angle gradient vector.
-
-    References
-    ----------
-    .. [1] Fossen, T.I., "Handbook of Marine Craft Hydrodynamics and Motion Control",
-    2nd Edition, equation 14.254, John Wiley & Sons, 2021.
     """
     q_w, q_x, q_y, q_z = q
-    u_y = 2.0 * (q_x * q_y + q_z * q_w)
-    u_x = 1.0 - 2.0 * (q_y**2 + q_z**2)
-    u = u_y / u_x
 
-    duda_scale = 1.0 / u_x**2
-    duda_x = -(q_w * q_y) * (1.0 - 2.0 * q_w**2) - (2.0 * q_w**2 * q_x * q_z)
-    duda_y = (q_w * q_x) * (1.0 - 2.0 * q_z**2) + (2.0 * q_w**2 * q_y * q_z)
-    duda_z = q_w**2 * (1.0 - 2.0 * q_y**2) + (2.0 * q_w * q_x * q_y * q_z)
-    duda = duda_scale * np.array([duda_x, duda_y, duda_z])
+    # Entries of the rotation matrix, R_nb, needed below
+    r_00 = 1.0 - 2.0 * (q_y**2 + q_z**2)
+    r_01 = 2.0 * (q_x * q_y - q_w * q_z)
+    r_02 = 2.0 * (q_x * q_z + q_w * q_y)
+    r_10 = 2.0 * (q_x * q_y + q_w * q_z)
+    r_11 = 1.0 - 2.0 * (q_x**2 + q_z**2)
+    r_12 = 2.0 * (q_y * q_z - q_w * q_x)
+
+    cos_pitch_sq = r_00**2 + r_10**2
 
-    dhda = 1.0 / (1.0 + u**2) * duda
+    dhda_y = -(r_00 * r_12 - r_10 * r_02) / cos_pitch_sq  # sin(roll) / cos(pitch)
+    dhda_z = (r_00 * r_11 - r_10 * r_01) / cos_pitch_sq  # cos(roll) / cos(pitch)
 
-    return dhda  # type: ignore[no-any-return]
+    return np.array([0.0, dhda_y, dhda_z])
 
 
 @njit  # type: ignore[misc]

tests/test_ins/test_common.py

@@ -5,36 +5,68 @@
 from smsfusion._ins import _common
 
 
+def _quaternion_from_euler(roll, pitch, yaw):
+    """Unit quaternion (qw, qx, qy, qz) from ZYX Euler angles in radians."""
+    quaternion = Rotation.from_euler("ZYX", (yaw, pitch, roll), degrees=False).as_quat()
+    return np.r_[quaternion[3], quaternion[:3]]
+
+
 @pytest.mark.parametrize(
-    "quaternion, dhda_expect",
+    "angles",
     [
-        (
-            np.array([1.0, 0.0, 0.0, 0.0]),
-            np.array([0.0, 0.0, 1.0]),
-        ),
-        (
-            np.array([0.89442719, 0.4472136, 0.0, 0.0]),  # gibbs -> [1.0, 0.0, 0.0]
-            np.array([0.0, 10.0, 20.0]) / (4.0 + 1.0) ** 2,
-        ),
-        (
-            np.array([0.89442719, 0.0, 0.4472136, 0.0]),  # gibbs -> [0.0, 1.0, 0.0]
-            np.array([6.0, 0.0, 12.0]) / (4.0 - 1.0) ** 2,
-        ),
-        (
-            np.array([0.89442719, 0.0, 0.0, 0.4472136]),  # gibbs -> [0.0, 0.0, 1.0]
-            np.array([0.0, 0.0, 20.0]) / ((4.0 - 1.0) ** 2 * (1 + (4.0 / 3.0) ** 2)),
-        ),
-        (
-            np.array(
-                [0.92387953, 0.22094238, 0.22094238, 0.22094238]
-            ),  # gibbs -> [0.47829262, 0.47829262, 0.47829262]
-            np.array([0.06751864, 0.29609696, 0.87452584]),
-        ),
+        (0.0, 0.0, 0.0),
+        (0.0, 0.0, 35.0),
+        (30.0, 0.0, 0.0),
+        (0.0, 30.0, 0.0),
+        (25.0, -40.0, -125.0),
+        (-70.0, 60.0, 15.0),
     ],
 )
-def test__dhda(quaternion, dhda_expect):
+def test__yaw_gradient(angles):
+    """
+    The gradient is the bottom row of the Euler angle rate transformation matrix,
+    i.e. [0, sin(roll) / cos(pitch), cos(roll) / cos(pitch)].
+    """
+    roll, pitch, yaw = np.radians(angles)
+    quaternion = _quaternion_from_euler(roll, pitch, yaw)
+
+    dhda_expect = np.array(
+        [0.0, np.sin(roll) / np.cos(pitch), np.cos(roll) / np.cos(pitch)]
+    )
+
+    dhda_out = _common._yaw_gradient(quaternion)
+    np.testing.assert_allclose(dhda_out, dhda_expect, atol=1e-12)
+
+
+@pytest.mark.parametrize("seed", [0, 1, 2, 3, 4])
+def test__yaw_gradient_vs_finite_difference(seed):
+    """
+    The gradient must be taken wrt. the attitude error state as it is actually
+    applied, i.e. as a body-frame rotation via '_update_quaternion_with_gibbs2'.
+    """
+    rng = np.random.default_rng(seed)
+    quaternion = rng.normal(size=4)
+    quaternion /= np.linalg.norm(quaternion)
+
+    eps = 1e-7
+    dhda_expect = np.zeros(3)
+    for i in range(3):
+        da = np.zeros(3)
+        da[i] = eps
+
+        q_plus = quaternion.copy()
+        _common._update_quaternion_with_gibbs2(q_plus, da)
+
+        q_minus = quaternion.copy()
+        _common._update_quaternion_with_gibbs2(q_minus, -da)
+
+        dyaw = _common._yaw_from_quaternion(q_plus) - _common._yaw_from_quaternion(
+            q_minus
+        )
+        dhda_expect[i] = _common._signed_smallest_angle(dyaw) / (2.0 * eps)
+
     dhda_out = _common._yaw_gradient(quaternion)
-    np.testing.assert_allclose(dhda_out, dhda_expect)
+    np.testing.assert_allclose(dhda_out, dhda_expect, atol=1e-6)
 
 
 @pytest.mark.parametrize(