|
from numpy import asarray, array, arange, append, angle, complex128, float64, floor |
|
from numpy import nonzero, sign, mat, sin, cos, exp, zeros, log10, unique, fix, ceil |
|
from numpy import ones, prod, pi, NaN, zeros_like, ravel, any, linspace, diff |
|
from numpy.fft import fft, ifft |
|
from scipy.signal import convolve, freqz, roots, zpk2tf, tf2zpk, remez, get_window |
|
from scipy.interpolate import interp1d |
|
from scipy.linalg import toeplitz, hankel |
|
|
|
def find(condition): |
|
""" |
|
Return the indices where ravel(condition) is true |
|
""" |
|
res, = nonzero(ravel(condition)) |
|
return res |
|
|
|
def rem(x,y): |
|
""" |
|
Remainder after division. |
|
rem(x,y) is equivalent to x - y.*fix(x./y) in case y is not zero. |
|
By convention (but contrary to numpy), rem(x,0) returns None. |
|
This also differs from numpy.remainder, which uses floor instead of |
|
fix. |
|
""" |
|
x,y = asarray(x), asarray(y) |
|
if any(y == 0): |
|
return None |
|
return x - y * fix(x/y) |
|
|
|
|
|
def phase(x, tol=0.95) : |
|
""" |
|
Compute phase of input-signal with phase jump avoiding. |
|
|
|
Inputs : |
|
x : input complex sequence. |
|
tol : (optional) tolerance of phase jump detection. It should be 0.8 <= tol <= 1. |
|
|
|
Output : |
|
phi : phase of input sequence with phase jump avoiding. |
|
""" |
|
phi = angle(x) |
|
dphi = append(0, phi[1:] - phi[:-1]) |
|
p1 = arange(dphi.shape[0])[dphi > 2*tol*pi] |
|
p2 = arange(dphi.shape[0])[dphi < -2*tol**pi] |
|
|
|
for i in p1 : |
|
phi[i:] = phi[i:]-2*pi |
|
for i in p2 : |
|
phi[i:] = phi[i:]+2*pi |
|
|
|
dphi = append(0, phi[1:] - phi[:-1]) |
|
p1 = arange(dphi.shape[0])[dphi > tol*pi] |
|
p2 = arange(dphi.shape[0])[dphi < -tol*pi] |
|
for i in p1 : |
|
phi[i:] = phi[i:]-pi |
|
for i in p2 : |
|
phi[i:] = phi[i:]+pi |
|
return phi |
|
|
|
def phasejumb(x, tot=0.9) : |
|
""" |
|
Detect phase jump of input signal |
|
|
|
Inputs : |
|
x : input complex sequence. |
|
tol : (optional) tolerance of phase jump detection. It should be 0.8 <= tol <= 1. |
|
|
|
Outputs : |
|
jump : Boolean array. True == jump, False == not jump |
|
""" |
|
phi = angle(x) |
|
dphi = append(0, phi[1:] - phi[:-1]) |
|
return (abs(dphi) > 0.9*pi) + (dphi == NaN) |
|
|
|
def firtype(b, tol=1e-9): |
|
""" |
|
Detect type of FIR filter with its coefficients. |
|
|
|
Inputs : |
|
b : coefficients of FIR filter |
|
Output : |
|
type : type of FIR filter |
|
1 = even symmetry |
|
2 = odd symmetry |
|
3 = even anti symmetry |
|
4 = odd anty symmetry |
|
0 = not symmetry |
|
""" |
|
b = array(b) |
|
symm = abs(max(b - b[::-1])) <= abs(tol) |
|
antisymm = abs(max(b + b[::-1])) <= abs(tol) |
|
odd = len(b)%2 == 0 |
|
if not odd and symm : return 1 |
|
elif odd and symm : return 2 |
|
elif not odd and antisymm : return 3 |
|
elif odd and antisymm : return 4 |
|
else : return 0 |
|
|
|
def zerophase(b,a,l=512): |
|
""" |
|
Zerophase response of digital filters. Zerophase response or amplitude response |
|
is magnitude reponse with consideration of sign. It can be computed accurately, |
|
if input coefficients are coefficients of filter type I to IV. For another types of |
|
filters, signs of zerosphase response are estimated. |
|
|
|
Inputs : |
|
b,a : Filter coefficients |
|
l : length of output zeros response |
|
|
|
Outputs : Hr, phase, W |
|
Hr : Zerophase response |
|
phase : phase of freqeuncy response with H[W] = Hr[W]*exp(1j*phase[W]) |
|
W : Angular frequencies between 0 and pi |
|
|
|
""" |
|
def estsign(H,b,a,tol=1e-3): |
|
mag = abs(H) |
|
phi = angle(H) |
|
phij = phasejumb(phi) |
|
nnan = 1 |
|
while phi[nnan] == NaN : nnan += 1 |
|
if nnan == 1 : firstsign = sign(sum(b)/sum(a)) |
|
else : firstsign = sign(phi[nnan])/2+.5 |
|
|
|
nearzero = mag < tol |
|
sign_dphi = sign(append(0, phi[1:] - phi[:-1])) |
|
changesigndiff = (append(1, sign_dphi[1:]*sign_dphi[:-1])) == -1 |
|
Hsign = ones(len(H))*firstsign |
|
for i in arange(len(H))[nearzero*changesigndiff] : |
|
Hsign[i:] = -1*Hsign[i:] |
|
return Hsign |
|
|
|
b = array(b) |
|
a = array(a) |
|
ftype = firtype(b) |
|
if len(a) == 1 and 1 <= ftype <= 4: |
|
m = len(b) |
|
w = arange(l)*pi/l |
|
b = b/a[0] |
|
if ftype == 1 : |
|
coef = append(b[(m-1)/2], 2*(b[(m-3)/2::-1])) |
|
ang = mat(w).T*mat(arange(len(coef))) |
|
Hr = mat(cos(ang))*mat(coef).T |
|
elif ftype == 2 : |
|
coef = 2*(b[m/2-1::-1]) |
|
ang = mat(w).T*mat(arange(len(coef))+0.5) |
|
Hr = mat(cos(ang))*mat(coef).T |
|
if ftype == 3 : |
|
coef = 2*(b[(m-1)/2::-1]) |
|
ang = mat(w).T*mat(arange(len(coef))) |
|
Hr = mat(sin(ang))*mat(coef).T |
|
elif ftype == 4 : |
|
coef = 2*(b[m/2-1::-1]) |
|
ang = mat(w).T*mat(arange(len(coef))+0.5) |
|
Hr = mat(sin(ang))*mat(coef).T |
|
W, H = freqz(b,a,l) |
|
Hr = array(Hr)[:,0] |
|
return Hr, phase(H/Hr), W |
|
else : |
|
W, H = freqz(b,a,l) |
|
Hr = estsign(H,b,a)*abs(H) |
|
return Hr, phase(H/Hr), W |
|
|
|
def groupdelay(b,a,l=512,whole=False): |
|
""" |
|
Returns the group delay g of the IIR, FIR filter with coefficients b, a. |
|
The response is evaluated at 512 angular frequencies between 0 and |
|
pi. w is a vector containing the 512 frequencies. |
|
The group delay is in units of samples. It can be converted |
|
to seconds by multiplying by the sampling period (or dividing by |
|
the sampling rate fs). |
|
|
|
Inputs : |
|
b, a : FIR, IIR coefficients |
|
Outputs : |
|
w : Angular frequencies between 0 and pi |
|
g : Group delay |
|
|
|
Theory: group delay, g(w) = -d/dw [arg{H(e^jw)}], is the rate of change of |
|
phase with respect to frequency. It can be computed as: |
|
|
|
d/dw H(e^-jw) |
|
g(w) = ------------- |
|
H(e^-jw) |
|
|
|
where |
|
H(z) = B(z)/A(z) = sum(b_k z^k)/sum(a_k z^k). |
|
|
|
By the quotient rule, |
|
A(z) d/dw B(z) - B(z) d/dw A(z) |
|
d/dw H(z) = ------------------------------- |
|
A(z) A(z) |
|
Substituting into the expression above yields: |
|
A dB - B dA |
|
g(w) = ----------- = dB/B - dA/A |
|
A B |
|
|
|
Note that, |
|
d/dw B(e^-jw) = sum(k b_k e**(-1j*wk)) |
|
d/dw A(e^-jw) = sum(k a_k e**(-1j*wk)) |
|
which is just the FFT of the coefficients multiplied by a ramp. |
|
|
|
As a further optimization when nfft>>length(a), the IIR filter (b,a) |
|
is converted to the FIR filter conv(b,fliplr(conj(a))). |
|
For further details, see |
|
http://ccrma.stanford.edu/~jos/filters/Numerical_Computation_Group_Delay.html |
|
|
|
""" |
|
if len(a) == 1 : return smithdelay(b,a,l=l,whole=whole) |
|
else : return shpakdelay(b,a,l=l,whole=whole) |
|
|
|
def smithdelay(b,a,l=512,whole=False): |
|
""" |
|
Group delay with Smith algorithm (J. O. Smith, 9-May-1988). |
|
It is accurate for FIR filter type I-IV. |
|
For another filter type please use shpakdelay(). |
|
See groupdelay() for more information. |
|
|
|
Inputs : |
|
b, a : FIR, IIR coefficients |
|
Outputs : |
|
w : Angular frequencies between 0 and pi |
|
g : Group delay |
|
|
|
""" |
|
s = {True:1,False:2}[whole] |
|
w = 2*pi/s*arange(l)/float(l) |
|
if 1 <= firtype(b) <= 4 and len(a) == 1: |
|
b = b/a[0] |
|
f = find(b) |
|
if len(f) != 0 : b,g = b[f],max(f[0], 0) |
|
else : b,g = 0, 0 |
|
gd = ones(l)*((len(b)-1)/2+g) |
|
else : |
|
c = convolve(b,a[::-1].conj()) |
|
cr = c*arange(c.shape[0]) |
|
|
|
if s*l >= len(c) : |
|
zz = zeros(s*l-len(c)) |
|
gd = fft(append(cr, zz))/fft(append(c, zz)) |
|
gd = gd[:l]-ones(l)*(len(a)-1) |
|
else : |
|
n = s*ceil(len(c)/(s*l)) |
|
m = l*n |
|
gd = fft(cr,m)/ fft(c,m) |
|
gd = gd[:m:n]-ones(l)*(len(a)-1) |
|
return w, gd |
|
|
|
def shpakdelay(b,a,l=512,whole=False,tol=1e-9): |
|
""" |
|
Group delay with Shpak algorithm (D. Shpak, 17-Feb-2000). |
|
See groupdelay() for more information. |
|
|
|
Inputs : |
|
b, a : FIR, IIR coefficients |
|
Outputs : |
|
w : Angular frequencies between 0 and pi |
|
g : Group delay |
|
|
|
""" |
|
def grpsect(sos,cw,sw,cw2,sw2) : |
|
gd = zeros_like(cw) |
|
bi = sos.real |
|
bq = sos.imag |
|
if (sos[1:3] == [0, 0]).all() : pass |
|
elif abs(sos[2]) < tol : |
|
if 'complex' not in str(sos.dtype) and abs(bi[0] - bi[1]) < tol : |
|
gd = 0.5 |
|
elif 'complex' not in str(sos.dtype) and abs(bi[0] + bi[1]) < tol : |
|
gd = 0.5 |
|
else : |
|
u = bq[0]*cw + bi[0]*sw + bq[1] |
|
v = bi[0]*cw - bq[0]*sw + bi[1] |
|
du = -bq[0]*sw + bi[0]*cw |
|
dv = -bi[0]*sw - bq[0]*cw |
|
u2v2 = (bi[0]**2 + bq[0]**2 + bi[1]**2 + bq[1]**2) +\ |
|
2*(bi[0]*bi[1] + bq[0]*bq[1])*cw + 2*(bi[0]*bq[1] - bi[1]*bq[0])*sw |
|
k = find(abs(u2v2) > tol) |
|
gd[k] = gd[k] + 1 - (v[k]*du[k]-u[k]*dv[k])/u2v2[k] |
|
else : |
|
if 'complex' not in str(sos.dtype) and (abs(bi - bi[::-1]) < tol).all() : |
|
gd = 1.0 |
|
elif 'complex' not in str(sos.dtype) and (abs(bi + bi[::-1]) < tol).all() : |
|
gd = 1.0 |
|
else : |
|
u = bq[0]*cw2 + bi[0]*sw2 + bq[1]*cw + bi[1]*sw + bq[2] |
|
v = bi[0]*cw2 - bq[0]*sw2 + bi[1]*cw - bq[1]*sw + bi[2] |
|
du = -2*bq[0]*sw2 + 2*bi[0]*cw2 - bq[1]*sw + bi[1]*cw |
|
dv = -1*( 2*bi[0]*sw2 + 2*bq[0]*cw2 + bi[1]*sw + bq[1]*cw) |
|
u2v2 = ( bi[0]**2 + bq[0]**2) + (bi[1]**2 + bq[1]**2) + (bi[2]**2 + bq[2]**2) + \ |
|
2*(bi[0]*bi[1] + bq[0]*bq[1] + bi[1]*bi[2] + bq[1]*bq[2])*cw + \ |
|
2*(bi[0]*bq[1] - bi[1]*bq[0] + bi[1]*bq[2] - bi[2]*bq[1])*sw + \ |
|
2*(bi[0]*bi[2] + bq[0]*bq[2])*cw2 + 2*(bi[0]*bq[2] - bi[2]*bq[0])*sw2 |
|
k = arange(len(v)) |
|
gd[k] = gd[k] + 2 - (v[k]*du[k] - u[k]*dv[k])/u2v2[k] |
|
return gd |
|
|
|
|
|
s = {True:1,False:2}[whole] |
|
w = 2*pi/s*arange(l)/float(l) |
|
cw,cw2 ,sw,sw2 = cos(w),cos(2*w), sin(w), sin(2*w) |
|
gd = zeros(l) |
|
|
|
if 'complex' not in str(a.dtype)+str(b.dtype) : |
|
sos, k = tf2sos(b,a) |
|
m = sos.shape[0] |
|
else : |
|
z,p,k = tf2zpk(b,a) |
|
fz = find(abs(z) > tol) |
|
gd = gd - (len(z)-len(fz)) |
|
z = z[fz] |
|
|
|
fp = find(abs(p) > tol) |
|
gd = gd + (len(p)-len(fp)) |
|
p = p[fp] |
|
|
|
fz = find(abs(abs(z) - 1) > tol) |
|
gd = gd + (len(z)-len(fz))/ 2 |
|
z = z[fz] |
|
|
|
fp = find(abs(abs(p) - 1) > tol) |
|
gd = gd - (len(p)-len(fp)) / 2 |
|
p = p[fp] |
|
|
|
nb,na = len(z), len(p) |
|
m = int(max(ceil(nb/2.0), ceil(na/2.0))) |
|
sos = zeros((m,6), dtype=a.dtype) |
|
sos[:,0] = 1 |
|
sos[:,3] = 1 |
|
sections = fix(nb/2.0) |
|
|
|
z1 = z[0:2*sections:2] |
|
z2 = z[1:2*sections:2] |
|
sos[:sections, 1:3] = array([-(z1+z2),z1*z2]).T |
|
|
|
if rem(nb, 2) == 1 : |
|
sos[sections,2] = -z[nb-1] |
|
|
|
sections = fix(na/2) |
|
z1 = p[0:2*sections:2] |
|
z2 = p[1:2*sections:2] |
|
sos[:sections,4:6] = array([-(z1+z2),z1*z2]).T |
|
if rem(na, 2) == 1 : |
|
sos[sections,4] = -p[na-1] |
|
|
|
for i in range(m): |
|
gd = gd + grpsect(sos[i, :3],cw,sw,cw2,sw2) |
|
gd = gd - grpsect(sos[i,3:6],cw,sw,cw2,sw2) |
|
return w, gd |
|
|
|
def cplxpair(x, tol=1e-12) : |
|
""" |
|
Sort the numbers z into complex conjugate pairs ordered by |
|
increasing real part. With identical real parts, order by |
|
increasing imaginary magnitude. Place the negative imaginary |
|
complex number first within each pair. Place all the real numbers |
|
after all the complex pairs (those with `abs (z.imag / z) < tol)', |
|
where the default value of TOL is `100 * EPS'. |
|
|
|
Inputs : |
|
x : input array. |
|
tol : toleranze of differenz of complex number and his conjugate pair. |
|
|
|
Outputs : y |
|
y : Complex conjugate pair |
|
""" |
|
|
|
if sum(x.shape) == 0 : return x |
|
if not 'complex' in str(x.dtype) : return x |
|
|
|
# Reshape input to 1D array to simplify algorithm |
|
x_shape = x.shape |
|
x = x.reshape(prod(array(list(x_shape)))) |
|
xout = array([]) |
|
tol = abs(tol) |
|
|
|
# Save original class of input |
|
x_orig_class = x[0].__class__ |
|
|
|
# New rule to sort |
|
class __cplxpairsort__ (x_orig_class) : |
|
def __gt__(self, a) : |
|
return self.real > a.real |
|
def __ge__(self, a) : |
|
return self.real > a.real or self.__eq__(a) |
|
def __lt__(self, a) : |
|
return self.real < a.real |
|
def __le__(self, a) : |
|
return self.real < a.real or self.__eq__(a) |
|
def __eq__(self, a) : |
|
return abs(self.real-a.real) <= tol and abs(self.imag+a.imag) <= tol |
|
def __ne__(self, a) : |
|
return not self.__eq__(a) |
|
|
|
def post_sort(x_sort): |
|
i = 0 |
|
pair = [] |
|
nopair = [] |
|
while True : |
|
re, im = x_sort[i].real - x_sort[i+1].real,x_sort[i].imag + x_sort[i+1].imag |
|
if abs(re) <= tol and abs(im) <= tol : |
|
pair.append(x_sort[i]) |
|
pair.append(x_sort[i+1]) |
|
i += 1 |
|
else : |
|
nopair.append(x_sort[i]) |
|
i += 1 |
|
if i >= len(x_sort)-1 : break |
|
if len(pair) + len(nopair) != len(x_sort) : nopair.append(x_sort[-1]) |
|
return append(pair, nopair) |
|
|
|
# Change dtype of input to pair |
|
x = x.astype(__cplxpairsort__) |
|
|
|
# Do it like multi-demension array with array sclicing. |
|
for i in range(x.shape[0]/x_shape[-1]): |
|
x_sort = 1*x[i*x_shape[-1] : (i+1)*x_shape[-1]] |
|
x_sort.sort() |
|
x_sort = post_sort(x_sort) |
|
xout = append(xout, 1*x_sort) |
|
|
|
# Return with original shape and original class |
|
return xout.reshape(x_shape).astype(x_orig_class) |
|
|
|
def cplxreal(z, tol=1e-12) : |
|
""" |
|
Split the vector z into its complex (zc) and real (zr) elements, |
|
eliminating one of each complex-conjugate pair. |
|
|
|
Inputs: |
|
z : row- or column-vector of complex numbers |
|
tol : tolerance threshold for numerical comparisons |
|
|
|
Ouputs: |
|
zc : elements of z having positive imaginary parts |
|
zr : elements of z having zero imaginary part |
|
|
|
Each complex element of Z is assumed to have a complex-conjugate |
|
counterpart elsewhere in Z as well. Elements are declared real if |
|
their imaginary parts have magnitude less than tol. |
|
|
|
Note : This function is modified from signal package for octave |
|
(http://octave.sourceforge.net/signal/index.html) |
|
""" |
|
if z.shape[0] == 0 : zc=[];zr=[] |
|
else : |
|
zcp = cplxpair(z) |
|
nz = len(z) |
|
nzrsec = 0 |
|
i = nz |
|
while i and abs(zcp[i-1].imag) < tol: |
|
zcp[i-1] = zcp[i-1].real |
|
nzrsec = nzrsec+1 |
|
i=i-1 |
|
|
|
nzsect2 = nz-nzrsec |
|
if nzsect2%2 != 0 : |
|
raise ValueError, 'cplxreal: Odd number of complex values!' |
|
|
|
nzsec = nzsect2/2 |
|
zc = zcp[1:nzsect2:2] |
|
zr = zcp[nzsect2:nz] |
|
return asarray(zc), asarray(zr) |
|
|
|
def zpk2sos(z,p,k) : |
|
""" |
|
Convert filter poles and zeros to second-order sections. |
|
|
|
Inputs: |
|
z : column-vector containing the filter zeros |
|
p : column-vector containing the filter poles |
|
k : overall filter gain factor |
|
|
|
Outputs: |
|
sos : matrix of series second-order sections, one per row: |
|
k : is an overall gain factor that effectively scales any |
|
one of the Bi vectors. |
|
|
|
Example: |
|
z,p,k = tf2zpk([1, 0, 0, 0, 0, 1],[1, 0, 0, 0, 0, .9]) |
|
sos,k = zp2sos(z,p,k) |
|
|
|
sos = |
|
1.0000 0.6180 1.0000 1.0000 0.6051 0.9587 |
|
1.0000 -1.6180 1.0000 1.0000 -1.5843 0.9587 |
|
1.0000 1.0000 0 1.0000 0.9791 0 |
|
|
|
k = |
|
1 |
|
|
|
See also: tf2sos zpk2tf tf2zpk sos2zpk sos2tf. |
|
|
|
Note : This function is modified from signal package for octave |
|
(http://octave.sourceforge.net/signal/index.html) |
|
""" |
|
zc,zr = cplxreal(array(z)) |
|
pc,pr = cplxreal(array(p)) |
|
|
|
nzc = len(zc) |
|
npc = len(pc) |
|
nzr = len(zr) |
|
npr = len(pr) |
|
|
|
# Pair up real zeros: |
|
if nzr : |
|
if nzr%2 == 1 : zr = append(zr,0); nzr=nzr+1 |
|
nzrsec = nzr/2.0 |
|
zrms = -zr[:nzr-1:2]-zr[1:nzr:2] |
|
zrp = zr[:nzr-1:2]*zr[1:nzr:2] |
|
else : |
|
nzrsec = 0 |
|
|
|
# Pair up real poles: |
|
if npr : |
|
if npr%2 == 1 : pr = append(pr,0); npr=npr+1 |
|
nprsec = npr/2.0 |
|
prms = -pr[:npr-1:2]-pr[1:npr:2] |
|
prp = pr[:npr-1:2]*pr[1:npr:2] |
|
else : |
|
nprsec = 0 |
|
|
|
nsecs = max(nzc+nzrsec,npc+nprsec) |
|
|
|
# Convert complex zeros and poles to real 2nd-order section form: |
|
zcm2r = -2*zc.real |
|
zca2 = abs(zc)**2 |
|
pcm2r = -2*pc.real |
|
pca2 = abs(pc)**2 |
|
|
|
sos = zeros((nsecs,6)) |
|
|
|
# all 2nd-order polynomials are monic |
|
sos[:,0] = ones(nsecs) |
|
sos[:,3] = ones(nsecs) |
|
nzrl = nzc+nzrsec # index of last real zero section |
|
nprl = npc+nprsec # index of last real pole section |
|
|
|
for i in range(int(nsecs)) : |
|
if i+1 <= nzc : # lay down a complex zero pair: |
|
sos[i,1:3] = append(zcm2r[i], zca2[i]) |
|
elif i+1 <= nzrl: #lay down a pair of real zeros: |
|
sos[i,1:3] = append(zrms[i-nzc], zrp[i-nzc]) |
|
if i+1 <= npc : # lay down a complex pole pair: |
|
sos[i,4:6] = append(pcm2r[i], pca2[i]) |
|
elif i+1 <= nprl: # lay down a pair of real poles: |
|
sos[i,4:6] = append(prms[i-npc], prp[i-npc]) |
|
|
|
if len(sos.shape) == 1 : sos = array([sos]) |
|
return sos, k |
|
|
|
def mtf2zpk(b,a): |
|
b = (b+0.0) / a[0] |
|
a = (a+0.0) / a[0] |
|
k = b[0] |
|
b = b/b[0] |
|
z = roots(b) |
|
p = roots(a) |
|
return z, p, k |
|
|
|
def tf2sos(b,a): |
|
""" |
|
Convert direct-form filter coefficients to series second-order |
|
sections. |
|
|
|
INPUTS: |
|
b , a : filter coefficients |
|
|
|
Outputs: |
|
sos : matrix of series second-order sections, one per row: |
|
k : is an overall gain factor that effectively scales any |
|
one of the Bi vectors. |
|
|
|
EXAMPLE: |
|
sos,k = tf2sos([1, 0, 0, 0, 0, 1],[1, 0, 0, 0, 0, .9]) |
|
|
|
sos = |
|
|
|
1.00000 0.61803 1.00000 1.00000 0.60515 0.95873 |
|
1.00000 -1.61803 1.00000 1.00000 -1.58430 0.95873 |
|
1.00000 1.00000 -0.00000 1.00000 0.97915 -0.00000 |
|
|
|
k = 1 |
|
|
|
|
|
See also: zpk2sos zpk2tf tf2zpk sos2zpk sos2tf. |
|
|
|
Note : This function is modified from signal package for octave |
|
(http://octave.sourceforge.net/signal/index.html) |
|
""" |
|
b = asarray(b) |
|
a = asarray(a) |
|
b = b[abs(b) > 0] |
|
a = a[abs(a) > 0] |
|
z,p,k = mtf2zpk(b, a) |
|
return zpk2sos(z,p,k) |
|
|
|
def sos2zpk(sos,k=1): |
|
""" |
|
Convert series second-order sections to zeros, poles, and gains |
|
(pole residues). |
|
|
|
Inputs: |
|
sos : matrix of series second-order sections, one per row: |
|
k : is an overall gain factor that effectively scales any |
|
one of the Bi vectors. |
|
|
|
Outputs : |
|
z : column-vector containing the filter zeros |
|
p : column-vector containing the filter poles |
|
k : overall filter gain factor |
|
|
|
Example: |
|
z,p,k = sos2zpk([[1,0,1,1,0,-0.81],[1,0,0,1,0,0.49]]) |
|
|
|
z = [ 0.+1.j 0.-1.j 0.+0.j 0.+0.j] |
|
p = [-0.9+0.j 0.9+0.j 0.0+0.7j 0.0-0.7j] |
|
k = 1 |
|
|
|
See also: zpk2sos sos2tf tf2sos zpk2tf tf2zpk. |
|
|
|
Note : This function is modified from signal package for octave |
|
(http://octave.sourceforge.net/signal/index.html) |
|
""" |
|
b,a = sos2tf(sos, k) |
|
return mtf2zpk(b,a) |
|
|
|
def sos2tf(sos, k=1) : |
|
""" |
|
Convert series second-order sections to direct form. |
|
|
|
Inputs : |
|
sos : matrix of series second-order sections, one per row: |
|
k : is an overall gain factor that effectively scales any |
|
one of the Bi vectors. |
|
|
|
|
|
Outputs : |
|
b , a : filter coefficients |
|
|
|
See also: tf2sos zpk2sos sos2zpk zpk2tf tf2zpk. |
|
|
|
Note : This function is modified from signal package for octave |
|
(http://octave.sourceforge.net/signal/index.html) |
|
""" |
|
sos = array(sos) |
|
n,m = sos.shape |
|
if m!=6 : raise ValueError, 'sos2tf: sos matrix should be N by 6' |
|
a,b = [1],[1] |
|
for i in range(n) : b = convolve(b, sos[i, :3]); a = convolve(a, sos[i,3:6]); |
|
b = b[abs(b) > 0] |
|
a = a[abs(b) > 0] |
|
return k*b, a |
|
|
|
def remezord(bands, desired, dev, devmode='scalar'): |
|
""" |
|
Order estimation for Remez exchange algorithm optimal FIR filter. |
|
|
|
Inputs : |
|
bands : A montonic sequence containing the band edges. All elements |
|
must be non-negative and less than 1 the sampling frequency |
|
as given in pi units. |
|
desired : A sequency half the size of bands containing the desired gain |
|
in each of the specified bands |
|
dev : Allowable deviation or ripples between the frequency response |
|
and the desired amplitude of the output filter for each band |
|
devmode : [Optional] default = 'scalar' : mode of deviation. |
|
If it is set to 'dB', dev will be converted to scalar, |
|
before order estimation |
|
Outputs : |
|
m : estimated order |
|
w : weigting factor for Remez exchange algorithm |
|
|
|
Example : |
|
m, w = remezord([0.0, 0.2, 0.3, 1.0], [1,0], [0.25, 50], devmode='dB') |
|
print 'm=%d, w=%s'%(m,str(w)) |
|
m= 43, w=[ 1. 4.48601475] |
|
|
|
Estimate order and weighting factors for Remez exchange algorithm for lowpass filter |
|
with passband == [0,0.2*pi], stopband == [0.3*pi, pi], |
|
passband ripple = 0.25dB, stopband autenuation = 50dB. |
|
|
|
see also firremez, remez |
|
""" |
|
def db2delta(Rp, As): |
|
d1 = (1-10**(Rp/-20.0))/(1+10**(Rp/-20.0)) |
|
d2 = 10**(As/-20.0)*(1+d1) |
|
return d1, d2 |
|
|
|
estord = lambda f1, f2, d1, d2 : (-20*log10((d1*d2)**0.5)-13)/(2.285*(f2*pi-f1*pi))+1 |
|
|
|
bands, dev = array(bands).astype(float64), array(dev).astype(float64) |
|
n = bands.shape[0] |
|
|
|
if devmode not in ['scalar', 'dB'] : raise ValueError, "Mode must be 'dB', or 'scalar'" |
|
if n%2 == 1 : raise ValueError, 'The input bands must have even length.' |
|
if max(bands) > 1 : raise ValueError, 'Band edges should be less than 1.' |
|
if not (bands[1:] - bands[:-1] > 0).all() or bands[0] != 0 : raise ValueError, 'Bands must be monotonic starting at zero.' |
|
if len(desired*2) != len(bands): raise ValueError, 'The input bands must have twice the length of desired.' |
|
if dev.shape[0]*2 != n: raise ValueError, 'The input bands must have twice the length of deviations.' |
|
|
|
if devmode == 'dB' : |
|
devo = zeros_like(dev) |
|
for i in range(len(dev)-1) : |
|
if desired[i] > desired[i+1]: |
|
devo[i], devo[i+1] =db2delta(dev[i], dev[i+1]) |
|
else : |
|
devo[i+1], devo[i] =db2delta(dev[i+1], dev[i]) |
|
dev = devo[:] |
|
|
|
if n == 4 : m = [estord(bands[1], bands[2], dev[0], dev[1])] |
|
else : m = [estord(bands[2*i+1], bands[2*i+2], dev[i], dev[i+1]) for i in range(len(dev)-1)] |
|
w = ones(dev.shape[0])*max(dev)/dev |
|
|
|
return int(max(m)),w |
|
|
|
def firremez(bands, desired, dev, devmode='scalar', m=None): |
|
""" |
|
Calculate the minimax optimal for FIR filter using Remez exchange algorithm. |
|
|
|
Inputs : |
|
bands : A montonic sequence containing the band edges. All elements |
|
must be non-negative and less than 1 the sampling frequency |
|
as given in pi units. |
|
desired : A sequency half the size of bands containing the desired gain |
|
in each of the specified bands |
|
dev : Allowable deviation or ripples between the frequency response |
|
and the desired amplitude of the output filter for each band |
|
devmode : [Optional] default = 'scalar' : mode of deviation. |
|
If it is set to 'dB', dev will be converted to scalar, |
|
before order estimation |
|
m : [Optional] If m != None, the order is not etimated but set to m. |
|
Outputs : |
|
h : Inpulse response of calculated FIR filter. |
|
m : estimated order. |
|
|
|
Example : |
|
h, m = firremez([0.0, 0.2, 0.3, 1.0],[1,0],[0.25, 50], devmode='dB') |
|
|
|
Calculate impulse response for Remez exchange algorithm for lowpass filter |
|
with passband == [0,0.2*pi], stopband == [0.3*pi, pi], |
|
passband ripple = 0.25dB, stopband autenuation = 50dB. |
|
|
|
see also remezord, remez |
|
""" |
|
m_est, w = remezord(bands, desired, dev, devmode) |
|
if m == None : m = m_est |
|
h = remez(m, array(bands)/2.0, desired, w) |
|
return h, m |
|
|
|
def firfreq(m, bands, desired, l=512, width=None, window='hamming'): |
|
""" |
|
FIR filter design using frequency sampling method. |
|
|
|
Inputs : |
|
m : oder of FIR filter |
|
|
|
bands : A montonic sequence containing the band edges. All elements |
|
must be non-negative and less than 1 the sampling frequency |
|
as given in pi units. |
|
|
|
desired : A sequency with the same size of bands containing the desired gain |
|
in each of the specified bands |
|
|
|
l : Length of frequency response, which is used in frequecy domain. |
|
|
|
width : if width is not None, then assume it is the approximate width of |
|
the transition region (normalized so that 1 corresonds to pi) |
|
for use in kaiser FIR filter design. |
|
|
|
window : desired window to use. |
|
|
|
Outputs : |
|
h : coefficients of length m+1 fir filter. |
|
|
|
Example : |
|
|
|
h = firfreq(50, [0,0.2,0.3,1.0], [1,1,0,0]) |
|
|
|
Calculate impulse response for 51 tabs lowpass filter using frequency sampling method |
|
with passband == [0,0.2*pi], stopband == [0.3*pi, pi] |
|
|
|
Note : This function is modified from signal package for octave |
|
(http://octave.sourceforge.net/signal/index.html) |
|
""" |
|
if len(bands) != len(desired) : raise ValueError, 'Bands and desired must have the same length.' |
|
if not (bands[1:] - bands[:-1] > 0).all() or bands[0] != 0 : raise ValueError, 'Bands must be monotonic starting at zero.' |
|
if isinstance(width,float): |
|
A = 2.285*m*width + 8 |
|
if (A < 21): beta = 0.0 |
|
elif (A <= 50): beta = 0.5842*(A-21)**0.4 + 0.07886*(A-21) |
|
else: beta = 0.1102*(A-8.7) |
|
window=('kaiser',beta) |
|
else : |
|
width = l/20.0 |
|
|
|
bands, desired = array(bands), array(desired) |
|
win = get_window(window,m+1,fftbins=1) |
|
l = float(l) |
|
|
|
# make sure grid is big enough for the window |
|
if 2*l < m+1 : l = 2**(ceil(log(10)/log(2))) |
|
|
|
# Apply ramps to discontinuities |
|
if width > 0 : |
|
# remember original frequency points prior to applying ramps |
|
basef = bands[:] |
|
basem = desired[:] |
|
|
|
# separate identical frequencies, but keep the midpoint |
|
idx = find (diff(bands) == 0) |
|
bands[idx] = bands[idx] - width/l/2.0 |
|
bands[idx+1] = bands[idx+1] + width/l/2.0 |
|
bands = append(bands, basef[idx]) |
|
|
|
# make sure the grid points stay monotonic in [0,1] |
|
bands[bands<0] = 0 |
|
bands[bands>1] = 1 |
|
bands = unique(append(bands, basef[idx])) |
|
|
|
# preserve window shape even though f may have changed |
|
desired = interp1d(basef, basem)(bands) |
|
|
|
# interpolate between grid points |
|
grid = interp1d(bands, desired)(linspace(0,1,l+1)) |
|
|
|
# Transform frequency response into time response and |
|
# center the response about n/2, truncating the excess |
|
if rem(m,2) == 0 : |
|
b = ifft(append(grid , grid[l-1:2:-1])) |
|
mid = (m+1)/2.0 |
|
b = append( b[-floor(mid):], b[:ceil(mid)] ).real |
|
else : |
|
# Add zeros to interpolate by 2, then pick the odd values below. |
|
b = ifft(append(grid, append(zeros(l*2), grid[l-1:2:-1]))) |
|
b = 2 * append(b[-n::2], b[1:n+1:2]).real |
|
return b*win |
|
|
|
def firls(m, bands, desired, weight=None): |
|
""" |
|
FIR filter design using least squares method. |
|
|
|
Inputs : |
|
m : oder of FIR filter |
|
|
|
bands : A montonic sequence containing the band edges. All elements |
|
must be non-negative and less than 1 the sampling frequency |
|
as given in pi units. |
|
|
|
desired : A sequency with the same size of bands containing the desired gain |
|
in each of the specified bands |
|
weight : A relative weighting to give to each band region. |
|
|
|
Output : |
|
h : coefficients of length m+1 fir filter. |
|
|
|
Example : |
|
h = firls(50, [0,0.2,0.3,1.0], [1,1,0,0],[1,5.0]) |
|
|
|
Calculate impulse response for 51 tabs lowpass filter using least squares method |
|
with passband == [0,0.2*pi], stopband == [0.3*pi, pi], |
|
weight to passband == 1, weight to stopband == 5 |
|
|
|
Note : This function is modified from signal package for octave |
|
(http://octave.sourceforge.net/signal/index.html) |
|
""" |
|
|
|
bands, desired, weight = array(bands), array(desired), array(weight) |
|
if weight==None : weight = ones(len(bands)/2) |
|
|
|
M = m/2; |
|
w = kron(weight, [-1,1]) |
|
omega = bands * pi |
|
i1 = arange(1,M+1) |
|
|
|
# generate the matrix q |
|
# as illustrated in the above-cited reference, the matrix can be |
|
# expressed as the sum of a hankel and toeplitz matrix. a factor of |
|
# 1/2 has been dropped and the final filter hficients multiplied |
|
# by 2 to compensate. |
|
cos_ints = append(omega, sin(mat(arange(1,m+1)).T*mat(omega))).reshape((-1,omega.shape[0])) |
|
q = append(1, 1.0/arange(1.0,m+1)) * array(mat(cos_ints) * mat(w).T).T[0] |
|
q = toeplitz(q[:M+1]) + hankel(q[:M+1], q[M : ]) |
|
|
|
# the vector b is derived from solving the integral: |
|
# |
|
# _ w |
|
# / 2 |
|
# b = / w(w) d(w) cos(kw) dw |
|
# k / w |
|
# - 1 |
|
# |
|
# since we assume that w(w) is constant over each band (if not, the |
|
# computation of q above would be considerably more complex), but |
|
# d(w) is allowed to be a linear function, in general the function |
|
# w(w) d(w) is linear. the computations below are derived from the |
|
# fact that: |
|
# _ |
|
# / a ax + b |
|
# / (ax + b) cos(nx) dx = --- cos (nx) + ------ sin(nx) |
|
# / 2 n |
|
# - n |
|
# |
|
|
|
|
|
enum = append(omega[::2]**2 - omega[1::2]**2, cos(mat(i1).T * mat(omega[1::2])) - cos(mat(i1).T * mat(omega[::2]))).flatten() |
|
deno = mat(append(2, i1)).T * mat(omega[1::2] - omega[::2]) |
|
cos_ints2 = enum.reshape(deno.shape)/array(deno) |
|
|
|
d = zeros_like(desired) |
|
d[::2] = -weight * desired[::2] |
|
d[1::2] = weight * desired[1::2] |
|
|
|
b = append(1, 1.0/i1) * array(mat(kron (cos_ints2, [1, 1]) + cos_ints[:M+1,:]) * mat(d).T)[:,0] |
|
|
|
# having computed the components q and b of the matrix equation, |
|
# solve for the filter hficients. |
|
a = (array(inv(q)*mat(b).T).T)[0] |
|
h = append( a[:0:-1], append(2*a[0], a[1:])) |
|
|
|
return h |