visiongraph.dsp.OneEuroFilter
1import math 2from time import time 3from typing import Optional 4 5 6def _smoothing_factor(t_e, cutoff): 7 """ 8 Calculates the smoothing factor used in OneEuro filter. 9 10 :param t_e: Time elapsed since last measurement. 11 :param cutoff: Minimum cutoff frequency. 12 13 :return: Smoothing factor. 14 """ 15 r = 2 * math.pi * cutoff * t_e 16 return r / (r + 1) 17 18 19def _exponential_smoothing(a, x, x_prev): 20 """ 21 Applies exponential smoothing to a signal. 22 23 :param a: Smoothing factor. 24 :param x: New measurement value. 25 :param x_prev: Previous measurement value. 26 27 :return: Smoothed measurement value. 28 """ 29 return a * x + (1 - a) * x_prev 30 31 32class OneEuroFilter: 33 """ 34 A class to implement the OneEuro filter. This filter is used in many control systems, 35 especially those involving velocity sensors or accelerometers. 36 37 https://github.com/casiez/OneEuroFilter 38 """ 39 40 def __init__( 41 self, 42 x0: float, 43 t0: Optional[float] = None, 44 dx0: float = 0.0, 45 min_cutoff: float = 1.0, 46 beta: float = 0.0, 47 d_cutoff: float = 1.0, 48 ): 49 """ 50 Initializes the OneEuro filter. 51 52 :param x0: Initial measurement value. 53 :param t0: Initial time value. If None, uses current time. 54 :param dx0: Initial derivative of the signal. 55 :param min_cutoff: Minimum cutoff frequency. 56 :param beta: Parameter used in the cutoff calculation. 57 :param d_cutoff: Minimum derivative cutoff frequency. 58 """ 59 # The parameters. 60 self.min_cutoff = float(min_cutoff) 61 self.beta = float(beta) 62 self.d_cutoff = float(d_cutoff) 63 # Previous values. 64 self.x_prev = float(x0) 65 self.dx_prev = float(dx0) 66 self.t_prev = time() if t0 is None else t0 67 68 def __call__(self, x: float, t: Optional[float] = None) -> float: 69 """ 70 Computes the filtered signal. 71 72 :param x: New measurement value. 73 :param t: Time of new measurement. If None, uses current time. 74 75 :return: Filtered measurement value. 76 """ 77 if t is None: 78 t = time() 79 80 # The time elapsed since last measurement. 81 t_e = t - self.t_prev 82 83 # The filtered derivative of the signal. 84 a_d = _smoothing_factor(t_e, self.d_cutoff) 85 dx = (x - self.x_prev) / t_e 86 dx_hat = _exponential_smoothing(a_d, dx, self.dx_prev) 87 88 # The filtered signal. 89 cutoff = self.min_cutoff + self.beta * abs(dx_hat) 90 a = _smoothing_factor(t_e, cutoff) 91 x_hat = _exponential_smoothing(a, x, self.x_prev) 92 93 # Memorize the previous values. 94 self.x_prev = x_hat 95 self.dx_prev = dx_hat 96 self.t_prev = t 97 98 return x_hat
class
OneEuroFilter:
33class OneEuroFilter: 34 """ 35 A class to implement the OneEuro filter. This filter is used in many control systems, 36 especially those involving velocity sensors or accelerometers. 37 38 https://github.com/casiez/OneEuroFilter 39 """ 40 41 def __init__( 42 self, 43 x0: float, 44 t0: Optional[float] = None, 45 dx0: float = 0.0, 46 min_cutoff: float = 1.0, 47 beta: float = 0.0, 48 d_cutoff: float = 1.0, 49 ): 50 """ 51 Initializes the OneEuro filter. 52 53 :param x0: Initial measurement value. 54 :param t0: Initial time value. If None, uses current time. 55 :param dx0: Initial derivative of the signal. 56 :param min_cutoff: Minimum cutoff frequency. 57 :param beta: Parameter used in the cutoff calculation. 58 :param d_cutoff: Minimum derivative cutoff frequency. 59 """ 60 # The parameters. 61 self.min_cutoff = float(min_cutoff) 62 self.beta = float(beta) 63 self.d_cutoff = float(d_cutoff) 64 # Previous values. 65 self.x_prev = float(x0) 66 self.dx_prev = float(dx0) 67 self.t_prev = time() if t0 is None else t0 68 69 def __call__(self, x: float, t: Optional[float] = None) -> float: 70 """ 71 Computes the filtered signal. 72 73 :param x: New measurement value. 74 :param t: Time of new measurement. If None, uses current time. 75 76 :return: Filtered measurement value. 77 """ 78 if t is None: 79 t = time() 80 81 # The time elapsed since last measurement. 82 t_e = t - self.t_prev 83 84 # The filtered derivative of the signal. 85 a_d = _smoothing_factor(t_e, self.d_cutoff) 86 dx = (x - self.x_prev) / t_e 87 dx_hat = _exponential_smoothing(a_d, dx, self.dx_prev) 88 89 # The filtered signal. 90 cutoff = self.min_cutoff + self.beta * abs(dx_hat) 91 a = _smoothing_factor(t_e, cutoff) 92 x_hat = _exponential_smoothing(a, x, self.x_prev) 93 94 # Memorize the previous values. 95 self.x_prev = x_hat 96 self.dx_prev = dx_hat 97 self.t_prev = t 98 99 return x_hat
A class to implement the OneEuro filter. This filter is used in many control systems, especially those involving velocity sensors or accelerometers.
OneEuroFilter( x0: float, t0: Optional[float] = None, dx0: float = 0.0, min_cutoff: float = 1.0, beta: float = 0.0, d_cutoff: float = 1.0)
41 def __init__( 42 self, 43 x0: float, 44 t0: Optional[float] = None, 45 dx0: float = 0.0, 46 min_cutoff: float = 1.0, 47 beta: float = 0.0, 48 d_cutoff: float = 1.0, 49 ): 50 """ 51 Initializes the OneEuro filter. 52 53 :param x0: Initial measurement value. 54 :param t0: Initial time value. If None, uses current time. 55 :param dx0: Initial derivative of the signal. 56 :param min_cutoff: Minimum cutoff frequency. 57 :param beta: Parameter used in the cutoff calculation. 58 :param d_cutoff: Minimum derivative cutoff frequency. 59 """ 60 # The parameters. 61 self.min_cutoff = float(min_cutoff) 62 self.beta = float(beta) 63 self.d_cutoff = float(d_cutoff) 64 # Previous values. 65 self.x_prev = float(x0) 66 self.dx_prev = float(dx0) 67 self.t_prev = time() if t0 is None else t0
Initializes the OneEuro filter.
Parameters
- x0: Initial measurement value.
- t0: Initial time value. If None, uses current time.
- dx0: Initial derivative of the signal.
- min_cutoff: Minimum cutoff frequency.
- beta: Parameter used in the cutoff calculation.
- d_cutoff: Minimum derivative cutoff frequency.