unwrap_theta.py

import numpy as np


def unwrap_theta(theta: float, prev_theta: float, range: float) -> float:
    """
    Description
    ------------
    unwrap_theta removes a discontinuous jump in theta by comparing it to prev_theta.
    If |theta - \prev_theta| < (range/2), then we don't do anything to theta, since it
    is within the correct range of prev_theta. Otherwise, if
    |theta - prev_theta| >= (range/2), then we subtract "range" from theta until
    |theta - \prev_theta| < (range/2).

    Inputs
    -------
    theta: the theta value that possibly contains a discontinuous jump relative to
    prev_theta.
    prev_theta: the previous theta value that we are comparing theta against.
    range: theta must be withing (range/2) of prev_theta.

    Outputs
    -------
    theta: the new theta value with discontinuous jumps removed.
    """
    theta_min: float = prev_theta - (range / 2)
    n: float = np.floor((theta - theta_min) / range)
    theta: float = theta - range * n
    return theta


# test
prev_theta = np.pi * (np.random.rand() * 2 - 1)  # [-pi,pi]
eps = 1e-3
k: int = 4
theta = prev_theta + k * np.pi + eps
theta_unwrapped = unwrap_theta(theta, prev_theta, np.pi)
np.testing.assert_allclose(actual=theta_unwrapped, desired=prev_theta + eps)