Source code for utils.math

"""
Implementation of all math utility functions.
"""
import numpy as np
import numpy.typing as npt


def circular_mean(sigmas: npt.NDArray, Wm: npt.NDArray, normalize: bool = True) -> npt.NDArray:
    """
    Calculate weighted mean of angles.

    Since angles are periodic normal mean functions cannot be used,
    so this function implements one possibility to calculate circular means.

    Parameters
    ----------
    sigmas : npt.NDArray
        Sigma points for UKF.
    Wm : npt.NDArray
        Weights for the sigma points.

    Returns
    -------
    npt.NDArray
        Weighted mean.
    """
    sum_sin = np.sum(np.dot(np.sin(sigmas), Wm))
    sum_cos = np.sum(np.dot(np.cos(sigmas), Wm))
    result = np.arctan2(sum_sin, sum_cos)
    # somehow all weighted circular mean implementations make math errors, when mean is nearly zero.
    # Direction of vector on unit circle is inverted --> result is  shifted by 180 degrees / pi
    average = np.average(sigmas, axis=0, weights=Wm)
    if abs(normalize_rad_angle(result - average)) > 3:
        result = normalize_rad_angle(result + np.pi)
    if normalize is False:
        result = (np.round((average - result) / (2 * np.pi)) * 2 * np.pi) + result
    return result


def normalize_rad_angle(angle: npt.NDArray[np.floating]) -> npt.NDArray[np.floating]:
    """
    Normalize angle in unit [rad].

    Since angles are periodic, they have to be normalized.

    Parameters
    ----------
    angle : npt.NDArray[np.floating]
        Input angle.

    Returns
    -------
    npt.NDArray[np.floating]
        Normalized angle.

    See Also
    --------
    normalize_rad_angles: Normalize multiple angles in [rad] simultaneously.
    """
    angle = angle % (2 * np.pi)    # force in range [0, 2 pi)
    if angle > np.pi:          # move to [-pi, pi)
        angle -= 2 * np.pi
    return angle


def normalize_rad_angles(angles: npt.NDArray[np.floating]) -> npt.NDArray[np.floating]:
    """
    Normalize angles in unit [rad].

    Since angles are periodic, they have to be normalized.

    Parameters
    ----------
    angles : npt.NDArray[np.floating]
        Input angle.

    Returns
    -------
    npt.NDArray[np.floating]
        Normalized angles.
    """
    angles = np.mod(angles, 2*np.pi)
    angles[angles > np.pi] -= 2 * np.pi
    return angles


def normalize_degree_angle(angle: npt.NDArray[np.floating]) -> npt.NDArray[np.floating]:
    """
    Normalize angle in unit [degrees].

    Since angles are periodic, they have to be normalized.

    Parameters
    ----------
    angle : npt.NDArray[np.floating]
        Input angle.

    Returns
    -------
    npt.NDArray[np.floating]
        Normalized angle.
    """
    angle = angle % 360
    if angle > 180:
        angle -= 360
    return angle


def normalize_degree_angles(angles: npt.NDArray[np.floating]) -> npt.NDArray[np.floating]:
    """
    Normalize angles in unit [degrees].

    Since angles are periodic, they have to be normalized.

    Parameters
    ----------
    angles : npt.NDArray[np.floating]
        Input angle.

    Returns
    -------
    npt.NDArray[np.floating]
        Normalized angles.
    """
    angles = np.mod(angles, 360)
    angles[angles > 180] -= 360
    return angles