tracinginsights/api

1import math2 3import numpy as np4 5 6def 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    # Configuration13    #14    # Derivative method: two options: 'smooth' or 'centered'. Smooth is more conservative15    # but helps to supress the very noisy signals. 'centered' is more agressive but more noisy16    method = "smooth"17 18    t = t_in.copy()19    v = v_in.copy()20 21    # (0) Prepare inputs22 23    # (0.1) Time needs to be transformed to seconds24    try:25        for i in range(0, t.size):26            t.iloc[i] = t.iloc[i].total_seconds()27    except:28        pass29 30    t = np.array(t)31    v = np.array(v)32 33    # (0.1) Assert they have the same size34    assert t.size == v.size35 36    # (0.2) Initialize output37    dvdt = np.zeros(t.size)38 39    # (1) Manually compute points out of the stencil40 41    # (1.1) First point42    dvdt[0] = (v[1] - v[0]) / (t[1] - t[0])43 44    # (1.2) Second point45    dvdt[1] = (v[2] - v[0]) / (t[2] - t[0])46 47    # (1.3) Third point48    dvdt[2] = (v[3] - v[1]) / (t[3] - t[1])49 50    # (1.4) Last points51    n = t.size52    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 points57    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] += 063                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] += 072                else:73 74                    dvdt[i] = (v[i + 1] - v[i - 1]) / (t[i + 1] - t[i - 1])75 76    return dvdt77 78 79def truncated_remainder(dividend, divisor):80    divided_number = dividend / divisor81    divided_number = (82        -int(-divided_number) if divided_number < 0 else int(divided_number)83    )84 85    remainder = dividend - divisor * divided_number86 87    return remainder88 89 90def transform_to_pipi(input_angle):91    pi = math.pi92    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            + 299            * (100                np.sign(101                    math.fabs(102                        (truncated_remainder(input_angle + pi, 2 * pi)) / (2 * pi)103                    )104                )105                - 1106            )107        )108    ) * pi109 110    output_angle = p1 - p2111 112    return output_angle, revolutions113 114 115def remove_acceleration_outliers(acc):116    acc_threshold_g = 7.5117    if math.fabs(acc[0]) > acc_threshold_g:118        acc[0] = 0.0119 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 acc128 129 130def compute_accelerations(telemetry):131    v = np.array(telemetry["Speed"]) / 3.6132    lon_acc = smooth_derivative(telemetry["Time"], v) / 9.81133 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.81146 147    # Remove outliers148    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)152