Spaces:
Runtime error
Runtime error
File size: 4,121 Bytes
164a632 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 |
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)
|