85 lines
3 KiB
Python
85 lines
3 KiB
Python
|
''' Digital Biquadratic filter implementation '''
|
||
|
|
|
||
|
|
class BiquadFilter:
|
||
|
|
''' Implementation of a digital biquadratic filter. '''
|
||
|
|
def __init__(self, omega, q, dt, ftype):
|
||
|
|
''' Builds a biquad filter from the given parameters. '''
|
||
|
|
self.omega = omega
|
||
|
|
self.q = q
|
||
|
|
self.dt = dt
|
||
|
|
self.ftype = ftype
|
||
|
|
self.InitFilter(0)
|
||
|
|
self.ComputeContinuousTF(omega, q, dt, ftype)
|
||
|
|
self.ConvertContinuousToDiscrete()
|
||
|
|
|
||
|
|
|
||
|
|
def ComputeContinuousTF(self, omega, q, dt, ftype='lowpass'):
|
||
|
|
''' Computes the continuous time transfer function from the given parameters.
|
||
|
|
b0 + b1*s + b2*s^2
|
||
|
|
------------------
|
||
|
|
a0 + a1*s + a2*s^2
|
||
|
|
'''
|
||
|
|
if ftype == 'highpass':
|
||
|
|
self.b0c = 0
|
||
|
|
self.b1c = 0
|
||
|
|
self.b2c = 1
|
||
|
|
self.a0c = omega**2
|
||
|
|
self.a1c = omega/q
|
||
|
|
self.a2c = 1
|
||
|
|
elif ftype == 'bandpass':
|
||
|
|
self.b0c = 0
|
||
|
|
self.b1c = omega
|
||
|
|
self.b2c = 0
|
||
|
|
self.a0c = q*omega**2
|
||
|
|
self.a1c = omega
|
||
|
|
self.a2c = q
|
||
|
|
elif ftype == 'notch':
|
||
|
|
self.b0c = omega**2
|
||
|
|
self.b1c = 0
|
||
|
|
self.b2c = 1
|
||
|
|
self.a0c = omega**2
|
||
|
|
self.a1c = omega/q
|
||
|
|
self.a2c = 1
|
||
|
|
else:
|
||
|
|
# lowpass filter
|
||
|
|
self.b0c = omega**2
|
||
|
|
self.b1c = 0
|
||
|
|
self.b2c = 0
|
||
|
|
self.a0c = omega**2
|
||
|
|
self.a1c = omega/q
|
||
|
|
self.a2c = 1
|
||
|
|
|
||
|
|
def ConvertContinuousToDiscrete(self):
|
||
|
|
''' Converts the continuous coefficients to discrete coefficients using the bilinear transform. '''
|
||
|
|
self.b0d = 4*self.b2c - 2*self.b1c*self.dt + self.b0c*self.dt**2
|
||
|
|
self.b1d = - 8*self.b2c + 2*self.b0c*self.dt**2
|
||
|
|
self.b2d = 4*self.b2c + 2*self.b1c*self.dt + self.b0c*self.dt**2
|
||
|
|
self.a0d = 4*self.a2c - 2*self.a1c*self.dt + self.a0c*self.dt**2
|
||
|
|
self.a1d = - 8*self.a2c + 2*self.a0c*self.dt**2
|
||
|
|
self.a2d = 4*self.a2c + 2*self.a1c*self.dt + self.a0c*self.dt**2
|
||
|
|
|
||
|
|
def PrintContinuousTF(self):
|
||
|
|
''' Prints the continuous time transfer function coefficients. '''
|
||
|
|
print('%f + %f s + %f s**2' % (self.b0c, self.b1c, self.b2c))
|
||
|
|
print('----------------------------------------')
|
||
|
|
print('%f + %f s + %f s**2' % (self.a0c, self.a1c, self.a2c))
|
||
|
|
|
||
|
|
def InitFilter(self, v):
|
||
|
|
''' Initializes the filter with the value v. '''
|
||
|
|
self.xn_0 = v
|
||
|
|
self.xn_1 = v
|
||
|
|
self.xn_2 = v
|
||
|
|
self.yn_0 = v
|
||
|
|
self.yn_1 = v
|
||
|
|
self.yn_2 = v
|
||
|
|
|
||
|
|
def Filter(self, xn_0):
|
||
|
|
''' Applies the filter to the sample xn_0. '''
|
||
|
|
self.xn_0 = xn_0
|
||
|
|
self.yn_0 = (self.b2d*self.xn_0 + self.b1d*self.xn_1 + self.b0d*self.xn_2 - self.a1d*self.yn_1 - self.a0d*self.yn_2)/self.a2d
|
||
|
|
self.xn_2 = self.xn_1
|
||
|
|
self.xn_1 = self.xn_0
|
||
|
|
self.yn_2 = self.yn_1
|
||
|
|
self.yn_1 = self.yn_0
|
||
|
|
return self.yn_0
|