Biquadratic digital filter

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

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()