ewma_over_jerk_history.py

"""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))