def coef_filtro(tipo, C, R1, R2, Ts):
""" Retorna una lista con los coeficientes del filtro según sus parámetros.
Parámetros
----------
tipo : str
Tipo de filtro que cuyos parámetros se desean obtener.
C : float
Valor del capacitor en el filtro.
R1 : float
Valor de la resistencia del filtro.
R2 : float
Valor de la resistencia del filtro.
Ts : float
Período de muestreo de la señal a filtrar en segundos.
Retorna
-------
signal.bilinear() : list de narray.
Lista con los coeficientes del numerador y denominador.
"""
if tipo == 'rc':
return signal.bilinear([1], [R1 * C, 1], 1 / Ts)
elif tipo == 'lead-lag pasivo':
return signal.bilinear([R2 * C, 1], [(R1 + R2) * C, 1], 1 / Ts)
elif tipo == 'lead-lag activo':
return signal.bilinear([R2 * C, 1], [R1 * C, 0], 1 / Ts)
else:
return 0
def prepare_video_filters(): # TODO: test these CLV+innerCAV parameters. Should be same on PAL+NTSC t1 = 25 t2 = 13.75 [tf_b, tf_a] = sps.zpk2tf(-t2*(10**-8), -t1*(10**-8), t1 / t2) SP['f_emp'] = sps.bilinear(tf_b, tf_a, 1/(freq_hz/2)) # RF BPF and analog audio cut filters SP['f_videorf_bpf'] = sps.butter(1, [SP['vbpf'][0]/(freq_hz/2), SP['vbpf'][1]/(freq_hz/2)], btype='bandpass') if SP['analog_audio'] == True: SP['f_aleft_stop'] = sps.butter(1, [(SP['audio_lfreq'] - 750000)/(freq_hz/2), (SP['audio_lfreq'] + 750000)/(freq_hz/2)], btype='bandstop') SP['f_aright_stop'] = sps.butter(1, [(SP['audio_rfreq'] - 750000)/(freq_hz/2), (SP['audio_rfreq'] + 750000)/(freq_hz/2)], btype='bandstop') # standard post-demod LPF lowpass_filter_b, lowpass_filter_a = sps.butter(SP['vlpf_order'], SP['vlpf_freq']/(freq_hz/2), 'low') # post-demod deemphasis filter [tf_b, tf_a] = sps.zpk2tf(-SP['deemp'][1]*(10**-8), -SP['deemp'][0]*(10**-8), SP['deemp'][0] / SP['deemp'][1]) SP['f_deemp'] = sps.bilinear(tf_b, tf_a, 1/(freq_hz/2)) # if AC3: #SP['f_arightcut'] = sps.butter(1, [(2650000)/(freq_hz/2), (3150000)/(freq_hz/2)], btype='bandstop') # prepare for FFT: convert above filters to FIR using FDLS techniques first forder = 256 forderd = 0 [Bbpf_FDLS, Abpf_FDLS] = fdls.FDLS_fromfilt(SP['f_videorf_bpf'][0], SP['f_videorf_bpf'][1], forder, forderd, 0, phasemult = 1.00) [Bemp_FDLS, Aemp_FDLS] = fdls.FDLS_fromfilt(SP['f_emp'][0], SP['f_emp'][1], forder, forderd, 0) [Bdeemp_FDLS, Adeemp_FDLS] = fdls.FDLS_fromfilt(SP['f_deemp'][0], SP['f_deemp'][1], forder, forderd, 0) [Bplpf_FDLS, Aplpf_FDLS] = fdls.FDLS_fromfilt(lowpass_filter_b, lowpass_filter_a, forder, forderd, 0) Fbpf = np.fft.fft(Bbpf_FDLS, blocklen) Femp = np.fft.fft(Bemp_FDLS, blocklen) SP['fft_video'] = Fbpf * fft_hilbert if SP['analog_audio'] == True: [Bcutl_FDLS, Acutl_FDLS] = fdls.FDLS_fromfilt(SP['f_aleft_stop'][0], SP['f_aleft_stop'][1], forder, forderd, 0, phasemult = 1.00) [Bcutr_FDLS, Acutr_FDLS] = fdls.FDLS_fromfilt(SP['f_aright_stop'][0], SP['f_aright_stop'][1], forder, forderd, 0, phasemult = 1.00) Fcutl = np.fft.fft(Bcutl_FDLS, blocklen) Fcutr = np.fft.fft(Bcutr_FDLS, blocklen) SP['fft_video'] *= (Fcutl * Fcutr) SP['fft_video_inner'] = SP['fft_video'] * Femp # Post processing: lowpass filter + deemp Fplpf = np.fft.fft(Bplpf_FDLS, blocklen) Fdeemp = np.fft.fft(Bdeemp_FDLS, blocklen) SP['fft_post'] = Fplpf * Fdeemp # determine freq offset and mult for output stage hz_ire_scale = (SP['videorf_100ire'] - SP['videorf_0ire']) / 100 minn = SP['videorf_0ire'] + (hz_ire_scale * -60) SP['output_minfreq'] = sminn = minn / (freq_hz / tau) out_scale = 65534.0 / (SP['ire_max'] - SP['ire_min']) SP['output_scale'] = (freq_hz / tau) * (out_scale / hz_ire_scale)
def __init__(self):
'''Parameters Inicialization '''
self.rospy = rospy
self.aux_cmd_vel_topic = self.rospy.get_param("aux_cmd_vel_topic",
"/usr_cmd_vel")
self.frc_topic = self.rospy.get_param("frc_topic", "/frc")
self.acontroller_rate = self.rospy.get_param("acontroller_rate", 30)
# m = 4
self.controller_params = {
"m": self.rospy.get_param("mass", 0.1),
"b_l": self.rospy.get_param("lineal_damping_c", 20),
"k_l": self.rospy.get_param("lineal_stiffness_c", 11),
"j": self.rospy.get_param("inertia", 1),
"b_a": self.rospy.get_param("angular_damping_c", 15),
"k_a": self.rospy.get_param("angular_stiffness_c", 0.7),
"Ts": self.rospy.get_param("Ts", 1.0 / self.acontroller_rate)
}
'''Subscribers'''
self.sub_frc = self.rospy.Subscriber(self.frc_topic, Wrench,
self.callback_frc)
'''Publishers'''
self.pub_aux_cmd_vel = self.rospy.Publisher(self.aux_cmd_vel_topic,
Twist,
queue_size=10)
'''Node Configuration'''
self.rospy.init_node("Admitance_Controller", anonymous=True)
self.rate = self.rospy.Rate(self.acontroller_rate)
self.vel = Twist()
self.frc = 0
self.trq = 0
lnum = np.array([1, 0])
lden = [
self.controller_params["m"], self.controller_params["b_l"],
self.controller_params["k_l"]
]
anum = np.array([1, 0])
aden = [
self.controller_params["j"], self.controller_params["b_a"],
self.controller_params["k_a"]
]
[lnum_d, lden_d] = sg.bilinear(lnum, lden, self.acontroller_rate)
[anum_d, aden_d] = sg.bilinear(anum, aden, self.acontroller_rate)
self.systems = {
"linear":
sg.TransferFunction(lnum_d,
lden_d,
dt=self.controller_params["Ts"]),
"angular":
sg.TransferFunction(anum_d,
aden_d,
dt=self.controller_params["Ts"])
}
self.change = self.enabled = False
self.signal_in = {
"frc": [],
"trq": [],
}
self.main_controller()
def prepare_video_filters(SP):
# TODO: test these CLV+innerCAV parameters. Should be same on PAL+NTSC
t1 = 25
t2 = 13.75
[tf_b, tf_a] = sps.zpk2tf(-t2 * (10 ** -8), -t1 * (10 ** -8), t1 / t2)
Femp = filtfft(sps.bilinear(tf_b, tf_a, 1 / (freq_hz / 2)))
# RF BPF and analog audio cut filters
Fbpf = filtfft(sps.butter(1, [SP["vbpf"][0] / (freq_hz / 2), SP["vbpf"][1] / (freq_hz / 2)], btype="bandpass"))
# standard post-demod LPF
Fplpf = filtfft(sps.butter(SP["vlpf_order"], SP["vlpf_freq"] / (freq_hz / 2), "low"))
# post-demod deemphasis filter
[tf_b, tf_a] = sps.zpk2tf(
-SP["deemp"][1] * (10 ** -8), -SP["deemp"][0] * (10 ** -8), SP["deemp"][0] / SP["deemp"][1]
)
Fdeemp = filtfft(sps.bilinear(tf_b, tf_a, 1.0 / (freq_hz / 2)))
SP["fft_video"] = Fbpf * fft_hilbert
if SP["analog_audio"] == True:
Fcutl = filtfft(
sps.butter(
1,
[(SP["audio_lfreq"] - 750000) / (freq_hz / 2), (SP["audio_lfreq"] + 750000) / (freq_hz / 2)],
btype="bandstop",
)
)
Fcutr = filtfft(
sps.butter(
1,
[(SP["audio_rfreq"] - 750000) / (freq_hz / 2), (SP["audio_rfreq"] + 750000) / (freq_hz / 2)],
btype="bandstop",
)
)
# if AC3:
# SP['f_arightcut'] = sps.butter(1, [(2650000)/(freq_hz/2), (3150000)/(freq_hz/2)], btype='bandstop')
SP["fft_video"] *= Fcutl * Fcutr
SP["fft_video_inner"] = SP["fft_video"] * Femp
# Post processing: lowpass filter + deemp
SP["fft_post"] = Fplpf * Fdeemp
# determine freq offset and mult for output stage
hz_ire_scale = (SP["videorf_100ire"] - SP["videorf_0ire"]) / 100
minn = SP["videorf_0ire"] + (hz_ire_scale * -60)
SP["output_minfreq"] = sminn = minn / (freq_hz / tau)
out_scale = 65534.0 / (SP["ire_max"] - SP["ire_min"])
SP["output_scale"] = (freq_hz / tau) * (out_scale / hz_ire_scale)
def prepare_video_filters(SP):
# TODO: test these CLV+innerCAV parameters. Should be same on PAL+NTSC
t1 = 25
t2 = 13.75
[tf_b, tf_a] = sps.zpk2tf(-t2 * (10**-8), -t1 * (10**-8), t1 / t2)
Femp = filtfft(sps.bilinear(tf_b, tf_a, 1 / (freq_hz / 2)))
# RF BPF and analog audio cut filters
Fbpf = filtfft(
sps.butter(
1, [SP['vbpf'][0] / (freq_hz / 2), SP['vbpf'][1] / (freq_hz / 2)],
btype='bandpass'))
# standard post-demod LPF
Fplpf = filtfft(
sps.butter(SP['vlpf_order'], SP['vlpf_freq'] / (freq_hz / 2), 'low'))
# post-demod deemphasis filter
[tf_b,
tf_a] = sps.zpk2tf(-SP['deemp'][1] * (10**-8), -SP['deemp'][0] * (10**-8),
SP['deemp'][0] / SP['deemp'][1])
Fdeemp = filtfft(sps.bilinear(tf_b, tf_a, 1.0 / (freq_hz / 2)))
SP['fft_video'] = Fbpf * fft_hilbert
if SP['analog_audio'] == True:
Fcutl = filtfft(
sps.butter(1, [(SP['audio_lfreq'] - 750000) / (freq_hz / 2),
(SP['audio_lfreq'] + 750000) / (freq_hz / 2)],
btype='bandstop'))
Fcutr = filtfft(
sps.butter(1, [(SP['audio_rfreq'] - 750000) / (freq_hz / 2),
(SP['audio_rfreq'] + 750000) / (freq_hz / 2)],
btype='bandstop'))
# if AC3:
#SP['f_arightcut'] = sps.butter(1, [(2650000)/(freq_hz/2), (3150000)/(freq_hz/2)], btype='bandstop')
SP['fft_video'] *= (Fcutl * Fcutr)
SP['fft_video_inner'] = SP['fft_video'] * Femp
# Post processing: lowpass filter + deemp
SP['fft_post'] = Fplpf * Fdeemp
# determine freq offset and mult for output stage
hz_ire_scale = (SP['videorf_100ire'] - SP['videorf_0ire']) / 100
minn = SP['videorf_0ire'] + (hz_ire_scale * -60)
SP['output_minfreq'] = sminn = minn / (freq_hz / tau)
out_scale = 65534.0 / (SP['ire_max'] - SP['ire_min'])
SP['output_scale'] = (freq_hz / tau) * (out_scale / hz_ire_scale)
def __init__(self, Fs, method='Wk'):
'method is always "Wk"'
Hhb = [1, 0, 0]
Hha = [1, 3.55430635052669, 6.31654681669719]
Hlb = [394784.176043574]
Hla = [1, 888.576587631673, 394784.176043574]
Htb = [0.0127323954473516, 1]
Hta = [0.000162113893827740, 0.0202101515037327, 1]
Hsb = [1, 16.3639001956216, 221.746323841915]
Hsa = [1, 23.1304074495073, 443.046541564901]
self._Hh = ss.bilinear(Hhb, Hha, fs=Fs)
self._Hl = ss.bilinear(Hlb, Hla, fs=Fs)
self._Ht = ss.bilinear(Htb, Hta, fs=Fs)
self._Hs = ss.bilinear(Hsb, Hsa, fs=Fs)
def c_weighting(fs=48000):
"""
Digital C-Weighting Filter Design
Parameters
----------
fs : int
sampling frequency
Returns
-------
z : ndarray
Numerator of the transformed digital filter transfer function.
p : ndarray
Denominator of the transformed digital filter transfer function.
"""
f1 = 20.598997
f4 = 12194.217
C1000 = 0.0619
np.pi = 3.14159265358979
NUMs = np.array([(2 * np.pi * f4)**2 * (10**(C1000 / 20)), 0, 0])
DENs = np.convolve(
[1, 4 * np.pi * f4, (2 * np.pi * f4)**2],
[1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
return signal.bilinear(NUMs, DENs, fs)
def a_weighting(fs=48000):
"""Digital A-Weighting Filter Design
Parameters
----------
fs : int
sampling frequency
Returns
-------
z : ndarray
Numerator of the transformed digital filter transfer function.
p : ndarray
Denominator of the transformed digital filter transfer function.
"""
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = np.array([(2 * np.pi * f4)**2 * (10**(A1000 / 20)), 0, 0, 0, 0])
DENs = np.convolve(
[1, 4 * np.pi * f4, (2 * np.pi * f4)**2],
[1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
DENs = np.convolve(
np.convolve(DENs, [1, 2 * np.pi * f3]),
[1, 2 * np.pi * f2])
return signal.bilinear(NUMs, DENs, fs)
def __init__(self, kind):
assert kind in ["A", "B", "C"]
if kind == "A":
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 0.17
numerator = [(2 * np.pi * f4)**2 * (10**(A1000 / 20)), 0, 0, 0, 0]
denominator = sp.polymul([1, 4 * np.pi * f4, (2 * np.pi * f4)**2],
[1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
denominator = sp.polymul(
sp.polymul(denominator, [1, 2 * np.pi * f3]),
[1, 2 * np.pi * f2])
if kind == "B":
f1 = 20.598997
f2 = 158.5
f4 = 12194.217
A1000 = 1.9997
numerator = [(2 * np.pi * f4)**2 * (10**(A1000 / 20)), 0, 0, 0]
denominator = sp.polymul([1, 4 * np.pi * f4, (2 * np.pi * f4)**2],
[1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
denominator = sp.polymul(denominator, [1, 2 * np.pi * f2])
if kind == "C":
f1 = 20.598997
f4 = 12194.217
C1000 = 0.0619
numerator = [(2 * np.pi * f4)**2 * (10**(C1000 / 20.0)), 0, 0]
denominator = sp.polymul(
[1, 4 * np.pi * f4, (2 * np.pi * f4)**2.0],
[1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
self.filter = sps.bilinear(numerator, denominator, 16000)
def parse_filter(args, analog=False, sample_rate=None):
"""Parse arbitrary input args into a TF or ZPK filter definition
Parameters
----------
args : `tuple`, `~scipy.signal.lti`
filter definition, normally just captured positional ``*args``
from a function call
analog : `bool`, optional
`True` if filter definition has analogue coefficients
sample_rate : `float`, optional
sampling frequency at which to convert analogue filter to digital
via bilinear transform, required if ``analog=True``
Returns
-------
ftype : `str`
either ``'ba'`` or ``'zpk'``
filt : `tuple`
the filter components for the returned `ftype`, either a 2-tuple
for with transfer function components, or a 3-tuple for ZPK
"""
if analog and not sample_rate:
raise ValueError("Must give sample_rate frequency to convert "
"analog filter to digital")
# unpack filter
if isinstance(args, tuple) and len(args) == 1:
# either packed defintion ((z, p, k)) or simple definition (lti,)
args = args[0]
# parse FIR filter
if isinstance(args, numpy.ndarray) and args.ndim == 1: # fir
b, a = args, [1.]
if analog:
return 'ba', signal.bilinear(b, a)
return 'ba', (b, a)
# parse IIR filter
if isinstance(args, LinearTimeInvariant):
lti = args
elif (isinstance(args, numpy.ndarray) and args.ndim == 2
and args.shape[1] == 6):
lti = signal.lti(*signal.sos2zpk(args))
else:
lti = signal.lti(*args)
# convert to zpk format
lti = lti.to_zpk()
# convert to digital components
if analog:
return 'zpk', bilinear_zpk(lti.zeros,
lti.poles,
lti.gain,
fs=sample_rate)
# return zpk
return 'zpk', (lti.zeros, lti.poles, lti.gain)
def ITU_R_468_weighting(fs):
"""
Return ITU-R 468 digital weighting filter transfer function
fs : float
Sampling frequency
Example:
>>> from scipy.signal import freqz
>>> import matplotlib.pyplot as plt
>>> fs = 200000
>>> b, a = ITU_R_468_weighting(fs)
>>> f = np.logspace(np.log10(10), np.log10(fs/2), 1000)
>>> w = 2*pi * f / fs
>>> w, h = freqz(b, a, w)
>>> plt.semilogx(w*fs/(2*pi), 20*np.log10(abs(h)))
>>> plt.grid(True, color='0.7', linestyle='-', which='both', axis='both')
>>> plt.axis([10, 100e3, -50, 20])
"""
z, p, k = ITU_R_468_weighting_analog()
# Use the bilinear transformation to get the digital filter.
try:
# Currently private but more accurate
from scipy.signal.filter_design import _zpkbilinear
zz, pz, kz = _zpkbilinear(z, p, k, fs)
return zpk2tf(zz, pz, kz)
except ImportError:
b, a = zpk2tf(z, p, k)
return bilinear(b, a, fs)
def ITU_R_468_weighting(fs):
"""
Return ITU-R 468 digital weighting filter transfer function
fs : float
Sampling frequency
Example:
>>> from scipy.signal import freqz
>>> import matplotlib.pyplot as plt
>>> fs = 200000
>>> b, a = ITU_R_468_weighting(fs)
>>> f = np.logspace(np.log10(10), np.log10(fs/2), 1000)
>>> w = 2*pi * f / fs
>>> w, h = freqz(b, a, w)
>>> plt.semilogx(w*fs/(2*pi), 20*np.log10(abs(h)))
>>> plt.grid(True, color='0.7', linestyle='-', which='both', axis='both')
>>> plt.axis([10, 100e3, -50, 20])
"""
z, p, k = ITU_R_468_weighting_analog()
# Use the bilinear transformation to get the digital filter.
try:
# Currently private but more accurate
from scipy.signal.filter_design import _zpkbilinear
zz, pz, kz = _zpkbilinear(z, p, k, fs)
return zpk2tf(zz, pz, kz)
except ImportError:
b, a = zpk2tf(z, p, k)
return bilinear(b, a, fs)
def A_weighting(fs):
"""Design of an A-weighting filter.
b, a = A_weighting(fs) designs a digital A-weighting filter for
sampling frequency `fs`. Usage: y = scipy.signal.lfilter(b, a, x).
Warning: `fs` should normally be higher than 20 kHz. For example,
fs = 48000 yields a class 1-compliant filter.
References:
[1] IEC/CD 1672: Electroacoustics-Sound Level Meters, Nov. 1996.
"""
# Definition of analog A-weighting filter according to IEC/CD 1672.
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = [(2*pi * f4)**2 * (10**(A1000/20)), 0, 0, 0, 0]
DENs = polymul([1, 4*pi * f4, (2*pi * f4)**2],
[1, 4*pi * f1, (2*pi * f1)**2])
DENs = polymul(polymul(DENs, [1, 2*pi * f3]),
[1, 2*pi * f2])
# Use the bilinear transformation to get the digital filter.
# (Octave, MATLAB, and PyLab disagree about Fs vs 1/Fs)
return bilinear(NUMs, DENs, fs)
def _transform(b, a, Wn, analog, output):
"""
Shift prototype filter to desired frequency, convert to digital with
pre-warping, and return in various formats.
"""
Wn = np.asarray(Wn)
if not analog:
if np.any(Wn < 0) or np.any(Wn > 1):
raise ValueError("Digital filter critical frequencies "
"must be 0 <= Wn <= 1")
fs = 2.0
warped = 2 * fs * tan(pi * Wn / fs)
else:
warped = Wn
# Shift frequency
b, a = lp2lp(b, a, wo=warped)
# Find discrete equivalent if necessary
if not analog:
b, a = bilinear(b, a, fs=fs)
# Transform to proper out type (pole-zero, state-space, numer-denom)
if output in ('zpk', 'zp'):
return tf2zpk(b, a)
elif output in ('ba', 'tf'):
return b, a
elif output in ('ss', 'abcd'):
return tf2ss(b, a)
elif output in ('sos'):
raise NotImplementedError('second-order sections not yet implemented')
else:
raise ValueError('Unknown output type {0}'.format(output))
def _transform(b, a, Wn, analog, output):
"""
Shift prototype filter to desired frequency, convert to digital with
pre-warping, and return in various formats.
"""
Wn = np.asarray(Wn)
if not analog:
if np.any(Wn < 0) or np.any(Wn > 1):
raise ValueError("Digital filter critical frequencies "
"must be 0 <= Wn <= 1")
fs = 2.0
warped = 2 * fs * tan(pi * Wn / fs)
else:
warped = Wn
# Shift frequency
b, a = lp2lp(b, a, wo=warped)
# Find discrete equivalent if necessary
if not analog:
b, a = bilinear(b, a, fs=fs)
# Transform to proper out type (pole-zero, state-space, numer-denom)
if output in ('zpk', 'zp'):
return tf2zpk(b, a)
elif output in ('ba', 'tf'):
return b, a
elif output in ('ss', 'abcd'):
return tf2ss(b, a)
elif output in ('sos'):
raise NotImplementedError('second-order sections not yet implemented')
else:
raise ValueError('Unknown output type {0}'.format(output))
def build_3rd_filter(omega, zeta, omega_1, lp=True, freq=FREQ):
b = [1, 0, 0, 0] if not lp else [0, 0, 0, omega**3]
a = [
1, 2 * zeta * omega + omega_1, omega**2 + omega_1 * 2 * zeta * omega,
omega**2 * omega_1
]
return RealtimeFilter(*signal.bilinear(b, a, fs=freq))
def integrate(data, sample_frequency, integration_time):
"""Integrate the sound pressure squared using exponential integration.
:param data: Energetic quantity, e.g. :math:`p^2`.
:param sample_frequency: Sample frequency.
:param integration_time: Integration time.
:returns:
Time weighting is applied by applying a low-pass filter with one real pole at :math:`-1/\\tau`.
.. note:: Because :math:`f_s \\cdot t_i` is generally not an integer, samples are discarded. This results in a drift of samples for longer signals (e.g. 60 minutes at 44.1 kHz).
"""
integration_time = np.asarray(integration_time)
sample_frequency = np.asarray(sample_frequency)
samples = data.shape[-1]
b, a = zpk2tf([1.0], [1.0, integration_time], [1.0])
b, a = bilinear(b, a, fs=sample_frequency)
#b, a = bilinear([1.0], [1.0, integration_time], fs=sample_frequency) # Bilinear: Analog to Digital filter.
n = np.floor(integration_time * sample_frequency).astype(int)
data = data[..., 0:n*(samples//n)]
newshape = list(data.shape[0:-1])
newshape.extend([-1, n])
data = data.reshape(newshape)
#data = data.reshape((-1, n)) # Divide in chunks over which to perform the integration.
return lfilter(b, a, data)[...,n-1] / integration_time # Perform the integration. Select the final value of the integration.
def A_weighting(fs):
"""Design of an A-weighting filter.
b, a = A_weighting(fs) designs a digital A-weighting filter for
sampling frequency `fs`. Usage: y = scipy.signal.lfilter(b, a, x).
Warning: `fs` should normally be higher than 20 kHz. For example,
fs = 48000 yields a class 1-compliant filter.
References:
[1] IEC/CD 1672: Electroacoustics-Sound Level Meters, Nov. 1996.
"""
# Definition of analog A-weighting filter according to IEC/CD 1672.
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = [(2*pi * f4)**2 * (10**(A1000/20)), 0, 0, 0, 0]
DENs = polymul([1, 4*pi * f4, (2*pi * f4)**2],
[1, 4*pi * f1, (2*pi * f1)**2])
DENs = polymul(polymul(DENs, [1, 2*pi * f3]),
[1, 2*pi * f2])
# Use the bilinear transformation to get the digital filter.
# (Octave, MATLAB, and PyLab disagree about Fs vs 1/Fs)
return bilinear(NUMs, DENs, fs)
def integrate(data, sample_frequency, integration_time):
"""Integrate the sound pressure squared using exponential integration.
:param data: Energetic quantity, e.g. :math:`p^2`.
:param sample_frequency: Sample frequency.
:param integration_time: Integration time.
:returns:
Time weighting is applied by applying a low-pass filter with one real pole at :math:`-1/\\tau`.
.. note:: Because :math:`f_s \\cdot t_i` is generally not an integer, samples are discarded. This results in a drift of samples for longer signals (e.g. 60 minutes at 44.1 kHz).
"""
integration_time = np.asarray(integration_time)
sample_frequency = np.asarray(sample_frequency)
samples = data.shape[-1]
b, a = zpk2tf([1.0], [1.0, integration_time], [1.0])
b, a = bilinear(b, a, fs=sample_frequency)
#b, a = bilinear([1.0], [1.0, integration_time], fs=sample_frequency) # Bilinear: Analog to Digital filter.
n = np.floor(integration_time * sample_frequency).astype(int)
data = data[..., 0:n * (samples // n)]
newshape = list(data.shape[0:-1])
newshape.extend([-1, n])
data = data.reshape(newshape)
#data = data.reshape((-1, n)) # Divide in chunks over which to perform the integration.
return lfilter(
b, a, data
)[..., n -
1] / integration_time # Perform the integration. Select the final value of the integration.
def smooth_filter(x):
WC = 0.5 # Tuning
num = [1]
den = [1 / WC, 1]
(b, a) = signal.bilinear(num, den, fs=1)
return signal.filtfilt(b, a, x)
def calc_coefs(self, R):
C = 25.0e-9
RC = R * C
b_s = np.array([RC, -1.0])
a_s = np.array([RC, 1.0])
b, a = signal.bilinear(b_s, a_s, fs=self.fs)
return b, a
def saveHTC(self, filename, disc='bilinear'):
if disc.lower() == 'matched':
# return discrete coefficients using matched zero pole method
b, a = MatchedZeroPole(self.b * self.K, self.a, self.Ts)
elif disc.lower() == 'bilinear':
# return discrete coefficients using bilinear method
b, a = signal.bilinear(self.b * self.K, self.a, 1 / self.Ts)
saveHTC(b, a, filename)
def calc_coefs(self, R, fbAmt):
C = 15.0e-9
RC = R * C
b_s = np.array([RC * RC, -2 * RC, 1.0])
a_s = np.array(
[b_s[0] * (1.0 + fbAmt), -b_s[1] * (1.0 - fbAmt), 1.0 + fbAmt])
b, a = signal.bilinear(b_s, a_s, fs=self.fs)
return b, a
def discretise(C, Fs=100, method='bilinear', save=None):
# discretizes the controller transfer function and saves it to file.
if method == 'bilinear':
coefz = signal.bilinear(C.num, C.den, fs=Fs)
elif method.lower() == 'matchedzeropole':
coefz = MatchedZeroPole(C.num, C.den, 1 / Fs)
Cd = signal.TransferFunction(coefz[0], coefz[1], dt=1 / Fs)
return Cd
def make_C_weighting(fs): den = p1 den = np.polymul(den, p1) den = np.polymul(den, p4) den = np.polymul(den, p4) num = np.poly1d([(2.*np.pi*f4)**2, 0, 0]) B, A = signal.bilinear(num, den, fs) return B, A
def make_C_weighting(fs): den = p1 den = np.polymul(den, p1) den = np.polymul(den, p4) den = np.polymul(den, p4) num = np.poly1d([(2.*np.pi*f4)**2, 0, 0]) B, A = signal.bilinear(num, den, fs) return B, A
def c_weighting(sample_rate):
"""Return digital filter coefficients
for C weighting for given sample_rate"""
f1 = 20.598997
f4 = 12194.217
C1000 = 0.0619
numerators = [(2 * np.pi * f4)**2 * (10**(C1000 / 20)), 0, 0]
denominators = np.convolve([1, 4 * np.pi * f4, (2 * np.pi * f4)**2],
[1, 4 * np.pi * f1, (2 * np.pi * f1)**2])
b, a = bilinear(numerators, denominators, sample_rate)
return b, a
def emphasis_iir(t1, t2, fs):
"""Generate an IIR filter for 6dB/octave pre-emphasis (t1 > t2) or
de-emphasis (t1 < t2), given time constants for the two corners."""
# Convert time constants to frequencies, and pre-warp for bilinear transform
w1 = 2 * fs * np.tan((1 / t1) / (2 * fs))
w2 = 2 * fs * np.tan((1 / t2) / (2 * fs))
# Zero at t1, pole at t2
tf_b, tf_a = sps.zpk2tf([-w1], [-w2], w2 / w1)
return sps.bilinear(tf_b, tf_a, fs)
def __init__(self, order, ripple, Fs, Fc):
"""
:param order: int, Order of the analog filter
:param ripple: float, Pass-band ripple in dB
:param Fs: float, Sampling frequency of filter implementation in Hz
:param Fc: float, 3dB edge frequency of low-pass filter in Hz
"""
B, A = sp.ellip(order, ripple, 40, 2 * np.pi * Fc, analog=True)
b, a = sp.bilinear(B, A, Fs)
super(AnalogFilter, self).__init__(b, a)
def coefs(self, disc=None):
if disc == None:
#return continuous coefficients
return self.b * self.K, self.a
elif disc.lower() == 'matched':
# return discrete coefficients using matched zero pole method
return MatchedZeroPole(self.b * self.K, self.a, self.Ts)
elif disc.lower() == 'bilinear':
# return discrete coefficients using bilinear method
return signal.bilinear(self.b * self.K, self.a, 1 / self.Ts)
def A_weight(fs):
"""
Coefficients and formula based on: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4331191/
"""
o = 2*pi*np.array([20.598997, 107.65265, 737.86223, 12194.217])
G = -2.0
num = [G*o[3]**2, 0, 0, 0, 0]
denom = polymul(polymul(polymul([1, o[0]], [1, o[0]]), polymul([1, o[3]], [1, o[3]])),
polymul([1, o[1]], [1, o[2]]))
return bilinear(num, denom, fs)
def grayscale_filter(img_gs):
bil_1, bil_2 = bilinear(1, 4)
img_gs_df = pd.DataFrame(img_gs)
img_gs_sv_row = np.zeros(shape=[len(img_gs_df), len(img_gs_df)])
img_gs_sv_col = np.zeros(shape=[len(img_gs_df), len(img_gs_df)])
for i in range(0, len(img_gs_df)):
img_gs_sv_row[i] = lfilter(x=img_gs_df.loc[i, :], b=bil_1, a=bil_2)
for j in range(0, len(img_gs_df)):
img_gs_sv_col[j] = lfilter(x=pd.DataFrame(img_gs_sv_row).loc[:, j],
b=bil_1,
a=bil_2)
return (img_gs_sv_row, img_gs_sv_col)
def createFilter(fc, Q, fs):
"""
Returns digital BPF with given specs
:param fc: BPF center frequency (Hz)
:param Q: BPF Q (Hz/Hz)
:param fs: sampling rate (Samp/sec)
:returns: digital implementation of BPF
"""
wc = 2 * pi * fc
num = [wc / Q, 0]
den = [1, wc / Q, wc**2]
dig_tf = signal.bilinear(num, den, fs)
return dig_tf
def digital_filter(measurement_system, sampling_freq):
# transform continuous system to digital filter
bc, ac = sos_phys2filter(
measurement_system["S0"], measurement_system["delta"], measurement_system["f0"]
)
assert_almost_equal(bc, [20465611686.098896])
assert_allclose(ac, np.array([1.00000000e00, 4.52389342e03, 5.11640292e10]))
b, a = dsp.bilinear(bc, ac, sampling_freq)
assert_allclose(
b, np.array([0.019386043211510096, 0.03877208642302019, 0.019386043211510096])
)
assert_allclose(a, np.array([1.0, -1.7975690550957188, 0.9914294872108197]))
return {"b": b, "a": a}
def A_weighting(fs):
"""
Design of an A-weighting filter.
Designs a digital A-weighting filter for
sampling frequency `fs`. Usage: y = lfilter(b, a, x).
Warning: fs should normally be higher than 20 kHz. For example,
fs = 48000 yields a class 1-compliant filter.
fs : float
Sampling frequency
Since this uses the bilinear transform, frequency response around fs/2 will
be inaccurate at lower sampling rates. A-weighting is undefined above
20 kHz, though.
Example:
from scipy.signal import freqz
import matplotlib.pyplot as plt
fs = 200000 # change to 48000 to see truncation
b, a = A_weighting(fs)
f = np.logspace(np.log10(10), np.log10(fs/2), 1000)
w = 2*pi * f / fs
w, h = freqz(b, a, w)
plt.semilogx(w*fs/(2*pi), 20*np.log10(abs(h)))
plt.grid(True, color='0.7', linestyle='-', which='both', axis='both')
plt.axis([10, 100e3, -50, 20])
References:
[1] IEC/CD 1672: Electroacoustics-Sound Level Meters, Nov. 1996.
"""
# Definition of analog A-weighting filter according to IEC/CD 1672.
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = [(2 * pi * f4)**2 * (10**(A1000 / 20)), 0, 0, 0, 0]
DENs = polymul([1, 4 * pi * f4, (2 * pi * f4)**2],
[1, 4 * pi * f1, (2 * pi * f1)**2])
DENs = polymul(DENs, [1, 2 * pi * f3])
DENs = polymul(DENs, [1, 2 * pi * f2])
# Analog confirmed to match https://en.wikipedia.org/wiki/A-weighting#A_2
# Use the bilinear transformation to get the digital filter.
return bilinear(NUMs, DENs, fs)
def A_weighting(fs):
"""
Design of an A-weighting filter.
Designs a digital A-weighting filter for
sampling frequency `fs`. Usage: y = lfilter(b, a, x).
Warning: fs should normally be higher than 20 kHz. For example,
fs = 48000 yields a class 1-compliant filter.
fs : float
Sampling frequency
Since this uses the bilinear transform, frequency response around fs/2 will
be inaccurate at lower sampling rates. A-weighting is undefined above
20 kHz, though.
Example:
from scipy.signal import freqz
import matplotlib.pyplot as plt
fs = 200000 # change to 48000 to see truncation
b, a = A_weighting(fs)
f = np.logspace(np.log10(10), np.log10(fs/2), 1000)
w = 2*pi * f / fs
w, h = freqz(b, a, w)
plt.semilogx(w*fs/(2*pi), 20*np.log10(abs(h)))
plt.grid(True, color='0.7', linestyle='-', which='both', axis='both')
plt.axis([10, 100e3, -50, 20])
References:
[1] IEC/CD 1672: Electroacoustics-Sound Level Meters, Nov. 1996.
"""
# Definition of analog A-weighting filter according to IEC/CD 1672.
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = [(2*pi * f4)**2 * (10**(A1000/20)), 0, 0, 0, 0]
DENs = polymul([1, 4*pi * f4, (2*pi * f4)**2],
[1, 4*pi * f1, (2*pi * f1)**2])
DENs = polymul(DENs, [1, 2*pi * f3])
DENs = polymul(DENs, [1, 2*pi * f2])
# Analog confirmed to match https://en.wikipedia.org/wiki/A-weighting#A_2
# Use the bilinear transformation to get the digital filter.
return bilinear(NUMs, DENs, fs)
def sd1_tone_test(): import numpy as np from numpy.testing import assert_almost_equal from scipy.signal import bilinear x = np.arange(10000).reshape(1, -1) d = np.sin(x * 2 * np.pi * 1000 / 48000) out, b, a = tone_filter(d) b0, a0 = create_coeffs(.5) bref, aref = bilinear(b0, a0, sampling) assert_almost_equal(b[::-1], bref) assert_almost_equal(np.hstack(([1], -a[::-1])), aref)
def coeficientesPonderacionA(RATE=48000):
f1 = 20.598997
f2 = 107.65265
f3 = 737.86223
f4 = 12194.217
A1000 = 1.9997
NUMs = [(2 * pi * f4)**2 * (10**(A1000 / 20)), 0, 0, 0, 0]
DENs = convolve([1., +4 * pi * f4, (2 * pi * f4)**2],
[1., +4 * pi * f1, (2 * pi * f1)**2])
DENs = convolve(convolve(DENs, [1., 2 * pi * f3]), [1., 2 * pi * f2])
# Uso de una transformación bilineal para obtener el filtro digital
return signal.bilinear(NUMs, DENs, RATE)
def __init__(self, sigma, num, den, dt):
self.sigma = sigma
self.dt = dt
self._noise_time = np.zeros(0)
self._zi = np.zeros([len(sigma), 2]) #initial conditions for filters
self.chol_sigma = np.linalg.cholesky(sigma);
fs = 1/self.dt
[self.bz, self.az] = ss.bilinear(num, den, fs=fs)
self.make_samples(0, 3)
actual_sd = np.std(self._noise_samples, 1)
ideal_sd = np.sqrt(np.diag(self.sigma))
self._gain = np.mean(ideal_sd) / np.mean(actual_sd)
def calc_digital_characteristics(params, freq_sampling):
""" Model """
(T1, T2, t0, K) = params
# Синтез цифрового фильта
b_analog = array([K])
# Судя по всему это повышение порядка фильтра
p1 = P([T1, 1])
p2 = P([T2, 1])
ans = p1*p2
a_analog = ans.coef
#print a_analog
b, a = signal.bilinear(b_analog, a_analog, fs=freq_sampling)
return b, a, freq_sampling
def __init__(self, type, fc, gain = 0, Q = 1, enabled = True):
self._enabled = enabled
self._type = type
self._fc = fc
self._g = gain
self._Q = Q
if type == FilterType.HPBrickwall:
z, p, k = scsig.ellip(12, 0.01, 80, fc, 'high', output='zpk')
self._sos = zpk2sos(z, p, k)[0]
elif type == FilterType.LPBrickwall:
z, p, k = scsig.ellip(12, 0.01, 80, fc, 'low', output='zpk')
self._sos = zpk2sos(z, p, k)[0]
elif type == FilterType.HPButter:
z, p, k = scsig.butter(2 ** Q, fc, btype = 'high', output='zpk')
self._sos = zpk2sos(z, p, k)[0]
elif type == FilterType.LPButter:
z, p, k = scsig.butter(2 ** Q, fc, output='zpk')
self._sos = zpk2sos(z, p, k)[0]
elif type == FilterType.LShelving or type == FilterType.HShelving:
A = 10 ** (gain / 20)
wc = np.pi * fc
wS = np.sin(wc)
wC = np.cos(wc)
alpha = wS / (2 * Q)
beta = A ** 0.5 / Q
c = 1
if type == FilterType.LShelving:
c = -1
b0 = A * (A + 1 + c * (A - 1) * wC + beta * wS)
b1 = - c * 2 * A * (A - 1 + c * (A + 1) * wC)
b2 = A * (A + 1 + c * (A - 1) * wC - beta * wS)
a0 = (A + 1 - c * (A - 1) * wC + beta * wS)
a1 = c * 2 * (A - 1 - c * (A + 1) * wC)
a2 = (A + 1 - c * (A - 1) * wC - beta * wS)
self._sos = np.array([[ b0, b1, b2, a0, a1, a2 ]])
elif type == FilterType.Peak:
self.g = gain
wc = np.pi * fc
b, a = scsig.bilinear([1, 10 ** (gain / 20) * wc / Q, wc ** 2],
[1, wc / Q, wc ** 2])
self._sos = np.append(b, a).reshape(1, 6)
self._ord = self._sos.shape[0] * 2
self.icReset()
def make_A_weighting(fs): p1_squared = np.polymul(p1, p1) p4_squared = np.polymul(p4, p4) den = p1 den = np.polymul(den, p1) den = np.polymul(den, p4) den = np.polymul(den, p4) den = np.polymul(den, p2) den = np.polymul(den, p3) num = np.poly1d([(2.*np.pi*f4)**2, 0, 0, 0, 0]) B, A = signal.bilinear(num, den, fs) return B, A
def weigh(self, weighting='A', zero_phase=False):
"""Apply frequency-weighting. By default 'A'-weighting is applied.
:param weighting: Frequency-weighting filter to apply.
Valid options are 'A', 'C' and 'Z'. Default weighting is 'A'.
:returns: Weighted signal.
:rtype: :class:`Signal`.
By default the weighting filter is applied using
:func:`scipy.signal.lfilter` causing a frequency-dependent delay. In case a
delay is undesired, the filter can be applied using :func:`scipy.signal.filtfilt`
by setting `zero_phase=True`.
"""
num, den = WEIGHTING_SYSTEMS[weighting]()
b, a = bilinear(num, den, self.fs)
func = filtfilt if zero_phase else lfilter
return self._construct(func(b, a, self))
def integrate(data, sample_frequency, integration_time):
"""
Integrate the sound pressure squared using exponential integration.
:param data: Energetic quantity, e.g. :math:`p^2`.
:param sample_frequency: Sample frequency.
:param integration_time: Integration time.
:returns:
Time weighting is applied by applying a low-pass filter with one real pole at :math:`-1/\\tau`.
.. note:: Because :math:`f_s \\cdot t_i` is generally not an integer, samples are discarded. This results in a drift of samples for longer signals (e.g. 60 minutes at 44.1 kHz).
"""
b, a = bilinear([1.0], [1.0, integration_time], fs=sample_frequency) # Bilinear: Analog to Digital filter.
n = int(floor(integration_time * sample_frequency))
data = data[0:n*(len(data)//n)]
data = data.reshape((-1, n)) # Divide in chunks over which to perform the integration.
return lfilter(b, a, data)[:,n-1] # Perform the integration. Select the final value of the integration.
def ex4_2():
wp = 0.2*np.pi
ws = 0.3*np.pi
fs = 4000.0
T = 1/fs
Wp = (2/T)*np.tan(wp/2);
Ws = (2/T)*np.tan(ws/2);
n, x = signal.buttord(Wp, Ws, 2, 40, analog=True)
b, a = signal.butter(n, x, analog=True)
b2, a2 = signal.bilinear(b, a, 4000)
w, h = signal.freqz(b2, a2, worN=10000)
image1 = subplot(2, 1, 1)
image2 = subplot(2, 1, 2)
plt.sca(image1)
plt.plot(w*fs/2/np.pi, abs(h))
plt.sca(image2)
plt.plot(w*fs/2/np.pi, np.angle(h))
plt.show()
def __init__(self, source, gain=1, **kwds):
# Automatically duplicate mono input to fit the desired output shape
gain = np.atleast_1d(gain)
if len(gain) != source.nchannels and len(gain) != 1:
if source.nchannels != 1:
raise ValueError('Can only automatically duplicate source '
'channels for mono sources, use '
'RestructureFilterbank.')
source = RestructureFilterbank(source, len(gain))
samplerate = source.samplerate
zeros = np.array([-200, -200])
poles = np.array([-250 + 400j, -250 - 400j,
-2000 + 6000j, -2000 - 6000j])
# use an arbitrary gain here, will be normalized afterwards
b, a = signal.zpk2tf(zeros, poles * 2 * np.pi, 1.5e9)
# normalize the response at 1000Hz (of the analog filter)
resp = np.abs(signal.freqs(b, a, [1000*2*np.pi])[1]) # response magnitude
b /= resp
bd, ad = signal.bilinear(b, a, samplerate)
bd = (np.tile(bd, (source.nchannels, 1)).T * gain).T
ad = np.tile(ad, (source.nchannels, 1))
LinearFilterbank.__init__(self, source, bd, ad, **kwds)
def A_weighting(fs):
"""
Design of an A-weighting filter.
Designs a digital A-weighting filter for
sampling frequency `fs`. Usage: y = lfilter(b, a, x).
Warning: fs should normally be higher than 20 kHz. For example,
fs = 48000 yields a class 1-compliant filter.
fs : float
Sampling frequency
Since this uses the bilinear transform, frequency response around fs/2 will
be inaccurate at lower sampling rates. A-weighting is undefined above
20 kHz, though.
Example:
from scipy.signal import freqz
import matplotlib.pyplot as plt
fs = 200000 # change to 48000 to see truncation
b, a = A_weighting(fs)
f = np.logspace(np.log10(10), np.log10(fs/2), 1000)
w = 2*pi * f / fs
w, h = freqz(b, a, w)
plt.semilogx(w*fs/(2*pi), 20*np.log10(abs(h)))
plt.grid(True, color='0.7', linestyle='-', which='both', axis='both')
plt.axis([10, 100e3, -50, 20])
References:
[1] IEC/CD 1672: Electroacoustics-Sound Level Meters, Nov. 1996.
"""
b, a = A_weighting_analog()
# Use the bilinear transformation to get the digital filter.
return bilinear(b, a, fs)
import numpy as np from scipy import signal from numpy import exp from pylab import * b = np.array([1.0]) a = np.array([3.0, 1.0]) B, A = signal.bilinear(b, a) print B, A #print 7*B, 7*A
def prepare_video_filters():
# TODO: test these CLV+innerCAV parameters. Should be same on PAL+NTSC
t1 = 25
t2 = 13.75
[tf_b, tf_a] = sps.zpk2tf(-t2 * (10 ** -8), -t1 * (10 ** -8), t1 / t2)
SP["f_emp"] = sps.bilinear(tf_b, tf_a, 1 / (freq_hz / 2))
# RF BPF and analog audio cut filters
SP["f_videorf_bpf"] = sps.butter(
1, [SP["vbpf"][0] / (freq_hz / 2), SP["vbpf"][1] / (freq_hz / 2)], btype="bandpass"
)
if SP["analog_audio"] == True:
SP["f_aleft_stop"] = sps.butter(
1,
[(SP["audio_lfreq"] - 750000) / (freq_hz / 2), (SP["audio_lfreq"] + 750000) / (freq_hz / 2)],
btype="bandstop",
)
SP["f_aright_stop"] = sps.butter(
1,
[(SP["audio_rfreq"] - 750000) / (freq_hz / 2), (SP["audio_rfreq"] + 750000) / (freq_hz / 2)],
btype="bandstop",
)
# standard post-demod LPF
lowpass_filter_b, lowpass_filter_a = sps.butter(SP["vlpf_order"], SP["vlpf_freq"] / (freq_hz / 2), "low")
# post-demod deemphasis filter
[tf_b, tf_a] = sps.zpk2tf(
-SP["deemp"][1] * (10 ** -8), -SP["deemp"][0] * (10 ** -8), SP["deemp"][0] / SP["deemp"][1]
)
SP["f_deemp"] = sps.bilinear(tf_b, tf_a, 1 / (freq_hz / 2))
# if AC3:
# SP['f_arightcut'] = sps.butter(1, [(2650000)/(freq_hz/2), (3150000)/(freq_hz/2)], btype='bandstop')
# prepare for FFT: convert above filters to FIR using FDLS techniques first
forder = 256
forderd = 0
[Bbpf_FDLS, Abpf_FDLS] = fdls.FDLS_fromfilt(
SP["f_videorf_bpf"][0], SP["f_videorf_bpf"][1], forder, forderd, 0, phasemult=1.00
)
[Bemp_FDLS, Aemp_FDLS] = fdls.FDLS_fromfilt(SP["f_emp"][0], SP["f_emp"][1], forder, forderd, 0)
[Bdeemp_FDLS, Adeemp_FDLS] = fdls.FDLS_fromfilt(SP["f_deemp"][0], SP["f_deemp"][1], forder, forderd, 0)
[Bplpf_FDLS, Aplpf_FDLS] = fdls.FDLS_fromfilt(lowpass_filter_b, lowpass_filter_a, forder, forderd, 0)
Fbpf = np.fft.fft(Bbpf_FDLS, blocklen)
Femp = np.fft.fft(Bemp_FDLS, blocklen)
SP["fft_video"] = Fbpf * fft_hilbert
if SP["analog_audio"] == True:
[Bcutl_FDLS, Acutl_FDLS] = fdls.FDLS_fromfilt(
SP["f_aleft_stop"][0], SP["f_aleft_stop"][1], forder, forderd, 0, phasemult=1.00
)
[Bcutr_FDLS, Acutr_FDLS] = fdls.FDLS_fromfilt(
SP["f_aright_stop"][0], SP["f_aright_stop"][1], forder, forderd, 0, phasemult=1.00
)
Fcutl = np.fft.fft(Bcutl_FDLS, blocklen)
Fcutr = np.fft.fft(Bcutr_FDLS, blocklen)
SP["fft_video"] *= Fcutl * Fcutr
SP["fft_video_inner"] = SP["fft_video"] * Femp
# Post processing: lowpass filter + deemp
Fplpf = np.fft.fft(Bplpf_FDLS, blocklen)
Fdeemp = np.fft.fft(Bdeemp_FDLS, blocklen)
SP["fft_post"] = Fplpf * Fdeemp
# determine freq offset and mult for output stage
hz_ire_scale = (SP["videorf_100ire"] - SP["videorf_0ire"]) / 100
minn = SP["videorf_0ire"] + (hz_ire_scale * -60)
SP["output_minfreq"] = sminn = minn / (freq_hz / tau)
out_scale = 65534.0 / (SP["ire_max"] - SP["ire_min"])
SP["output_scale"] = (freq_hz / tau) * (out_scale / hz_ire_scale)
##### FIR filter parameters N = 12 # filter order tau = 6 # time delay # parameters of simulated measurement Fs = 500e3 Ts = 1 / Fs # sensor/measurement system f0 = 36e3; uf0 = 0.1e3 S0 = 0.124; uS0= 1.5e-4 delta = 0.0055; udelta = 5e-3 # transform continuous system to digital filter bc, ac = sos.phys2filter(S0,delta,f0) b, a = dsp.bilinear(bc, ac, Fs) # simulate input and output signals time = np.arange(0, 4e-3 - Ts, Ts) x = shocklikeGaussian(time, t0 = 2e-3, sigma = 1e-5, m0=0.8) y = dsp.lfilter(b, a, x) noise = 1e-3 yn = y + np.random.randn(np.size(y)) * noise # Monte Carlo for calculation of unc. assoc. with [real(H),imag(H)] runs = 10000 MCS0 = S0 + rst.randn(runs)*uS0 MCd = delta+ rst.randn(runs)*udelta MCf0 = f0 + rst.randn(runs)*uf0 f = np.linspace(0, 120e3, 200) HMC = sos.FreqResp(MCS0, MCd, MCf0, f)
# 1664/345 deemp_pole = .1480 * 1 deemp_zero = 3.20 * 1 lowpass_b = sps.firwin(45, [.390/NYQUIST_MHZ, 2.700/NYQUIST_MHZ], pass_zero=False) lowpass_a = [1.0] lowpass_b, lowpass_a = sps.butter(4, [0.39/NYQUIST_MHZ, 2.700/NYQUIST_MHZ], btype='bandpass') # 1664/345 deemp_pole = .1480 * 1 deemp_zero = 3.20 * 1 lowpass_b = sps.firwin(45, [.390/NYQUIST_MHZ, 2.600/NYQUIST_MHZ], pass_zero=False) lowpass_a = [1.0] [tf_b, tf_a] = sps.zpk2tf([-deemp_pole*(10**-8)], [-deemp_zero*(10**-8)], deemp_pole / deemp_zero) [f_emp_b, f_emp_a] = sps.bilinear(tf_b, tf_a, .5/FREQ_HZ) # .295 leftover scale: 1053 frames found, 34731 good samps, 374 ERRORS bandpass = sps.firwin(55, [.335/NYQUIST_MHZ, 1.870/NYQUIST_MHZ], pass_zero=False) # .295 leftover scale: 1053 frames found, 34731 good samps, 374 ERRORS # 1647/352 bandpass = sps.firwin(53, [.335/NYQUIST_MHZ, 1.870/NYQUIST_MHZ], pass_zero=False) # 1651/348 bandpass = sps.firwin(53, [.355/NYQUIST_MHZ, 1.870/NYQUIST_MHZ], pass_zero=False) # 1652/343 bandpass = sps.firwin(53, [.355/NYQUIST_MHZ, 2.800/NYQUIST_MHZ], pass_zero=False) # 1660/341 bandpass = sps.firwin(39, [.600/NYQUIST_MHZ, 2.800/NYQUIST_MHZ], pass_zero=False) # 1660/339
# -*- coding: utf-8 -*- from scipy import signal import pylab as pl from numpy import * fs = 8000.0 # 取样频率 fc = 1000.0 # 通带频率 # 频率为2kHz的3阶低通滤波器 b, a = signal.butter(3, 2*pi*fc, analog=1) w, h = signal.freqs(b,a,worN=10000) p = 20*log10(abs(h)) # 转换为取样频率为8kHz的数字滤波器 b2, a2 = signal.bilinear(b,a,fs=fs) # 计算数字滤波器的频率响应 w2, h2 = signal.freqz(b2,a2,worN=10000) # 计算增益特性 p2 = 20*log10(abs(h2)) # 找到增益为-3dB[精确值为： 10*log10(0.5)]的下标 idx = argmin(abs(p2-10*log10(0.5))) # 输出增益为-3dB时的频率 print w2[idx]/2/pi*8000 # 输出公式 w = 2/T*arctan(wa*T/2) 计算的频率 print 2*fs*arctan(2*pi*fc/2/fs) /2/pi
##### FIR filter parameters N = 12 # filter order tau = 6 # time delay # parameters of simulated measurement Fs = 500e3 # sampling frequency (in Hz) Ts = 1 / Fs # sampling intervall length (in s) # sensor/measurement system f0 = 36e3; uf0 = 0.01*f0 # resonance frequency S0 = 0.124; uS0= 0.001*S0 # static gain delta = 0.0055; udelta = 0.1*delta # damping # transform continuous system to digital filter bc, ac = sos_phys2filter(S0,delta,f0) # calculate analogue filter coefficients b, a = dsp.bilinear(bc, ac, Fs) # transform to digital filter coefficients # simulate input and output signals time = np.arange(0, 4e-3 - Ts, Ts) # time values x = shocklikeGaussian(time, t0 = 2e-3, sigma = 1e-5, m0=0.8) # input signal y = dsp.lfilter(b, a, x) # output signal noise = 1e-3 # noise std yn = y + rst.randn(np.size(y)) * noise # add white noise # Monte Carlo for calculation of unc. assoc. with [real(H),imag(H)] runs = 10000 MCf0 = f0 + rst.randn(runs)*uf0 # MC draws of resonance frequency MCS0 = S0 + rst.randn(runs)*uS0 # MC draws of static gain MCd = delta+ rst.randn(runs)*udelta # MC draws of damping f = np.linspace(0, 120e3, 200) # frequencies at which to calculate frequency response HMC = np.zeros((runs, len(f)),dtype=complex) # initialise frequency response with zeros
x = np.arange(10000).reshape(1, -1) d = np.sin(x * 2 * np.pi * 1000 / 48000) out, b, a = tone_filter(d) b0, a0 = create_coeffs(.5) bref, aref = bilinear(b0, a0, sampling) assert_almost_equal(b[::-1], bref) assert_almost_equal(np.hstack(([1], -a[::-1])), aref) if __name__ == "__main__": import numpy as np from scipy import signal import matplotlib.pyplot as plt b0, a0 = create_coeffs(.9) print(b0, a0) print(np.roots(b0[::-1]), np.roots(a0[::-1])) # a0 roots should have a real part negative. It seems to be related to R3 smaller than R2. The equation must be wrong somewhere. b0d, a0d = signal.bilinear(b0, a0, sampling) print(b0d, a0d) print(np.roots(b0d), np.roots(a0d)) size = 1000 x = np.arange(size, dtype=np.float64).reshape(1, -1)/ sampling d = np.sin(x * 2 * np.pi * 1000) out, b, a = tone_filter(d) outt = signal.lfilter(b0d, a0d, d) plt.plot(x[0], d[0], label="Raw") plt.plot(x[0], out[0], label="Filtered")
def calculate_Pst(data):
show_time_signals = 0 #Aktivierung des Plots der Zeitsignale im Flickermeter
show_filter_responses = 0 #Aktivierung des Plots der Amplitudengänge der Filter.
#(zu Prüfzecken der internen Filter)
fs = 4000
## Block 1: Modulierung des Spannungssignals
u = data - np.mean(data) # entfernt DC-Anteil
u_rms = np.sqrt(np.mean(np.power(u,2)))
u = u / (u_rms * np.sqrt(2)) # Normierung des Eingangssignals
## Block 2: Quadrierer
u_0 = u**2
## Block 3: Hochpass-, Tiefpass- und Gewichtungsfilter
# Konfiguration der Filter
HIGHPASS_ORDER = 1 #Ordnungszahl der Hochpassfilters
HIGHPASS_CUTOFF = 0.05 #Hz Grenzfrequenz
LOWPASS_ORDER = 6 #Ordnungszahl des Tiefpassfilters
if (f_line == 50):
LOWPASS_CUTOFF = 35.0 #Hz Grenzfrequenz
if (f_line == 60):
LOWPASS_CUTOFF = 42.0 #Hz Grenzfrequenz
# subtract DC component to limit filter transients at start of simulation
u_0_ac = u_0 - np.mean(u_0)
#Hochpassfilter
b_hp, a_hp = signal.butter(HIGHPASS_ORDER, (HIGHPASS_CUTOFF/(fs/2)), 'highpass')
u_hp = signal.lfilter(b_hp, a_hp, u_0_ac)
# smooth start of signal to avoid filter transient at start of simulation
smooth_limit = min(round(fs / 10), len(u_hp))
u_hp[ : smooth_limit] = u_hp[ : smooth_limit] * np.linspace(0, 1, smooth_limit)
#Tiefpassfilter
b_bw, a_bw = signal.butter(LOWPASS_ORDER, (LOWPASS_CUTOFF/(fs/2)), 'lowpass')
u_bw = signal.lfilter(b_bw, a_bw, u_hp)
# Gewichtungsfilter (Werte sind aus der Norm)
if (f_line == 50):
K = 1.74802
LAMBDA = 2 * np.pi * 4.05981
OMEGA1 = 2 * np.pi * 9.15494
OMEGA2 = 2 * np.pi * 2.27979
OMEGA3 = 2 * np.pi * 1.22535
OMEGA4 = 2 * np.pi * 21.9
if (f_line == 60):
K = 1.6357
LAMBDA = 2 * np.pi * 4.167375
OMEGA1 = 2 * np.pi * 9.077169
OMEGA2 = 2 * np.pi * 2.939902
OMEGA3 = 2 * np.pi * 1.394468
OMEGA4 = 2 * np.pi * 17.31512
num1 = [K * OMEGA1, 0]
denum1 = [1, 2 * LAMBDA, OMEGA1**2]
num2 = [1 / OMEGA2, 1]
denum2 = [1 / (OMEGA3 * OMEGA4), 1 / OMEGA3 + 1 / OMEGA4, 1]
b_w, a_w = signal.bilinear(np.convolve(num1, num2),
np.convolve(denum1, denum2), fs)
u_w = signal.lfilter(b_w, a_w, u_bw)
## Block 4: Quadrierung und Varianzschätzer
LOWPASS_2_ORDER = 1
LOWPASS_2_CUTOFF = 1 / (2 * np.pi * 300e-3) # Zeitkonstante 300 msek.
SCALING_FACTOR = 1238400 # Skalierung auf eine Wahrnehmbarkeitsskala
#Quadrierer
u_q = u_w**2
#Varianzschätzer
b_lp, a_lp = signal.butter(LOWPASS_2_ORDER,(LOWPASS_2_CUTOFF/(fs/2)),'low')
s = SCALING_FACTOR * signal.lfilter(b_lp, a_lp, u_q)
## Block 5: Statistische Berechnung
p_50s = np.mean([np.percentile(s, 100-30, interpolation="linear"),
np.percentile(s, 100-50, interpolation="linear"),
np.percentile(s, 100-80, interpolation="linear")])
p_10s = np.mean([np.percentile(s, 100-6, interpolation="linear"),
np.percentile(s, 100-8, interpolation="linear"),
np.percentile(s, 100-10, interpolation="linear"),
np.percentile(s, 100-13, interpolation="linear"),
np.percentile(s, 100-17, interpolation="linear")])
p_3s = np.mean([np.percentile(s, 100-2.2, interpolation="linear"),
np.percentile(s, 100-3, interpolation="linear"),
np.percentile(s, 100-4, interpolation="linear")])
p_1s = np.mean([np.percentile(s, 100-0.7, interpolation="linear"),
np.percentile(s, 100-1, interpolation="linear"),
np.percentile(s, 100-1.5, interpolation="linear")])
p_0_1s = np.percentile(s, 100-0.1, interpolation="linear")
P_st = np.sqrt(0.0314*p_0_1s+0.0525*p_1s+0.0657*p_3s+0.28*p_10s+0.08*p_50s)
if (show_time_signals):
t = np.linspace(0, len(u) / fs, num=len(u))
plt.figure()
plt.clf()
#plt.subplot(2, 2, 1)
plt.hold(True)
plt.plot(t, u, 'b', label="u")
plt.plot(t, u_0, 'm', label="u_0")
plt.plot(t, u_hp, 'r', label="u_hp")
plt.xlim(0, len(u)/fs)
plt.hold(False)
plt.legend(loc=1)
plt.grid(True)
#plt.subplot(2, 2, 2)
plt.figure()
plt.clf()
plt.hold(True)
plt.plot(t, u_bw, 'b', label="u_bw")
plt.plot(t, u_w, 'm', label="u_w")
plt.xlim(0, len(u)/fs)
plt.legend(loc=1)
plt.hold(False)
plt.grid(True)
#plt.subplot(2, 2, 3)
plt.figure()
plt.clf()
plt.plot(t, u_q, 'b', label="u_q")
plt.xlim(0, len(u)/fs)
plt.legend(loc=1)
plt.grid(True)
#plt.subplot(2, 2, 4)
plt.figure()
plt.clf()
plt.plot(t, s, 'b', label="s")
plt.xlim(0, len(u)/fs)
plt.legend(loc=1)
plt.grid(True)
if (show_filter_responses):
f, h_hp = signal.freqz(b_hp, a_hp, 4096)
f, h_bw = signal.freqz(b_bw, a_bw, 4096)
f, h_w = signal.freqz(b_w, a_w, 4096)
f, h_lp = signal.freqz(b_lp, a_lp, 4096)
f = f/np.pi*fs/2
plt.figure()
plt.clf()
plt.hold(True)
plt.plot(f, abs(h_hp), 'b', label="Hochpass 1. Ordnung")
plt.plot(f, abs(h_bw), 'r', label="Butterworth Tiefpass 6.Ordnung")
plt.plot(f, abs(h_w), 'g', label="Gewichtungsfilter")
plt.plot(f, abs(h_lp), 'm', label="Varianzschätzer")
plt.legend(bbox_to_anchor=(1., 1.), loc=2)
plt.hold(False)
plt.grid(True)
plt.axis([0, 35, 0, 1])
return P_st, max(s)
sd10 = signal.freqs(b0, a0, worN=np.logspace(1, 4.5, 1000))
sd11 = signal.freqs(b1, a1, worN=np.logspace(1, 4.5, 1000))
sd1 = signal.freqs(b, a, worN=np.logspace(1, 4.5, 1000))
fig = plt.figure()
plt.title("Analog filter frequency response (SD1 tone circuit)")
ax1 = fig.add_subplot(111)
plt.loglog(sd10[0], np.abs(sd10[1]), "b", label="alpha=0")
plt.loglog(sd11[0], np.abs(sd11[1]), "g", label="alpha=1")
plt.loglog(sd1[0], np.abs(sd1[1]), "r", label="alpha=0.5")
plt.xlabel("Frequency (Hz)")
plt.ylabel("Amplitude (dB)")
plt.legend()
b0d, a0d = signal.bilinear(b0, a0, 44100)
b1d, a1d = signal.bilinear(b1, a1, 44100)
bd, ad = signal.bilinear(b, a, 44100)
sd10d = signal.freqz(b0d, a0d)
sd11d = signal.freqz(b1d, a1d)
sd1d = signal.freqz(bd, ad)
fig = plt.figure()
plt.title("Digital filter frequency response (SD1 tone circuit) 44.1kHz")
ax1 = fig.add_subplot(111)
plt.loglog(sd10d[0], np.abs(sd10d[1]), "b", label="alpha=0")
plt.loglog(sd11d[0], np.abs(sd11d[1]), "g", label="alpha=1")
plt.loglog(sd1d[0], np.abs(sd1d[1]), "r", label="alpha=0.5")
plt.xlabel("Frequency (rad/sample)")
def main(): global lowpass_filter_b, lowpass_filter_a global hz_ire_scale, minn global f_deemp_b, f_deemp_a global deemp_t1, deemp_t2 global Bcutr, Acutr global Inner global blocklen outfile = sys.stdout #.buffer audio_mode = 0 CAV = 0 byte_start = 0 byte_end = 0 f_seconds = False optlist, cut_argv = getopt.getopt(sys.argv[1:], "d:D:hLCaAwSs:") for o, a in optlist: if o == "-d": deemp_t1 = np.double(a) if o == "-D": deemp_t2 = np.double(a) if o == "-a": audio_mode = 1 blocklen = blocklen * 4 if o == "-L": Inner = 1 if o == "-A": CAV = 1 Inner = 1 if o == "-h": # use full spec deemphasis filter - will result in overshoot, but higher freq resonse f_deemp_b = [3.778720395899611e-01, -2.442559208200777e-01] f_deemp_a = [1.000000000000000e+00, -8.663838812301168e-01] if o == "-C": Bcutr, Acutr = sps.butter(1, [2.50/(freq/2), 3.26/(freq/2)], btype='bandstop') Bcutr, Acutr = sps.butter(1, [2.68/(freq/2), 3.08/(freq/2)], btype='bandstop') if o == "-w": hz_ire_scale = (9360000 - 8100000) / 100 minn = 8100000 + (hz_ire_scale * -60) if o == "-S": f_seconds = True if o == "-s": ia = int(a) if ia == 0: lowpass_filter_b, lowpass_filter_a = sps.butter(5, (4.2/(freq/2)), 'low') if ia == 1: lowpass_filter_b, lowpass_filter_a = sps.butter(5, (4.4/(freq/2)), 'low') if ia == 2: lowpass_filter_b, lowpass_filter_a = sps.butter(6, (4.6/(freq/2)), 'low') lowpass_filter_b, lowpass_filter_a = sps.butter(6, (4.6/(freq/2)), 'low') deemp_t1 = .825 deemp_t2 = 2.35 if ia == 3: # high frequency response - and ringing. choose your poison ;) lowpass_filter_b, lowpass_filter_a = sps.butter(10, (5.0/(freq/2)), 'low') lowpass_filter_b, lowpass_filter_a = sps.butter(7, (5.0/(freq/2)), 'low') if ia == 4: lowpass_filter_b, lowpass_filter_a = sps.butter(10, (5.3/(freq/2)), 'low') lowpass_filter_b, lowpass_filter_a = sps.butter(7, (5.3/(freq/2)), 'low') if ia == 51: lowpass_filter_b, lowpass_filter_a = sps.butter(5, (4.4/(freq/2)), 'low') if ia == 61: lowpass_filter_b, lowpass_filter_a = sps.butter(6, (4.4/(freq/2)), 'low') if ia == 62: lowpass_filter_b, lowpass_filter_a = sps.butter(6, (4.7/(freq/2)), 'low') # set up deemp filter [tf_b, tf_a] = sps.zpk2tf(-deemp_t2*(10**-8), -deemp_t1*(10**-8), deemp_t1 / deemp_t2) [f_deemp_b, f_deemp_a] = sps.bilinear(tf_b, tf_a, 1/(freq_hz/2)) # test() argc = len(cut_argv) if argc >= 1: infile = open(cut_argv[0], "rb") else: infile = sys.stdin byte_start = 0 if (argc >= 2): byte_start = float(cut_argv[1]) if (argc >= 3): byte_end = float(cut_argv[2]) limit = 1 else: limit = 0 if f_seconds: byte_start *= freq_hz byte_end *= freq_hz else: byte_end += byte_start byte_end -= byte_start byte_start = int(byte_start) byte_end = int(byte_end) if (byte_start > 0): infile.seek(byte_start) if CAV and byte_start > 11454654400: CAV = 0 Inner = 0 # set up deemp filter [tf_b, tf_a] = sps.zpk2tf(-deemp_t2*(10**-8), -deemp_t1*(10**-8), deemp_t1 / deemp_t2) [f_deemp_b, f_deemp_a] = sps.bilinear(tf_b, tf_a, 1/(freq_hz/2)) total = toread = blocklen inbuf = infile.read(toread) indata = np.fromstring(inbuf, 'uint8', toread) total = 0 total_prevread = 0 total_read = 0 while (len(inbuf) > 0): toread = blocklen - indata.size if toread > 0: inbuf = infile.read(toread) indata = np.append(indata, np.fromstring(inbuf, 'uint8', len(inbuf))) if indata.size < blocklen: exit() if audio_mode: output, osamp = process_audio(indata) nread = osamp outfile.write(output) else: output_16 = process_video(indata) outfile.write(output_16) nread = len(output_16) total_pread = total_read total_read += nread if CAV: if (total_read + byte_start) > 11454654400: CAV = 0 Inner = 0 indata = indata[nread:len(indata)] if limit == 1: byte_end -= toread if (byte_end < 0): inbuf = ""
