Normalise an angle to be in the range [-pi, pi].

def normalise_angle(th):
    """Normalise an angle to be in the range [-pi, pi]."""
    return th - (2.0 * np.pi) * np.floor((th + np.pi) / (2.0 * np.pi))