File size: 2,396 Bytes
a10ba7f | 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 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 | import numpy as np
import time
class LowPassFilter:
def __init__(self, alpha):
self.__set_alpha(alpha)
self.__y = None
def __set_alpha(self, alpha):
if not (0.0 <= alpha <= 1.0):
raise ValueError("alpha must be between 0.0 and 1.0")
self.__alpha = alpha
def __call__(self, value, timestamp=None, alpha=None):
if alpha is not None:
self.__set_alpha(alpha)
if self.__y is None:
self.__y = value
else:
self.__y = self.__alpha * value + (1.0 - self.__alpha) * self.__y
return self.__y
def last_value(self):
return self.__y
class OneEuroFilter:
"""
Adaptive low-pass filter for jitter suppression.
Based on: http://www.lifl.fr/~casiez/1euro/
"""
def __init__(self, freq, min_cutoff=1.0, beta=0.007, d_cutoff=1.0):
self.__freq = freq
self.__min_cutoff = min_cutoff
self.__beta = beta
self.__d_cutoff = d_cutoff
self.__x_filt = LowPassFilter(self.__alpha(min_cutoff))
self.__dx_filt = LowPassFilter(self.__alpha(d_cutoff))
self.__last_time = None
def __alpha(self, cutoff):
tau = 1.0 / (2 * np.pi * cutoff)
te = 1.0 / self.__freq
return 1.0 / (1.0 + tau / te)
def __call__(self, x, timestamp=None):
# Update frequency if timestamp is provided
if timestamp is not None and self.__last_time is not None:
self.__freq = 1.0 / (timestamp - self.__last_time)
self.__last_time = timestamp
# Estimate the derivative
prev_x = self.__x_filt.last_value()
dx = 0.0 if prev_x is None else (x - prev_x) * self.__freq
edx = self.__dx_filt(dx, alpha=self.__alpha(self.__d_cutoff))
# Compute the optimal cutoff frequency
cutoff = self.__min_cutoff + self.__beta * abs(edx)
# Filter the value
return self.__x_filt(x, alpha=self.__alpha(cutoff))
if __name__ == "__main__":
# Quick test
filter = OneEuroFilter(freq=30, min_cutoff=1.0, beta=0.01)
data = [10.0, 10.1, 10.5, 13.0, 15.0, 15.1]
timestamps = [0.0, 0.033, 0.066, 0.1, 0.133, 0.166]
print("Testing OneEuroFilter:")
for val, ts in zip(data, timestamps):
filtered = filter(val, timestamp=ts)
print(f"Input: {val:.2f}, Filtered: {filtered:.2f}")
|