dump it on gist in case I never finish it
scipy's functions use tf representation instead of zpk or ss, making this kind of pointless so far because high-order filters can't be generated in the first place
I plan to rewrite this from scratch so it's not GPL and can be included in scipy? maybe?
| # First part: | |
| # Copyright (C) 2005 Julius O. Smith III <jos@ccrma.stanford.edu> | |
| # | |
| # This program is free software; you can redistribute it and/or modify it under | |
| # the terms of the GNU General Public License as published by the Free Software | |
| # Foundation; either version 3 of the License, or (at your option) any later | |
| # version. | |
| # | |
| # This program is distributed in the hope that it will be useful, but WITHOUT | |
| # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or | |
| # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more | |
| # details. | |
| # | |
| # You should have received a copy of the GNU General Public License along with | |
| # this program; if not, see <http://www.gnu.org/licenses/>. | |
| # (This is a direct translation of GPL code and cannot be included in SciPy as-is) | |
| # Translated by endolith@gmail.com | |
| # Also translated by Kittipong Piyawanno <project.dictator@ximplesoft.com> | |
| # though that version has some bugs http://projects.scipy.org/scipy/ticket/648 | |
| from __future__ import division | |
| from scipy.signal import tf2zpk, convolve, lfilter | |
| from numpy import asarray, asfarray, array, atleast_2d, trim_zeros, empty, \ | |
| roots, prod, append, rank, spacing, ones, remainder, \ | |
| isrealobj, atleast_1d, sort, conj, mean | |
| from numpy.random import rand | |
| def tf2sos(b, a): | |
| """ | |
| Convert direct-form filter coefficients to series second-order | |
| sections. | |
| Parameters | |
| ---------- | |
| b, a : array_like | |
| [copy from sos2tf] | |
| Returns | |
| ------- | |
| sos : array_like | |
| [copy from sos2tf] | |
| b0 must be nonzero for each section (zeros at infinity not supported). | |
| k : float | |
| [copy from sos2tf] | |
| Examples | |
| -------- | |
| >>> B = array([1, 0, 0, 0, 0, 1]) | |
| >>> A = array([1, 0, 0, 0, 0, .9]) | |
| >>> sos, k = tf2sos(B, A) | |
| >>> sos | |
| array([[ 1. , 0.618 , 1. , 1. , 0.6051, 0.9587], | |
| [ 1. , -1.618 , 1. , 1. , -1.5843, 0.9587], | |
| [ 1. , 1. , -0. , 1. , 0.9791, -0. ]]) | |
| >>> k | |
| 1.0 | |
| See Also | |
| -------- | |
| sos2tf, zp2sos, sos2pz, zp2tf, tf2zp | |
| """ | |
| b = asarray(b) | |
| a = asarray(a) | |
| z, p, k = tf2zpk(b, a) | |
| sos, k = zpk2sos(z, p, k) | |
| return sos, k | |
| def sos2tf(sos, k=1): | |
| """ | |
| Convert series second-order sections to direct form H(z) = B(z)/A(z). | |
| Parameters | |
| ---------- | |
| sos : array_like | |
| array of series second-order sections, one per row: | |
| sos = [[B0, A0], [B1, A1], ... [Bn, An]], where | |
| B1 == [b0, b1, b2] and A1 == [1, a1, a2] for | |
| section 1, etc. | |
| b0 must be nonzero for each section. | |
| See `lfilter` for documentation of the | |
| second-order direct-form filter coefficients bn and an. | |
| k : float | |
| overall gain factor that effectively scales the output `B` vector (or | |
| any one of the input Bi vectors). | |
| Returns | |
| ------- | |
| b : ndarray | |
| Numerator polynomial of the IIR filter H(z). | |
| a : ndarray | |
| Denominator polynomial of the IIR filter H(z). | |
| Examples | |
| -------- | |
| >>> b, a = sos2tf([[1, 0, 1, 1, 0, -0.81], [1, 0, 0, 1, 0, 0.49]]) | |
| >>> b | |
| array([ 1., 0., 1.]) | |
| >>> a | |
| array([ 1. , 0. , -0.32 , 0. , -0.3969]) | |
| See Also | |
| -------- | |
| tf2sos, zp2sos, sos2pz, zp2tf, tf2zp | |
| """ | |
| sos = atleast_2d(asarray(sos)) | |
| N, M = sos.shape | |
| if M != 6: | |
| raise ValueError('sos array should be N by 6') | |
| a = array([1]) | |
| b = array([1]) | |
| for i in range(N): | |
| b = convolve(b, sos[i, :3]) | |
| a = convolve(a, sos[i, 3:]) | |
| b = trim_zeros(b, 'b') | |
| a = trim_zeros(a, 'b') | |
| b *= k | |
| return b, a | |
| def sos2zpk(sos, k=1): | |
| """ | |
| Return zero, pole, gain (z,p,k) representation from a series second-order | |
| sections ("SOS") representation of a linear filter. | |
| (pole residues). | |
| Parameters | |
| ---------- | |
| [copy from sos2tf] | |
| Returns | |
| ------- | |
| z : ndarray | |
| Zeros of the IIR filter transfer function (roots of B(z)). | |
| p : ndarray | |
| Poles of the IIR filter transfer function (roots of A(z)). | |
| k : float | |
| Overall system gain of the IIR filter transfer function (= B(Inf)). TODO: WTF | |
| Examples | |
| -------- | |
| >>> z, p, k = sos2zpk([[1, 0, 1, 1, 0, -0.81], [1, 0, 0, 1, 0, 0.49]]) | |
| >>> z | |
| array([ 0.+1.j, 0.-1.j, 0.+0.j, 0.+0.j]) | |
| >>> p | |
| array([ 0.9+0.j , -0.9+0.j , 0.0+0.7j, 0.0-0.7j]) | |
| >>> k | |
| 1.0 | |
| See Also | |
| -------- | |
| zp2sos, sos2tf, tf2sos, zp2tf, tf2zp | |
| """ | |
| sos = atleast_2d(asfarray(sos)) # TODO else the following lines won't work for 1D array or list, right? chec kfor 3d? | |
| gains = sos[:, 0] # All b0 coefficients | |
| k = prod(gains) * k # pole-zero gain | |
| if k == 0: | |
| raise ValueError('One or more section gains is zero') | |
| N, M = sos.shape | |
| if M != 6: | |
| raise ValueError('Array sos should be N by 6') | |
| sos[:, :3] /= array([gains, gains, gains]).T | |
| z = empty(2 * N, dtype=complex) | |
| p = empty(2 * N, dtype=complex) | |
| for i in range(N): | |
| # every 2 rows [0:2], [2:4], ... | |
| zi = roots(sos[i, :3]) | |
| z[2*i:2*(i+1)] = zi | |
| pi = roots(sos[i, 3:]) | |
| p[2*i:2*(i+1)] = pi | |
| return z, p, k | |
| def zpk2sos(z=None, p=array([]), k=1): | |
| """ | |
| Zero-pole-gain representation to second-order sections representation | |
| Return the series second-order section (SOS) representation of a linear | |
| filter from its zero, pole, gain (ZPK) representation. | |
| Parameters | |
| ---------- | |
| [copy from sos2zpk] | |
| Returns | |
| ------- | |
| [copy from sos2tf] | |
| b0 must be nonzero for each section.@* | |
| See @code{filter()} for documentation of the | |
| second-order direct-form filter coefficients `B`i and | |
| %`A`i, i=1:N. | |
| k : float | |
| [copy from others] | |
| overall gain factor that effectively scales | |
| any one of the `B`i vectors. | |
| Examples | |
| -------- | |
| >>> z,p,k = tf2zpk([1, 0, 0, 0, 0, 1],[1, 0, 0, 0, 0, .9]) | |
| >>> sos,k = zpk2sos(z, p, k) | |
| >>> sos | |
| array([[ 1. , 0.618 , 1. , 1. , 0.6051, 0.9587], | |
| [ 1. , -1.618 , 1. , 1. , -1.5843, 0.9587], | |
| [ 1. , 1. , -0. , 1. , 0.9791, -0. ]]) | |
| >>> k | |
| 1.0 | |
| See Also | |
| -------- | |
| sos2pz, sos2tf, tf2sos, zp2tf, tf2zp | |
| """ | |
| zc, zr = cplxreal(asarray(z)) | |
| pc, pr = cplxreal(asarray(p)) | |
| # zc,zr,pc,pr | |
| 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 = int(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 = empty((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 | |
| # prms, prp, zrms, zrp # TODO: are these always real? cast from complex warning | |
| for i in range(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 rank(sos) == 1: | |
| sos = array([sos]) | |
| return sos, k | |
| # TODO: array almost equal uses decimal=6, so maybe do that? that seems like a better idea, according to https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ | |
| def cplxreal(z, thresh = 100*spacing(1)): | |
| """ | |
| Split the vector z into its complex (`zc`) and real (`zr`) elements, | |
| eliminating one of each complex-conjugate pair. | |
| Parameters | |
| ---------- | |
| z : array_like | |
| vector of complex numbers | |
| thresh : float | |
| tolerance threshold for numerical comparisons (default = 100*eps) | |
| Returns | |
| ------- | |
| zc : ndarray | |
| elements of `z` having positive imaginary parts | |
| zr : ndarray | |
| 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 `thresh`. | |
| See Also | |
| -------- | |
| cplxpair | |
| """ | |
| z = asarray(z) | |
| if z.size == 0: | |
| return array([]), array([]) | |
| zcp = cplxpair(z) # sort complex pairs, real roots at end | |
| nz = len(z) | |
| nzrsec = 0 | |
| # determine number of real values | |
| i = nz | |
| while i and abs(zcp[i-1].imag) < thresh: | |
| zcp[i-1] = zcp[i-1].real | |
| nzrsec = nzrsec + 1 | |
| i = i - 1 | |
| nzsect2 = nz - nzrsec | |
| if remainder(nzsect2, 2) != 0: | |
| raise ValueError('Odd number of complex values!') | |
| zc = zcp[1:nzsect2:2] | |
| zr = zcp[nzsect2:nz] | |
| return zc, zr # asarray? | |
| ##################################################### | |
| # Everything below this written by myself and probably BSD licensed for scipy: | |
| def cplxpair(x, tol=3): | |
| """ | |
| Sort `x` into pairs of complex conjugates. | |
| Currently this is for 1D arrays only. | |
| Complex conjugates are sorted by increasing real part. In each pair, the | |
| number with negative imaginary part appears first. | |
| If pairs have identical real parts, they are sorted by decreasing | |
| imaginary magnitude. TODO: Fix this | |
| Two complex numbers are considered a conjugate pair if their real and | |
| imaginary parts differ in magnitude by less than `tol`. The conjugate | |
| pairs are forced to be exact complex conjugates by averaging the real and | |
| imaginary parts. | |
| Purely real numbers are also sorted, but placed after the complex | |
| conjugate pairs. A number is considered real if its imaginary part is | |
| smaller than `tol`. | |
| Parameters | |
| ---------- | |
| x : array_like | |
| Input array to be sorted | |
| tol : float | |
| Tolerance used for testing equality of conjugate pairs and | |
| negligibility of imaginary part for real numbers | |
| Returns | |
| ------- | |
| y : ndarray | |
| Complex conjugate pairs followed by real numbers | |
| Raises | |
| ------ | |
| ValueError | |
| If there are any complex numbers in `x` for which a conjugate pair | |
| cannot be found. | |
| See Also | |
| -------- | |
| cplxreal: Splits the real and complex pair components of the input | |
| """ | |
| # TODO: maybe use this enhanced version instead? | |
| # http://www.mathworks.com/matlabcentral/fileexchange/8037-more-flexible-sorting-and-multiplicity-of-roots-of-a-polynomial/content/cmplx_roots_sort.m | |
| # TODO: N-D arrays, dim parameter. "By default the complex pairs are | |
| # sorted along the first non-singleton dimension of z. If dim is | |
| # specified, then the complex pairs are sorted along this dimension." | |
| # TODO: proper float comparison | |
| # "those with abs (imag (z) / z) < tol)" | |
| # "(those with `abs (z.imag / z) < tol)'," | |
| x = sort(atleast_1d(x)) | |
| if x.size == 0 or isrealobj(x): | |
| return x | |
| # TODO: Do the tolerance testing correctly, using spacing(max(a,b)) or something | |
| def conjpair(a, b): | |
| """a and b are approximately a complex conjugate pair""" | |
| if a == conj(b): | |
| return True | |
| elif a * b == 0: | |
| return abs(a - conj(b)) < tol * spacing(abs(a)) | |
| else: | |
| return abs(a - conj(b)) / (abs(a) + abs(b)) < tol * spacing(abs(a)) | |
| def iscomplex(a): | |
| """Imaginary part of input is non-negligible""" | |
| if a.imag == 0: | |
| return False | |
| else: | |
| return abs(a.imag) > tol * spacing(abs(a)) | |
| indices = abs(x.imag) < tol * spacing(abs(x)) | |
| reals = x[indices] | |
| x = x[~indices] #complex numbers | |
| # Pair up conjugates | |
| blacklist = set() # Indices we've already found matches for | |
| output = empty(len(x), complex) | |
| n = 0 | |
| for a in xrange(len(x)): | |
| if a not in blacklist: | |
| # Find pair of x[a] | |
| if iscomplex(x[a]): | |
| blacklist.add(a) | |
| for b in xrange(a + 1, len(x)): | |
| if b in blacklist: | |
| continue | |
| elif conjpair(x[a], x[b]): | |
| # x[a] and x[b] are conjugate pairs | |
| # Force the pair to be exactly equal | |
| repart = mean([x[a].real, x[b].real]) | |
| impart = 1j * mean([abs(x[a].imag), abs(x[b].imag)]) | |
| # Number with negative imag part first | |
| output[n:n+2] = repart - impart, repart + impart | |
| n += 2 | |
| blacklist.add(b) | |
| break | |
| else: | |
| raise ValueError('Array contains complex value ' + str(x[a]) + | |
| ' with no matching conjugate') | |
| assert n == len(x), "Sorry, you've found a bug in the cplxpair() loop" | |
| return append(output, reals) | |
| def sosfilt(sos, x, axis=-1, zi=None): | |
| """ | |
| Filter `x` using second-order sections | |
| Filter a data sequence, `x`, using a digital filter. This works for many | |
| fundamental data types (including Object type). The filter is a direct | |
| form II transposed implementation of the standard difference equation | |
| (see Notes). | |
| Parameters | |
| ---------- | |
| sos : array_like | |
| [copy from sos2tf] | |
| The second order section filter is described by the matrix sos with: | |
| [ B1 A1 ] | |
| sos = [ ... ], | |
| [ BN AN ] | |
| where B1=[b0 b1 b2] and A1=[1 a1 a2] for section 1, etc. | |
| b0 must be nonzero for each section. | |
| If a[0] | |
| is not 1, then both a and b are normalized by a[0]. | |
| x : array_like | |
| An N-dimensional input array. | |
| axis : int | |
| The axis of the input data array along which to apply the | |
| linear filter. The filter is applied to each subarray along | |
| this axis (*Default* = -1) | |
| zi : array_like (optional) | |
| Initial conditions for the filter delays. It is a vector | |
| (or array of vectors for an N-dimensional input) of length TODO FIX THIS | |
| max(len(a),len(b))-1. If zi=None or is not given, then initial | |
| rest is assumed. SEE signal.lfiltic for more information. | |
| Returns | |
| ------- | |
| y : array | |
| The output of the digital filter. | |
| zf : array (optional) | |
| If zi is None, this is not returned, otherwise, zf holds the | |
| final filter delay values. | |
| Notes | |
| ----- | |
| This is implemented simply by performing `lfilter` for each second-order | |
| section. See `lfilter` for implementation details. | |
| """ | |
| sos = atleast_2d(sos) | |
| n, m = sos.shape | |
| for i in range(n): | |
| if zi is None: | |
| x = lfilter(sos[i, :3], sos[i, 3:], x, axis) | |
| else: | |
| x, zf = lfilter(sos[i, :3], sos[i, 3:], x, axis, zi) | |
| return x | |
| if __name__ == "__main__": | |
| from numpy.testing import assert_array_almost_equal, assert_raises | |
| from numpy.random import shuffle | |
| from numpy import arange, pi, exp, sin, copy | |
| # cplxpair tests | |
| assert (cplxpair([]).size == 0) | |
| assert (cplxpair(1) == 1) | |
| assert_array_almost_equal(cplxpair([1+1j, 1-1j]), [1-1j, 1+1j]) | |
| a = [1+1j, 1+1j, 1, 1-1j, 1-1j, 2] | |
| b = [1-1j, 1+1j, 1-1j, 1+1j, 1, 2] | |
| assert_array_almost_equal(cplxpair(a), b) | |
| assert_array_almost_equal(cplxpair([0, 1, 2]), [0, 1, 2]) | |
| # Example from Mathnium, though they sort by real part only and don't | |
| # put the purely real numbers at the end | |
| c = [ | |
| 5.0921 - 0j, | |
| 0.4166 + 0.2604j, | |
| 0.5133 - 0j, | |
| 0.4166 - 0.2604j, | |
| -1.1964 - 0j, | |
| 0.0841 - 0j, | |
| -0.4472 + 0.2427j, | |
| -0.7675 + 0j, | |
| -0.4472 - 0.2427j, | |
| -0.4242 - 0j, | |
| ] | |
| c2 = [ | |
| -0.4472 - 0.2427j, | |
| -0.4472 + 0.2427j, | |
| 0.4166 - 0.2604j, | |
| 0.4166 + 0.2604j, | |
| -1.1964 - 0j, | |
| -0.7675 + 0j, | |
| -0.4242 - 0j, | |
| 0.0841 - 0j, | |
| 0.5133 - 0j, | |
| 5.0921 - 0j, | |
| ] | |
| assert_array_almost_equal(c2, cplxpair(c)) | |
| z = exp(2j*pi*array([4, 3, 5, 2, 6, 1, 0])/7) | |
| z1 = copy(z) | |
| shuffle(z) | |
| assert_array_almost_equal(cplxpair(z), z1) | |
| shuffle(z) | |
| assert_array_almost_equal(cplxpair(z), z1) | |
| shuffle(z) | |
| assert_array_almost_equal(cplxpair(z), z1) | |
| # tolerance test | |
| assert_array_almost_equal(cplxpair([1j, -1j, 1+(1j*spacing(1))], 2), | |
| [-1j, 1j, 1+(1j*spacing(1))]) | |
| # sorting close to 0 | |
| assert_array_almost_equal(cplxpair([-spacing(1)+1j, +spacing(1)-1j]), | |
| [0 - 1j, -0 + 1j]) | |
| assert_array_almost_equal(cplxpair([+spacing(1)+1j, -spacing(1)-1j]), | |
| [-0 - 1j, 0 + 1j]) | |
| assert_array_almost_equal(cplxpair([0+1j, 0-1j]), | |
| [0 - 1j, 0 + 1j]) | |
| # Should be able to pair up all the conjugates | |
| x = rand(10000) + 1j * rand(10000) | |
| y = conj(x) | |
| z = rand(10000) | |
| x = append(append(x, y), z) | |
| shuffle(x) | |
| #timeit cplxpair(x) # 738 ms vs 13 seconds for Octave's function | |
| c = cplxpair(x) | |
| # Every other element of head should be conjugates: | |
| assert_array_almost_equal(c[0:20000:2], conj(c[1:20000:2])) | |
| # Real parts should be in sorted order: | |
| assert_array_almost_equal(c[0:20000:2].real, sort(c[0:20000:2].real)) | |
| # Tail should be sorted real numbers: | |
| assert_array_almost_equal(c[20000:], sort(c[20000:])) | |
| # Octave comparison: | |
| """ | |
| x = rand(10000,1) + i*rand(10000,1); | |
| y = conj(x); | |
| z = rand(10000,1); | |
| x = [x;y;z]; | |
| ix = randperm(length(x)); | |
| xsh = x(ix); | |
| length(xsh) | |
| cplxpair (xsh); | |
| """ | |
| assert_raises(ValueError, cplxpair, [1+3j, 1-3j, 1+2j]) | |
| # SOS tests | |
| B = [1, 0, 0, 0, 0, 1] | |
| A = [1, 0, 0, 0, 0, .9] | |
| z, p, k = tf2zpk(B, A) | |
| sos, k = zpk2sos(z, p, k) | |
| Bh, Ah = sos2tf(sos, k) | |
| assert_array_almost_equal([Bh, Ah], [B, A]) | |
| # works | |
| sos, k = tf2sos(B, A) | |
| Bh, Ah = sos2tf(sos, k) | |
| assert_array_almost_equal([Bh, Ah], [B, A]) | |
| # works | |
| sos2 = [[1.00000, 0.61803, 1.0000, 1.00000, 0.60515, 0.95873], | |
| [1.00000, -1.61803, 1.0000, 1.00000, -1.58430, 0.95873], | |
| [1.00000, 1.00000, 0.0000, 1.00000, 0.97915, 0.00000]] | |
| k2 = 1 | |
| z, p, k = tf2zpk(B, A) | |
| sos, k = zpk2sos(z, p, k) | |
| assert_array_almost_equal(sos, sos2, decimal=5) | |
| assert_array_almost_equal(k, k2) | |
| sos, k = tf2sos(B, A) | |
| assert_array_almost_equal(sos, sos2, decimal=5) | |
| assert_array_almost_equal(k, k2) | |
| B = array([1, 1]) | |
| A = array([1, 0.5]) | |
| sos, k = tf2sos(B, A) | |
| Bh, Ah = sos2tf(sos, k) | |
| assert_array_almost_equal([Bh, Ah], [B, A]) | |
| # works | |
| zc, zr = cplxreal(roots(array([1, 0, 0, 1]))) | |
| assert_array_almost_equal(append(zc, zr), [0.5 + 1j * sin(pi / 3), -1]) | |
| # works | |
| a1 = cplxpair(exp(2j*pi*arange(5 )/5)) | |
| a2 = exp(2j*pi*array([3, 2, 4, 1, 0])/5) | |
| ans = [ | |
| -0.80902 - 0.58779j, | |
| -0.80902 + 0.58779j, | |
| 0.30902 - 0.95106j, | |
| 0.30902 + 0.95106j, | |
| 1.00000 + 0.00000j, | |
| ] | |
| assert_array_almost_equal(a1, ans, decimal=5) | |
| assert_array_almost_equal(a2, ans, decimal=5) | |
| # Matlab example | |
| sos = [[1, 1, 1, 1, 0, -1], [-2, 3, 1, 1, 10, 1]] | |
| b, a = sos2tf(sos) | |
| b2 = [-2, 1, 2, 4, 1] | |
| a2 = [1, 10, 0, -10, -1] | |
| assert_array_almost_equal(a, a2) | |
| assert_array_almost_equal(b, b2) | |
| # works | |
| # ordering wrong: | |
| z, p, k = sos2zpk(sos) | |
| z2 = [-0.5000 + 0.8660j, -0.5000 - 0.8660j, 1.7808, -0.2808] | |
| p2 = [-1.0000, 1.0000, -9.8990, -0.1010] | |
| k2 = -2 | |
| assert_array_almost_equal(z, z2, decimal=4) | |
| assert_array_almost_equal(sort(p), sort(p2), decimal=4) | |
| assert_array_almost_equal(k, k2) | |
| # order of poles isn't important | |
| z, p, k = sos2zpk([[1, 0, 1, 1, 0, -0.81], [1, 0, 0, 1, 0, 0.49]]) | |
| z2 = [1j, -1j, 0, 0] | |
| p2 = [0.9, -0.9, 0.7j, -0.7j] | |
| k2 = 1 | |
| assert_array_almost_equal(z, z2, decimal=4) | |
| assert_array_almost_equal(p, p2, decimal=4) | |
| assert_array_almost_equal(k, k2) | |
| b1t = array([1, 2, 3]) | |
| a1t = array([1, .2, .3]) | |
| b2t = array([4, 5, 6]) | |
| a2t = array([1, .4, .5]) | |
| sos = array([append(b1t, a1t), append(b2t, a2t)]) | |
| z = array([-1 - 1.41421356237310j, -1 + 1.41421356237310j, | |
| -0.625 - 1.05326872164704j, -0.625 + 1.05326872164704j]) | |
| p = array([-0.2 - 0.678232998312527j, -0.2 + 0.678232998312527j, | |
| -0.1 - 0.538516480713450j, -0.1 + 0.538516480713450j]) | |
| k = 4 | |
| z2, p2, k2 = sos2zpk(sos, 1) | |
| assert_array_almost_equal(cplxpair(z2), z) | |
| assert_array_almost_equal(cplxpair(p2), p) | |
| assert_array_almost_equal(k2, k) | |
| # doesn't work | |
| z = [-1, -1] | |
| p = [0.57149 + 0.29360j, 0.57149 - 0.29360j] | |
| sos, k = zpk2sos(z, p) | |
| sos2 = [[1.00000, 2.00000, 1.00000, 1.00000, -1.14298, 0.41280]] | |
| k2 = 1 | |
| assert_array_almost_equal(sos, sos2, decimal=5) | |
| assert_array_almost_equal(k2, k) | |
| N = [1, -.5, -.315, -.0185] | |
| D = [1, -.5, .5, -.25] | |
| sos, k = tf2sos(N, D) | |
| sos2 = [[1.0000, 0.3813, 0.0210, 1.0000, 0.0000, 0.5000], | |
| [1.0000, -0.8813, 0.0000, 1.0000, -0.5000, 0.0000], | |
| ] | |
| k2 = 1 | |
| assert_array_almost_equal(sos, sos2, decimal=4) | |
| assert_array_almost_equal(k2, k) | |
| # sos order doesn't matter either? so reordered sections to match | |
| b = [0.5, 0, -0.18] | |
| a = [1, 0.1, -0.72] | |
| z, p, c = tf2zpk(b, a) | |
| z2 = [0.6, -0.6] | |
| p2 = [-0.9, 0.8] | |
| c2 = 0.5 | |
| assert_array_almost_equal(z, z2, decimal=6) | |
| assert_array_almost_equal(p, p2, decimal=6) | |
| assert_array_almost_equal(c, c2) | |
| # num = [2, 16, 44, 56, 32] | |
| # den = [3, 3, -15, 18, -12] | |
| # sos = tf2sos(num, den) | |
| # sos2 = [[0.6667, 4.0000, 5.3333, 1.0000, 2.0000, -4.0000], | |
| # [1.0000, 2.0000, 2.0000, 1.0000, -1.0000, 1.0000]] | |
| # # Matlab doesn't normalize by k if you don't ask for it? | |
| # # different order? | |
| # assert_array_almost_equal(sos, sos2) | |
| # Matches Octave behavior though | |
| B = [1, 0, 0, 0, 0, 1] | |
| A = [1, 0, 0, 0, 0, 0.9] | |
| sos, g = tf2sos(B,A) | |
| sos2 = [ | |
| [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], | |
| ] | |
| g2 = 1 | |
| assert_array_almost_equal(sos, sos2, decimal=5) | |
| assert_array_almost_equal(g, g2) | |
| b = [1, -3, 11, -27, 18] | |
| a = [16, 12, 2, -4, -1] | |
| sos, G = tf2sos(b, a) | |
| G2 = 0.0625 | |
| sos2 = [[1.0000, 0.0000, 9.0000, 1.0000, 1.0000, 0.5000], | |
| [1.0000, -3.0000, 2.0000, 1.0000, -.25000, -.12500]] | |
| # matlab different order? | |
| assert_array_almost_equal(sos, sos2) | |
| assert_array_almost_equal(G, G2) | |