mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-18 23:33:58 +08:00
23 lines
534 B
Python
23 lines
534 B
Python
class FirstOrderFilter:
|
|
def __init__(self, x0, rc, dt, initialized=True):
|
|
self.x = x0
|
|
self._dt = dt
|
|
self.update_alpha(rc)
|
|
self.initialized = initialized
|
|
|
|
def update_dt(self, dt):
|
|
self._dt = dt
|
|
self.update_alpha(self._rc)
|
|
|
|
def update_alpha(self, rc):
|
|
self._rc = rc
|
|
self._alpha = self._dt / (self._rc + self._dt)
|
|
|
|
def update(self, x):
|
|
if self.initialized:
|
|
self.x = (1. - self._alpha) * self.x + self._alpha * x
|
|
else:
|
|
self.initialized = True
|
|
self.x = x
|
|
return self.x
|