filter_utils.py 3.64 KB
Newer Older
1
from dynamic_graph.sot.core.filter_differentiator import FilterDifferentiator
2
3
4
5
6
7

def create_butter_lp_filter_Wn_05_N_2(name, dt, size):
    lp_filter = FilterDifferentiator(name);
    # from scipy.signal import butter
    # (b,a) = butter(N=2, Wn=0.05)
    # delay about 9*dt
8
    lp_filter.init(dt, size, (0.00554272,  0.01108543,  0.00554272), (1., -1.77863178,  0.80080265));
9
10
11
12
13
14
    return lp_filter;

def create_butter_lp_filter_Wn_05_N_3(name, dt, size):
    lp_filter = FilterDifferentiator(name);
    # (b,a) = butter(N=3, Wn=0.05)
    # delay about 13*dt
15
    lp_filter.init(dt, size, (0.00041655,  0.00124964,  0.00124964,  0.00041655), (1., -2.6861574 ,  2.41965511, -0.73016535));
16
17
18
19
20
21
    return lp_filter;

def create_butter_lp_filter_Wn_05_N_4(name, dt, size):
    lp_filter = FilterDifferentiator(name);
    # (b,a) = butter(N=4, Wn=0.05)
    # delay about 16*dt
22
23
    lp_filter.init(dt, size, (3.12389769e-05,   1.24955908e-04,   1.87433862e-04, 1.24955908e-04,   3.12389769e-05), 
                             (1., -3.58973389,  4.85127588, -2.92405266,  0.66301048));
24
25
26
27
28
29
    return lp_filter;

def create_butter_lp_filter_Wn_03_N_3(name, dt, size):
    lp_filter = FilterDifferentiator(name);
    # (b,a) = butter(N=3, Wn=0.03)
    # delay about 20*dt
30
    lp_filter.init(dt, size, (9.54425084e-05,   2.86327525e-04,   2.86327525e-04, 9.54425084e-05), (1., -2.81157368,  2.64048349, -0.82814628));
31
32
33
34
35
36
    return lp_filter;

def create_butter_lp_filter_Wn_03_N_4(name, dt, size):
    lp_filter = FilterDifferentiator(name);
    # (b,a) = butter(N=4, Wn=0.03)
    # delay about 28*dt
37
38
    lp_filter.init(dt, size, (4.37268880e-06,   1.74907552e-05,   2.62361328e-05, 1.74907552e-05,   4.37268880e-06), 
                             (1., -3.75376276,  5.29115258, -3.3189386 , 0.78161874));
39
40
41
42
43
44
    return lp_filter;

def create_chebi2_lp_filter_Wn_03_N_4(name, dt, size):
    lp_filter = FilterDifferentiator(name);
    # (b,a) = cheby2(4, 20, 0.03)
    # delay about 23*dt
45
46
    lp_filter.init(dt, size, (0.09147425, -0.35947268,  0.53605377, -0.35947268,  0.09147425), 
                             ( 1.        , -3.7862251 ,  5.38178322, -3.40348456,  0.80798333));
47
48
    return lp_filter;

49
def filter_series(b1, a1, b2, a2):
Rohan Budhiraja's avatar
Rohan Budhiraja committed
50
   import numpy as np    
51
52
53
54
55
56
57
58
59
60
61
   b = np.polymul(b1,b2)
   a = np.polymul(a1,a2)
   return b, a

def create_chebi1_checby2_series_filter(name, dt, size):
   #b1,a1=cheby2(2, 20,0.05);
   #b2,a2 = cheby1(4,0.05,0.08);
   #(b,a) = filter_series(b1,a1,b2,a2);
   lp_filter = FilterDifferentiator(name);
   
   lp_filter.init(dt, size, 
62
                  (2.16439898e-05, 4.43473520e-05, -1.74065002e-05, -8.02197247e-05,  -1.74065002e-05,   4.43473520e-05, 2.16439898e-05),
63
64
                  (1.,-5.32595322,11.89749109,-14.26803139, 9.68705647,  -3.52968633,   0.53914042))
   return lp_filter;
65
66
67
68

def mfreqz(fs, b, a=1, show_plot=True):
    import scipy.signal as signal
    from numpy import log10, pi, unwrap, arctan2, real, imag
69
    from pylab import subplot, plot, ylim, ylabel, xlabel, title, subplots_adjust, show, grid
70
71
72
    w,h = signal.freqz(b,a)
    f = w*fs/(2*max(w))
    h_dB = 20 * log10 (abs(h))
73
    ax1 = subplot(211)
74
75
76
77
78
    plot(f,abs(h))
    ylim(0, 1)
    ylabel('Magnitude')
    xlabel(r'Frequency (Hz)')
    title(r'Frequency response')
79
80
    grid(True)
    subplot(212, sharex=ax1)
81
82
83
84
85
86
    h_Phase = unwrap(arctan2(imag(h),real(h)))
    delay = h_Phase / (2*pi*f)
    plot(f,delay)
    ylabel('Delay (s)')
    xlabel(r'Frequency (Hz)')
    title(r'Phase delay')
87
    grid(True)
88
89
90
91
92
93
94
95
96
97
    subplots_adjust(hspace=0.5)
    if(show_plot):
      show()

def compare_with_savgol(fs, b, a):
    from scipy.signal import savgol_coeffs
    b_sg = savgol_coeffs(81, 2)
    mfreqz(fs,b_sg,1, False)
    mfreqz(fs,b,a)