Spaces:
Runtime error
Runtime error
File size: 1,419 Bytes
9667e74 |
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 |
import math
def get_alpha(rate=30, cutoff=1):
tau = 1 / (2 * math.pi * cutoff)
te = 1 / rate
return 1 / (1 + tau / te)
class LowPassFilter:
def __init__(self):
self.x_previous = None
def __call__(self, x, alpha=0.5):
if self.x_previous is None:
self.x_previous = x
return x
x_filtered = alpha * x + (1 - alpha) * self.x_previous
self.x_previous = x_filtered
return x_filtered
class OneEuroFilter:
def __init__(self, freq=15, mincutoff=1, beta=0.05, dcutoff=1):
self.freq = freq
self.mincutoff = mincutoff
self.beta = beta
self.dcutoff = dcutoff
self.filter_x = LowPassFilter()
self.filter_dx = LowPassFilter()
self.x_previous = None
self.dx = None
def __call__(self, x):
if self.dx is None:
self.dx = 0
else:
self.dx = (x - self.x_previous) * self.freq
dx_smoothed = self.filter_dx(self.dx, get_alpha(self.freq, self.dcutoff))
cutoff = self.mincutoff + self.beta * abs(dx_smoothed)
x_filtered = self.filter_x(x, get_alpha(self.freq, cutoff))
self.x_previous = x
return x_filtered
if __name__ == '__main__':
filter = OneEuroFilter(freq=15, beta=0.1)
for val in range(10):
x = val + (-1)**(val % 2)
x_filtered = filter(x)
print(x_filtered, x)
|