Last active
February 11, 2022 07:25
-
-
Save RubenKelevra/51b579aba48cb991af3b6098c1cedd40 to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
"""Calculate jerks in the recent history (sliding exponential average) from lateral acceleration data""" | |
from __future__ import annotations | |
import time | |
from collections import deque | |
from math import hypot | |
from numpy import ma, logspace | |
# number of samples to use for the sliding exponential average | |
_sample_size = 150 | |
# temporary storage for sliding exponential average | |
_last_sample_jerks = deque([]) | |
# the x/y acceleration data for the last sample | |
_last_sample = None | |
# last sample's timestamp | |
_last_timestamp = None | |
_exponential_weights = numpy.logspace(0, 1, _sample_size, base=_sample_size) | |
def process_lat_long_acceleration(x: float, y: float) -> float | None: | |
"""Process a single sample of lateral acceleration data | |
returns: None if _sample_size is not yet reached | |
""" | |
global _last_sample_jerks, _last_timestamp, _last_sample | |
# get current time | |
_timestamp = int(time.time()) | |
if _last_timestamp is not None: | |
# calculate time delta in ms | |
_delta_t = _timestamp - _last_timestamp | |
# calculate 2D jerk from last sample | |
_current_jerk = hypot(abs(_last_sample[0] - x) / _delta_t, abs(_last_sample[1] - y) / _delta_t) | |
# update last sample | |
_last_timestamp = _timestamp | |
_last_sample = (x, y) | |
# add current sample to the storage | |
if len(_last_sample_jerks) > 0: | |
if _current_jerk is None: | |
raise Exception("Could not calculate jerk") | |
_last_sample_jerks.append(_current_jerk) | |
else: | |
_last_sample_jerks.append(0.0) | |
if len(_last_sample_jerks) > _sample_size: | |
_last_sample_jerks.popleft() | |
else: | |
return None | |
# calculate the ewma over _last_sample_jerks | |
return float(ma.average(_last_sample_jerks, weights=_exponential_weights)) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment