1import math
2
3class Filt2:
4   """ filtre du second ordre Yann """
5   def __init__(self, wn = 1.0, ksi = 1.0):
6      self.wn = wn
7      self.ksi = ksi
8      self.x = [0.0, 0.0, 0.0, 0.0 ]
9      self.xn = [0.0, 0.0 ]
10
11   def setParam(self,wn , ksi):
12      self.wn = wn
13      self.ksi = ksi
14
15   def init():
16      self.x = [0.0, 0.0, 0.0, 0.0 ]
17
18   def simulate(self,u, dt):
19      self.xn[0] = self.x[0] + (self.x[3] + self.x[1])*dt/2.0
20      self.xn[1] += (self.x[2] + self.wn * self.wn * (u - self.x[0]) - 2.0 * self.ksi * self.wn * self.x[1]) * dt / 2.0
21
22      self.x[2] = self.wn * self.wn * (u - self.x[0]) - 2.0 * self.ksi * self.wn * self.x[1]
23      self.x[3] = self.x[1]
24      self.x[0] = self.xn[0]
25      self.x[1] = self.xn[1]
26
27