filter_design.py

# 20210205 for psd
from file_io import *
import matplotlib.pyplot as plt
from scipy import signal
from scipy.signal import butter, sosfilt, sosfreqz


def sin_wave(A, f, fs, phi, t):
    '''
    :params A:    振幅
    :params f:    信号频率
    :params fs:   采样频率
    :params phi:  相位
    :params t:    时间长度
    '''
    # 若时间序列长度为 t=1s,
    # 采样频率 fs=1000 Hz, 则采样时间间隔 Ts=1/fs=0.001s
    # 对于时间序列采样点个数为 n=t/Ts=1/0.001=1000, 即有1000个点,每个点间隔为 Ts
    Ts = 1/fs
    n = t / Ts
    n = np.arange(n)
    y = A*np.sin(2*np.pi*f*n*Ts + phi*(np.pi/180))
    return y


def butter_bandpass(lowcut, highcut, fs, order=5):
    nyq = 0.5 * fs
    low = lowcut / nyq
    high = highcut / nyq
    sos = butter(order, [low, high], analog=False, btype='band', output='sos')
    return sos

def butter_bandpass_filter(data, lowcut, highcut, fs, order=5):
        sos = butter_bandpass(lowcut, highcut, fs, order=order)
        y = sosfilt(sos, data)
        return y

[raw, hdr] = read_txt('src.txt', 1000)
raw = np.reshape(raw, (hdr['n_points']))
f, Pxx = signal.welch(raw, 1000, window='hamming', noverlap=1, nperseg=2048)


# f=50 hz
fs = 1000
hz_29 = sin_wave(A=1, f=29, fs=fs, phi=0, t=10)
hz_40 = sin_wave(A=2, f=40, fs=fs, phi=0, t=10)
hz_24 = sin_wave(A=0.8, f=24, fs=fs, phi=0, t=10)
hz_noise = hz_29 + hz_24 +hz_40
for i in range(10*1000):
    hz_noise[i] = hz_noise[i] + 0.3 * np.random.normal(0, 1)
# 滤波前psd
fig, ax = plt.subplots()
ax.plot(f[f < 150], Pxx[f < 150], linewidth=2)

# sample by sample filter
smoothWin = 50
farray = [0] * smoothWin
# farray = np.zeros(smoothWin)

fp1 = 28/500
fp2 = 30/500
fs1 = 26/500
fs2 = 32/500
fs = 1000
sos = signal.iirdesign([fp1,fp2], [fs1,fs2], gpass=0.5, gstop=10, ftype='cheby1',output='sos')
zi = signal.sosfilt_zi(sos)

def smooth_x(x):
    farray.pop(0)
    farray.append(x)
    sx = np.mean(farray)
    return sx


def filter(x):
    global zi
    # zf, dss = signal.lfilter(b, a, [x], zi=dss)
    # zr = np.abs(zf)
    # zs = smooth_x(np.abs(zr))
    # return zf, zr, zs
    y, zi = signal.sosfilt(sos,[x],zi = zi)
    yrectify = np.abs(y)
    ysmooth = smooth_x(yrectify)
    return y, yrectify, ysmooth



# 滤波
bp = butter_bandpass_filter(hz_noise,28,30,1000)

bp2 = []
for i in range(len(hz_noise)):
    a,b,c = filter(hz_noise[i])
    bp2.append(a)


f, Pxx = signal.welch(bp, 1000, window='hamming', noverlap=1, nperseg=2048)
fig, ax = plt.subplots()
ax.plot(f[f < 150], Pxx[f < 150], label='bad', linewidth=2)

# xinhao
fig, ax = plt.subplots(3, 1, sharex=True)
ax[0].plot(hz_noise)
ax[1].plot(bp)
ax[2].plot(bp2)
# ax[1].plot(t, mfm.memory['beta_filter'],label='filter')

plt.show()