diff --git a/src/smsfusion/_ins/_common.py b/src/smsfusion/_ins/_common.py index 03710818..eb325d4d 100644 --- a/src/smsfusion/_ins/_common.py +++ b/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] diff --git a/tests/test_ins/test_common.py b/tests/test_ins/test_common.py index 236c7fbb..73be0995 100644 --- a/tests/test_ins/test_common.py +++ b/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(