tracinginsights commited on
Commit
164a632
1 Parent(s): 842269e

Create accelerations.py

Browse files
Files changed (1) hide show
  1. accelerations.py +151 -0
accelerations.py ADDED
@@ -0,0 +1,151 @@
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import math
2
+
3
+ import numpy as np
4
+
5
+
6
+ def smooth_derivative(t_in, v_in):
7
+ #
8
+ # Function to compute a smooth estimation of a derivative.
9
+ # [REF: http://holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/]
10
+ #
11
+
12
+ # Configuration
13
+ #
14
+ # Derivative method: two options: 'smooth' or 'centered'. Smooth is more conservative
15
+ # but helps to supress the very noisy signals. 'centered' is more agressive but more noisy
16
+ method = "smooth"
17
+
18
+ t = t_in.copy()
19
+ v = v_in.copy()
20
+
21
+ # (0) Prepare inputs
22
+
23
+ # (0.1) Time needs to be transformed to seconds
24
+ try:
25
+ for i in range(0, t.size):
26
+ t.iloc[i] = t.iloc[i].total_seconds()
27
+ except:
28
+ pass
29
+
30
+ t = np.array(t)
31
+ v = np.array(v)
32
+
33
+ # (0.1) Assert they have the same size
34
+ assert t.size == v.size
35
+
36
+ # (0.2) Initialize output
37
+ dvdt = np.zeros(t.size)
38
+
39
+ # (1) Manually compute points out of the stencil
40
+
41
+ # (1.1) First point
42
+ dvdt[0] = (v[1] - v[0]) / (t[1] - t[0])
43
+
44
+ # (1.2) Second point
45
+ dvdt[1] = (v[2] - v[0]) / (t[2] - t[0])
46
+
47
+ # (1.3) Third point
48
+ dvdt[2] = (v[3] - v[1]) / (t[3] - t[1])
49
+
50
+ # (1.4) Last points
51
+ n = t.size
52
+ dvdt[n - 1] = (v[n - 1] - v[n - 2]) / (t[n - 1] - t[n - 2])
53
+ dvdt[n - 2] = (v[n - 1] - v[n - 3]) / (t[n - 1] - t[n - 3])
54
+ dvdt[n - 3] = (v[n - 2] - v[n - 4]) / (t[n - 2] - t[n - 4])
55
+
56
+ # (2) Compute the rest of the points
57
+ if method == "smooth":
58
+ c = [5.0 / 32.0, 4.0 / 32.0, 1.0 / 32.0]
59
+ for i in range(3, t.size - 3):
60
+ for j in range(1, 4):
61
+ if (t[i + j] - t[i - j]) == 0:
62
+ dvdt[i] += 0
63
+ else:
64
+ dvdt[i] += (
65
+ 2 * j * c[j - 1] * (v[i + j] - v[i - j]) / (t[i + j] - t[i - j])
66
+ )
67
+ elif method == "centered":
68
+ for i in range(3, t.size - 2):
69
+ for j in range(1, 4):
70
+ if (t[i + j] - t[i - j]) == 0:
71
+ dvdt[i] += 0
72
+ else:
73
+
74
+ dvdt[i] = (v[i + 1] - v[i - 1]) / (t[i + 1] - t[i - 1])
75
+
76
+ return dvdt
77
+
78
+
79
+ def truncated_remainder(dividend, divisor):
80
+ divided_number = dividend / divisor
81
+ divided_number = (
82
+ -int(-divided_number) if divided_number < 0 else int(divided_number)
83
+ )
84
+
85
+ remainder = dividend - divisor * divided_number
86
+
87
+ return remainder
88
+
89
+
90
+ def transform_to_pipi(input_angle):
91
+ pi = math.pi
92
+ revolutions = int((input_angle + np.sign(input_angle) * pi) / (2 * pi))
93
+
94
+ p1 = truncated_remainder(input_angle + np.sign(input_angle) * pi, 2 * pi)
95
+ p2 = (
96
+ np.sign(
97
+ np.sign(input_angle)
98
+ + 2
99
+ * (
100
+ np.sign(
101
+ math.fabs(
102
+ (truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi)
103
+ )
104
+ )
105
+ - 1
106
+ )
107
+ )
108
+ ) * pi
109
+
110
+ output_angle = p1 - p2
111
+
112
+ return output_angle, revolutions
113
+
114
+
115
+ def remove_acceleration_outliers(acc):
116
+ acc_threshold_g = 7.5
117
+ if math.fabs(acc[0]) > acc_threshold_g:
118
+ acc[0] = 0.0
119
+
120
+ for i in range(1, acc.size - 1):
121
+ if math.fabs(acc[i]) > acc_threshold_g:
122
+ acc[i] = acc[i - 1]
123
+
124
+ if math.fabs(acc[-1]) > acc_threshold_g:
125
+ acc[-1] = acc[-2]
126
+
127
+ return acc
128
+
129
+
130
+ def compute_accelerations(telemetry):
131
+ v = np.array(telemetry["Speed"]) / 3.6
132
+ lon_acc = smooth_derivative(telemetry["Time"], v) / 9.81
133
+
134
+ dx = smooth_derivative(telemetry["Distance"], telemetry["X"])
135
+ dy = smooth_derivative(telemetry["Distance"], telemetry["Y"])
136
+
137
+ theta = np.zeros(dx.size)
138
+ theta[0] = math.atan2(dy[0], dx[0])
139
+ for i in range(0, dx.size):
140
+ theta[i] = (
141
+ theta[i - 1] + transform_to_pipi(math.atan2(dy[i], dx[i]) - theta[i - 1])[0]
142
+ )
143
+
144
+ kappa = smooth_derivative(telemetry["Distance"], theta)
145
+ lat_acc = v * v * kappa / 9.81
146
+
147
+ # Remove outliers
148
+ lon_acc = remove_acceleration_outliers(lon_acc)
149
+ lat_acc = remove_acceleration_outliers(lat_acc)
150
+
151
+ return np.round(lon_acc, 2), np.round(lat_acc, 2)