AkvoFluo/notch.py

from numpy import pi, cos, zeros

def notch(x, R, fn, dt, scale=True):
    '''Simple Pole and Zero Notch Filter. Note that this filter 
    is not zero phase, if you need a zero phase filter use 
    zpnotch.
    x                        # Input Sequence
    fn                       # Frequency to Filter 
    R                        # Pole and Zero locations
    dt                       # Sampling rate
    '''
    wn = 2. * pi * fn * dt    
    b0 = 1.
    b1 = -2.*cos(wn)
    b2 = 1.
    a1 = -R*2.*cos(wn)
    a2 = R*R

    if scale == True:
        sc = (1. + a1 + a2)/(b0 + b1 + b2)
        b0 *= sc
        b1 *= sc
        b2 *= sc

    xnm1 = 0.
    xnm2 = 0.
    ynm1 = 0.
    ynm2 = 0.
    y = zeros(len(x))
    for i in range(len(x)):
        y[i] = b0*x[i]+b1*xnm1+b2*xnm2-a1*ynm1-a2*ynm2
        xnm2 = xnm1
        xnm1 = x[i]
        ynm2 = ynm1
        ynm1 = y[i]

    return y