mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-19 05:24:06 +08:00
51698f04bc
* Use first order filter in INDI
* use first order filter in LP as well
* Update selfdrive/controls/lib/lane_planner.py
Co-authored-by: Willem Melching <willem.melching@gmail.com>
* RC->rc
* division safe
Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: c900bce1b0
14 lines
312 B
Python
14 lines
312 B
Python
class FirstOrderFilter:
|
|
# first order filter
|
|
def __init__(self, x0, rc, dt):
|
|
self.x = x0
|
|
self.dt = dt
|
|
self.update_alpha(rc)
|
|
|
|
def update_alpha(self, rc):
|
|
self.alpha = self.dt / (rc + self.dt)
|
|
|
|
def update(self, x):
|
|
self.x = (1. - self.alpha) * self.x + self.alpha * x
|
|
return self.x
|