api / accelerations.py
tracinginsights's picture
Create accelerations.py
164a632
import math
import numpy as np
def smooth_derivative(t_in, v_in):
#
# Function to compute a smooth estimation of a derivative.
# [REF: http://holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/]
#
# Configuration
#
# Derivative method: two options: 'smooth' or 'centered'. Smooth is more conservative
# but helps to supress the very noisy signals. 'centered' is more agressive but more noisy
method = "smooth"
t = t_in.copy()
v = v_in.copy()
# (0) Prepare inputs
# (0.1) Time needs to be transformed to seconds
try:
for i in range(0, t.size):
t.iloc[i] = t.iloc[i].total_seconds()
except:
pass
t = np.array(t)
v = np.array(v)
# (0.1) Assert they have the same size
assert t.size == v.size
# (0.2) Initialize output
dvdt = np.zeros(t.size)
# (1) Manually compute points out of the stencil
# (1.1) First point
dvdt[0] = (v[1] - v[0]) / (t[1] - t[0])
# (1.2) Second point
dvdt[1] = (v[2] - v[0]) / (t[2] - t[0])
# (1.3) Third point
dvdt[2] = (v[3] - v[1]) / (t[3] - t[1])
# (1.4) Last points
n = t.size
dvdt[n - 1] = (v[n - 1] - v[n - 2]) / (t[n - 1] - t[n - 2])
dvdt[n - 2] = (v[n - 1] - v[n - 3]) / (t[n - 1] - t[n - 3])
dvdt[n - 3] = (v[n - 2] - v[n - 4]) / (t[n - 2] - t[n - 4])
# (2) Compute the rest of the points
if method == "smooth":
c = [5.0 / 32.0, 4.0 / 32.0, 1.0 / 32.0]
for i in range(3, t.size - 3):
for j in range(1, 4):
if (t[i + j] - t[i - j]) == 0:
dvdt[i] += 0
else:
dvdt[i] += (
2 * j * c[j - 1] * (v[i + j] - v[i - j]) / (t[i + j] - t[i - j])
)
elif method == "centered":
for i in range(3, t.size - 2):
for j in range(1, 4):
if (t[i + j] - t[i - j]) == 0:
dvdt[i] += 0
else:
dvdt[i] = (v[i + 1] - v[i - 1]) / (t[i + 1] - t[i - 1])
return dvdt
def truncated_remainder(dividend, divisor):
divided_number = dividend / divisor
divided_number = (
-int(-divided_number) if divided_number < 0 else int(divided_number)
)
remainder = dividend - divisor * divided_number
return remainder
def transform_to_pipi(input_angle):
pi = math.pi
revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi))
p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi)
p2 = (
np.sign(
np.sign(input_angle)
+ 2
* (
np.sign(
math.fabs(
(truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi)
)
)
- 1
)
)
) * pi
output_angle = p1 - p2
return output_angle, revolutions
def remove_acceleration_outliers(acc):
acc_threshold_g = 7.5
if math.fabs(acc[0]) > acc_threshold_g:
acc[0] = 0.0
for i in range(1, acc.size - 1):
if math.fabs(acc[i]) > acc_threshold_g:
acc[i] = acc[i - 1]
if math.fabs(acc[-1]) > acc_threshold_g:
acc[-1] = acc[-2]
return acc
def compute_accelerations(telemetry):
v = np.array(telemetry["Speed"]) / 3.6
lon_acc = smooth_derivative(telemetry["Time"], v) / 9.81
dx = smooth_derivative(telemetry["Distance"], telemetry["X"])
dy = smooth_derivative(telemetry["Distance"], telemetry["Y"])
theta = np.zeros(dx.size)
theta[0] = math.atan2(dy[0], dx[0])
for i in range(0, dx.size):
theta[i] = (
theta[i - 1] + transform_to_pipi(math.atan2(dy[i], dx[i]) - theta[i - 1])[0]
)
kappa = smooth_derivative(telemetry["Distance"], theta)
lat_acc = v * v * kappa / 9.81
# Remove outliers
lon_acc = remove_acceleration_outliers(lon_acc)
lat_acc = remove_acceleration_outliers(lat_acc)
return np.round(lon_acc, 2), np.round(lat_acc, 2)