Attempt at Chamberlin DSVF

dsvf.py

# Python 3.10 required
import numpy as np
from numpy import pi, sin, tan
from typing import Optional
import matplotlib.pyplot as plt
import scipy.fft as fft

class FilterModes:
    DSVF_CHAMBERLIN = 'Chamberlin'
    DSVF_LAZZARINI_TIMONEY = 'Lazzarini-Timoney'

class DSVF:
    def __init__(self,
                 sample_rate: int = 48000,
                 Q: np.single = np.sqrt(2),
                 frequency: np.single = 440.0,
                 mode: str = FilterModes.DSVF_CHAMBERLIN):
        self._mode = mode
        self._fs = sample_rate
        self.set_state(Q=Q,
                       q = None,
                       frequency=frequency,
                       bandpass_z1=0.0,
                       lowpass_z1=0.0,
                       highpass=0.0,
                       lowpass=0.0,
                       bandpass=0.0)
    
    def _get_f(self, frequency: np.single):
        f = np.single(0)
        match self._mode:
            case FilterModes.DSVF_LAZZARINI_TIMONEY:
                # Needs a different f to deal with frequency warping
                f = np.single(tan(pi * frequency / self._fs))
            case _:
                f = np.single(2 * sin(pi * frequency / self._fs))
        return f
    
    def filter_sample(self, sample: Optional[np.single] = 0.0):
        # Follows the hardware implementation, rather than calculating
        # the transfer function.

        ##############################
        ## Generate Highpass output ##
        ##############################
        # Highpass output is sample - bandpass z^-1 * q - lowpass feedback
        highpass = sample - self._bandpass_z1 * self._q
        match self._mode:
            case FilterModes.DSVF_LAZZARINI_TIMONEY:
                # L-T DSVF uses a lowpass z^-1 with a feed-forward
                highpass -= self._lowpass_z1
            case _:
                highpass -= self._lowpass
        
        ##############################
        ## Generate Bandpass Output ##
        ##############################
        # Bandpass is bandpass feedback plus f*highpass
        f_highpass = highpass * self._f
        bandpass = self._bandpass_z1 + f_highpass
        bandpass_z1 = bandpass
        f_bandpass = None
        match self._mode:
            case FilterModes.DSVF_LAZZARINI_TIMONEY:
                # L-Z includes a feed-forward and doesn't delay the bandpass sample
                bandpass_z1 += f_highpass
                f_bandpass = bandpass * self._f
            case _:
                f_bandpass = self._bandpass_z1 * self._f
        
        #############################
        ## Generate Lowpass Output ##
        #############################
        # Lowpass filter is a straight feedback loop
        lowpass = f_bandpass + self._lowpass_z1
        # Replace state
        # Need the last cycle's z^-1
        lowpass_z1 = None
        match self._mode:
            case FilterModes.DSVF_LAZZARINI_TIMONEY:
                # Again, feed-forward incorporated
                lowpass_z1 = np.single(self._lowpass + self._f * self._bandpass)
            case _:
                # XXX: Is this correct?  Neither works
                lowpass_z1 = np.single(lowpass) #self._lowpass

        self._highpass = highpass
        self._bandpass = bandpass
        self._bandpass_z1 = bandpass_z1
        self._lowpass = lowpass
        self._lowpass_z1 = lowpass_z1
    
    def get_state(self):
        return { 'Lowpass':  self._lowpass,
                 'Bandpass': self._bandpass,
                 'Highpass': self._highpass,
                 'Bandpass z1': self._bandpass_z1,
                 'Lowpass z1': self._lowpass_z1
               }

    def set_state(self,
                   Q: Optional[np.single] = None,
                   q: Optional[np.single] = None,
                   frequency: Optional[np.single] = None,
                   bandpass_z1: Optional[np.single] = None,
                   lowpass_z1: Optional[np.single] = None,
                   highpass: Optional[np.single] = None,
                   bandpass: Optional[np.single] = None,
                   lowpass: Optional[np.single] = None):
        if Q is not None: self._q = np.single(1 / Q)
        if q is not None: self._q = np.single(q)
        if frequency is not None: self._f = self._get_f(frequency)
        if bandpass_z1 is not None: self._bandpass_z1 = np.single(bandpass_z1)
        if lowpass_z1 is not None: self._lowpass_z1 = np.single(lowpass_z1)
        if highpass is not None: self._highpass = np.single(highpass)
        if bandpass is not None: self._bandpass = np.single(bandpass)
        if lowpass is not None: self._lowpass = np.single(lowpass)

fs=int(48000)
fc=np.single(8000)
dsvFilter = DSVF(frequency=fc, sample_rate=fs, Q=np.single(1/np.sqrt(2)))#, mode=FilterModes.DSVF_LAZZARINI_TIMONEY)
#dsvFilter.set_state(q=0, bandpass_z1 = 1, lowpass_z1 = 0)

sin_wave = np.array([],dtype='single')

ff = range(25,20000,250)#[440, 880, 4000, 5000, 6000, 8000]
mix_wave = np.array([], dtype='single')
for i in range(0,fs):
    mix = np.single(0)
    for j in ff:
        mix += np.single(sin(2*pi*i*j/fs))
    dsvFilter.filter_sample(mix)
    s = dsvFilter.get_state()
    #s16 = np.int16(0.999*s['Lowpass z1'] * 0.5 * (2**16-1))
    #s16 = np.int16(np.floor(s['Lowpass z1']))
    sin_wave = np.append(sin_wave, s['Lowpass z1'])
    mix_wave = np.append(mix_wave, mix)

for w in [mix_wave, sin_wave]:
    plt.plot(range(0,fs),w)
    plt.show()
    yf = fft.fft(w)
    #xf = [x / f0 for x in fft.fftfreq(np.uint(fs)*2,1/fs)]
    xf = fft.fftfreq(fs, 1/fs)
    plt.plot(xf, np.abs(yf))
    plt.show()