Reformulate compass aiding in terms of unit quaternions

[ace-e4s]

This PR is related to user story DLAB-

Description

Functions related to compass aiding (_dhda_head and _h_head) where expressed in terms of scaled Gibbs vectors. Numerically, this could create difficulties due to gibbs vector singularities.

Therefore, both functions where re-expressed in terms of unit quaternions.

The unit tests are updated, but extensive random testing can also be done with the code below:

import numpy as np
from smsfusion._ins import _dhda_head

def _dhda_head_old(a):
    """
    Compute yaw angle gradient wrt to the scaled Gibbs vector, see ref [1]_.

    Parameters
    ----------
    a : numpy.ndarray, shape (3,)
        Scaled Gibbs vector.

    Returns
    -------
    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.
    """
    a_x, a_y, a_z = a

    u_y = 2.0 * (a_x * a_y + 2.0 * a_z)
    u_x = 4.0 + a_x**2 - a_y**2 - a_z**2
    u = u_y / u_x

    duda_scale = 1.0 / (4.0 + a_x**2 - a_y**2 - a_z**2) ** 2
    duda_x = -2.0 * ((a_x**2 + a_z**2 - 4.0) * a_y + a_y**3 + 4.0 * a_x * a_z)
    duda_y = 2.0 * ((a_y**2 - a_z**2 + 4.0) * a_x + a_x**3 + 4.0 * a_y * a_z)
    duda_z = 4.0 * (a_z**2 + a_x * a_y * a_z + a_x**2 - a_y**2 + 4.0)
    duda = duda_scale * np.array([duda_x, duda_y, duda_z])

    dhda = 1.0 / (1.0 + np.sum(u**2)) * duda

    return dhda  # type: ignore[no-any-return]

theta_list = np.random.uniform(0, np.pi, 1000)
axis_list = np.random.uniform(-1, 1, (1000, 3))
axis_list = axis_list / np.linalg.norm(axis_list, axis=1)[:, np.newaxis]

gibbs = 2.0 * np.tan(theta_list / 2.0)[:, np.newaxis] * axis_list

for a in gibbs:
    q = 1.0 / np.sqrt(4.0 + a.dot(a)) * np.array([2.0, a[0], a[1], a[2]])

    dhda_new = _dhda_head(q)
    dhda_old = _dhda_head_old(a)
    assert np.allclose(dhda_new[:], dhda_old[:])

Checklist

• [x] PR title is descriptive and fit for injection into release notes (see tips below).
• [x] Correct label(s) are used.

PR title tips:

• Use imperative mood.
• Describe the motivation for change, issue that has been solved or what has been improved - not how.
• Examples:
  • Add functionality for Allan variance to smsfusion.simulate
  • Upgrade to support Python 3.10
  • Remove MacOS from CI