33 lines
948 B
Python
33 lines
948 B
Python
import numpy as np
|
|
|
|
|
|
def make_continuous_copy(alpha):
|
|
alpha = (alpha + np.pi) % (2.0 * np.pi) - np.pi
|
|
continuous_x = np.zeros_like(alpha)
|
|
continuous_x[0] = alpha[0]
|
|
for i in range(1, len(alpha)):
|
|
if not (np.sign(alpha[i]) == np.sign(alpha[i - 1])) and np.abs(alpha[i]) > np.pi / 2:
|
|
continuous_x[i] = continuous_x[i - 1] + (
|
|
alpha[i] - alpha[i - 1]) - np.sign(
|
|
(alpha[i] - alpha[i - 1])) * 2 * np.pi
|
|
else:
|
|
continuous_x[i] = continuous_x[i - 1] + (alpha[i] - alpha[i - 1])
|
|
|
|
return continuous_x
|
|
|
|
|
|
def derivative_of(x, dt=1, radian=False):
|
|
if radian:
|
|
x = make_continuous_copy(x)
|
|
|
|
not_nan_mask = ~np.isnan(x)
|
|
masked_x = x[not_nan_mask]
|
|
|
|
if masked_x.shape[-1] < 2:
|
|
return np.zeros_like(x)
|
|
|
|
dx = np.full_like(x, np.nan)
|
|
dx[not_nan_mask] = np.ediff1d(masked_x, to_begin=(masked_x[1] - masked_x[0])) / dt
|
|
|
|
return dx
|
|
|