Added low-pass and high-pass compensator filters

src/lasp/filter/biquad.py

@@ -23,11 +23,11 @@ y[n] = 1/ba[3] * (  ba[0] * x[n]   + ba[1] * x[n-1] + ba[2] * x[n-2] +
 
 """
 __all__ = ['peaking', 'biquadTF', 'notch', 'lowpass', 'highpass',
-           'highshelf', 'lowshelf']
+           'highshelf', 'lowshelf', 'LPcompensator', 'HPcompensator']
 
 from numpy import array, cos, pi, sin, sqrt
 from scipy.interpolate import interp1d
-from scipy.signal import sosfreqz
+from scipy.signal import sosfreqz, bilinear_zpk, zpk2sos
 
 
 def peaking(fs, f0, Q, gain):
@@ -157,6 +157,79 @@ def lowshelf(fs, f0, Q, gain):
     a2 = (A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha
     return array([b0/a0, b1/a0, b2/a0, a0/a0, a1/a0, a2/a0])
 
+def LPcompensator(fs, f0o, Qo, f0n, Qn):
+    """
+    Shelving type filter that, when multiplied with a second-order low-pass 
+    filter, alters the response of that filter to a different second-order
+    low-pass filter.
+
+    Args:
+        fs: Sampling frequency [Hz]
+        f0o: Cut-off frequency of the original filter [Hz]
+        Qo: Quality factor of the original filter (~inverse of bandwidth)
+        f0n: Desired cut-off frequency [Hz]
+        Qn: Desired quality factor(~inverse of bandwidth)  
+    """
+    
+    omg0o = 2*pi*f0o
+    omg0n = 2*pi*f0n
+    
+    zRe = omg0o/(2*Qo)
+    zIm = omg0o*sqrt(1-1/(4*Qo**2))
+    z1 = -zRe + zIm*1j
+    z2 = -zRe - zIm*1j
+
+    pRe = omg0n/(2*Qn)
+    pIm = omg0n*sqrt(1-1/(4*Qn**2))
+    p1 = -pRe + pIm*1j
+    p2 = -pRe - pIm*1j
+
+    z= [z1, z2]
+    p = [p1, p2]
+    k = (pRe**2 + pIm**2)/(zRe**2 + zIm**2)
+    
+    zd, pd, kd = bilinear_zpk(z, p, k, fs)
+
+    sos = zpk2sos(zd,pd,kd)
+    
+    return sos[0]
+
+def HPcompensator(fs, f0o, Qo, f0n, Qn):
+    """
+    Shelving type filter that, when multiplied with a second-order high-pass
+    filter, alters the response of that filter to a different second-order 
+    high-pass filter.
+
+    Args:
+        fs: Sampling frequency [Hz]
+        f0o: Cut-on frequency of the original filter [Hz]
+        Qo: Quality factor of the original filter (~inverse of bandwidth)
+        f0n: Desired cut-on frequency [Hz]
+        Qn: Desired quality factor(~inverse of bandwidth)  
+    """
+    
+    omg0o = 2*pi*f0o
+    omg0n = 2*pi*f0n
+    
+    zRe = omg0o/(2*Qo)
+    zIm = omg0o*sqrt(1-1/(4*Qo**2))
+    z1 = -zRe + zIm*1j
+    z2 = -zRe - zIm*1j
+    
+    pRe = omg0n/(2*Qn)
+    pIm = omg0n*sqrt(1-1/(4*Qn**2))
+    p1 = -pRe + pIm*1j
+    p2 = -pRe - pIm*1j
+
+    z= [z1, z2]
+    p = [p1, p2]
+    k = 1
+    
+    zd, pd, kd = bilinear_zpk(z, p, k, fs)
+
+    sos = zpk2sos(zd,pd,kd)
+    
+    return sos[0]
 
 def biquadTF(fs, freq, sos):
     """