Written by Kittipong Piyawanno project.dictator@ximplesoft.com, who posted them on SciPy bug tracking system: scipy/scipy#1175 and they were then lost
These are translations of Octave code and are therefore GPL, and also copyright the people who originally wrote each function for Octave.
Beware, I think I encountered bugs while using this, but I forget where.
| 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 |