diff --git a/BiquadFilter.py b/BiquadFilter.py new file mode 100644 index 0000000..f7bd1c5 --- /dev/null +++ b/BiquadFilter.py @@ -0,0 +1,84 @@ +''' 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 diff --git a/test_biquad.py b/test_biquad.py new file mode 100644 index 0000000..54890ab --- /dev/null +++ b/test_biquad.py @@ -0,0 +1,44 @@ +# test of biquad filter +import numpy as np +from BiquadFilter import BiquadFilter +import matplotlib.pyplot as plt + +omega = 10 +q = 2 +dt = 0.01 + +tflow = BiquadFilter(omega, q, dt, "lowpass") +tfhigh = BiquadFilter(omega, q, dt, "highpass") +tfband = BiquadFilter(omega, q, dt, "bandpass") +tfnotch = BiquadFilter(omega, q, dt, "notch") + +tflow.PrintContinuousTF(); print('') +tfhigh.PrintContinuousTF(); print('') +tfband.PrintContinuousTF(); print('') +tfnotch.PrintContinuousTF(); print('') + +# simulate filter response +tend = 10 +Npts = int(np.ceil(tend/dt)) +Yl = np.zeros(Npts) +Yh = np.zeros(Npts) +Yband = np.zeros(Npts) +Ynotch = np.zeros(Npts) +T = np.zeros(Npts) + +for i in range(Npts): + t = i*dt + if t >= 1: + x = 1 + else: + x = 0 + + T[i] = t + Yl[i] = tflow.Filter(x) + Yh[i] = tfhigh.Filter(x) + Yband[i] = tfband.Filter(x) + Ynotch[i] = tfnotch.Filter(x) + +plt.plot(T, Yl, T, Yh, T, Yband, T, Ynotch) +plt.grid(True) +plt.show()