diff --git a/src/lasp/filter/biquad.py b/src/lasp/filter/biquad.py index dfff947..1ce16f5 100644 --- a/src/lasp/filter/biquad.py +++ b/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): """