1"""
2ltisys -- a collection of functions to convert linear time invariant systems
3from one representation to another.
4"""
5import numpy
6import numpy as np
7from numpy import (r_, eye, atleast_2d, poly, dot,
8                   asarray, prod, zeros, array, outer)
9from scipy import linalg
10
11from .filter_design import tf2zpk, zpk2tf, normalize
12
13
14__all__ = ['tf2ss', 'abcd_normalize', 'ss2tf', 'zpk2ss', 'ss2zpk',
15           'cont2discrete']
16
17
18def tf2ss(num, den):
19    r"""Transfer function to state-space representation.
20
21    Parameters
22    ----------
23    num, den : array_like
24        Sequences representing the coefficients of the numerator and
25        denominator polynomials, in order of descending degree. The
26        denominator needs to be at least as long as the numerator.
27
28    Returns
29    -------
30    A, B, C, D : ndarray
31        State space representation of the system, in controller canonical
32        form.
33
34    Examples
35    --------
36    Convert the transfer function:
37
38    .. math:: H(s) = \frac{s^2 + 3s + 3}{s^2 + 2s + 1}
39
40    >>> num = [1, 3, 3]
41    >>> den = [1, 2, 1]
42
43    to the state-space representation:
44
45    .. math::
46
47        \dot{\textbf{x}}(t) =
48        \begin{bmatrix} -2 & -1 \\ 1 & 0 \end{bmatrix} \textbf{x}(t) +
49        \begin{bmatrix} 1 \\ 0 \end{bmatrix} \textbf{u}(t) \\
50
51        \textbf{y}(t) = \begin{bmatrix} 1 & 2 \end{bmatrix} \textbf{x}(t) +
52        \begin{bmatrix} 1 \end{bmatrix} \textbf{u}(t)
53
54    >>> from scipy.signal import tf2ss
55    >>> A, B, C, D = tf2ss(num, den)
56    >>> A
57    array([[-2., -1.],
58           [ 1.,  0.]])
59    >>> B
60    array([[ 1.],
61           [ 0.]])
62    >>> C
63    array([[ 1.,  2.]])
64    >>> D
65    array([[ 1.]])
66    """
67    # Controller canonical state-space representation.
68    #  if M+1 = len(num) and K+1 = len(den) then we must have M <= K
69    #  states are found by asserting that X(s) = U(s) / D(s)
70    #  then Y(s) = N(s) * X(s)
71    #
72    #   A, B, C, and D follow quite naturally.
73    #
74    num, den = normalize(num, den)   # Strips zeros, checks arrays
75    nn = len(num.shape)
76    if nn == 1:
77        num = asarray([num], num.dtype)
78    M = num.shape[1]
79    K = len(den)
80    if M > K:
81        msg = "Improper transfer function. `num` is longer than `den`."
82        raise ValueError(msg)
83    if M == 0 or K == 0:  # Null system
84        return (array([], float), array([], float), array([], float),
85                array([], float))
86
87    # pad numerator to have same number of columns has denominator
88    num = r_['-1', zeros((num.shape[0], K - M), num.dtype), num]
89
90    if num.shape[-1] > 0:
91        D = atleast_2d(num[:, 0])
92
93    else:
94        # We don't assign it an empty array because this system
95        # is not 'null'. It just doesn't have a non-zero D
96        # matrix. Thus, it should have a non-zero shape so that
97        # it can be operated on by functions like 'ss2tf'
98        D = array([[0]], float)
99
100    if K == 1:
101        D = D.reshape(num.shape)
102
103        return (zeros((1, 1)), zeros((1, D.shape[1])),
104                zeros((D.shape[0], 1)), D)
105
106    frow = -array([den[1:]])
107    A = r_[frow, eye(K - 2, K - 1)]
108    B = eye(K - 1, 1)
109    C = num[:, 1:] - outer(num[:, 0], den[1:])
110    D = D.reshape((C.shape[0], B.shape[1]))
111
112    return A, B, C, D
113
114
115def _none_to_empty_2d(arg):
116    if arg is None:
117        return zeros((0, 0))
118    else:
119        return arg
120
121
122def _atleast_2d_or_none(arg):
123    if arg is not None:
124        return atleast_2d(arg)
125
126
127def _shape_or_none(M):
128    if M is not None:
129        return M.shape
130    else:
131        return (None,) * 2
132
133
134def _choice_not_none(*args):
135    for arg in args:
136        if arg is not None:
137            return arg
138
139
140def _restore(M, shape):
141    if M.shape == (0, 0):
142        return zeros(shape)
143    else:
144        if M.shape != shape:
145            raise ValueError("The input arrays have incompatible shapes.")
146        return M
147
148
149def abcd_normalize(A=None, B=None, C=None, D=None):
150    """Check state-space matrices and ensure they are 2-D.
151
152    If enough information on the system is provided, that is, enough
153    properly-shaped arrays are passed to the function, the missing ones
154    are built from this information, ensuring the correct number of
155    rows and columns. Otherwise a ValueError is raised.
156
157    Parameters
158    ----------
159    A, B, C, D : array_like, optional
160        State-space matrices. All of them are None (missing) by default.
161        See `ss2tf` for format.
162
163    Returns
164    -------
165    A, B, C, D : array
166        Properly shaped state-space matrices.
167
168    Raises
169    ------
170    ValueError
171        If not enough information on the system was provided.
172
173    """
174    A, B, C, D = map(_atleast_2d_or_none, (A, B, C, D))
175
176    MA, NA = _shape_or_none(A)
177    MB, NB = _shape_or_none(B)
178    MC, NC = _shape_or_none(C)
179    MD, ND = _shape_or_none(D)
180
181    p = _choice_not_none(MA, MB, NC)
182    q = _choice_not_none(NB, ND)
183    r = _choice_not_none(MC, MD)
184    if p is None or q is None or r is None:
185        raise ValueError("Not enough information on the system.")
186
187    A, B, C, D = map(_none_to_empty_2d, (A, B, C, D))
188    A = _restore(A, (p, p))
189    B = _restore(B, (p, q))
190    C = _restore(C, (r, p))
191    D = _restore(D, (r, q))
192
193    return A, B, C, D
194
195
196def ss2tf(A, B, C, D, input=0):
197    r"""State-space to transfer function.
198
199    A, B, C, D defines a linear state-space system with `p` inputs,
200    `q` outputs, and `n` state variables.
201
202    Parameters
203    ----------
204    A : array_like
205        State (or system) matrix of shape ``(n, n)``
206    B : array_like
207        Input matrix of shape ``(n, p)``
208    C : array_like
209        Output matrix of shape ``(q, n)``
210    D : array_like
211        Feedthrough (or feedforward) matrix of shape ``(q, p)``
212    input : int, optional
213        For multiple-input systems, the index of the input to use.
214
215    Returns
216    -------
217    num : 2-D ndarray
218        Numerator(s) of the resulting transfer function(s). `num` has one row
219        for each of the system's outputs. Each row is a sequence representation
220        of the numerator polynomial.
221    den : 1-D ndarray
222        Denominator of the resulting transfer function(s). `den` is a sequence
223        representation of the denominator polynomial.
224
225    Examples
226    --------
227    Convert the state-space representation:
228
229    .. math::
230
231        \dot{\textbf{x}}(t) =
232        \begin{bmatrix} -2 & -1 \\ 1 & 0 \end{bmatrix} \textbf{x}(t) +
233        \begin{bmatrix} 1 \\ 0 \end{bmatrix} \textbf{u}(t) \\
234
235        \textbf{y}(t) = \begin{bmatrix} 1 & 2 \end{bmatrix} \textbf{x}(t) +
236        \begin{bmatrix} 1 \end{bmatrix} \textbf{u}(t)
237
238    >>> A = [[-2, -1], [1, 0]]
239    >>> B = [[1], [0]]  # 2-D column vector
240    >>> C = [[1, 2]]    # 2-D row vector
241    >>> D = 1
242
243    to the transfer function:
244
245    .. math:: H(s) = \frac{s^2 + 3s + 3}{s^2 + 2s + 1}
246
247    >>> from scipy.signal import ss2tf
248    >>> ss2tf(A, B, C, D)
249    (array([[1., 3., 3.]]), array([ 1.,  2.,  1.]))
250    """
251    # transfer function is C (sI - A)**(-1) B + D
252
253    # Check consistency and make them all rank-2 arrays
254    A, B, C, D = abcd_normalize(A, B, C, D)
255
256    nout, nin = D.shape
257    if input >= nin:
258        raise ValueError("System does not have the input specified.")
259
260    # make SIMO from possibly MIMO system.
261    B = B[:, input:input + 1]
262    D = D[:, input:input + 1]
263
264    try:
265        den = poly(A)
266    except ValueError:
267        den = 1
268
269    if (prod(B.shape, axis=0) == 0) and (prod(C.shape, axis=0) == 0):
270        num = numpy.ravel(D)
271        if (prod(D.shape, axis=0) == 0) and (prod(A.shape, axis=0) == 0):
272            den = []
273        return num, den
274
275    num_states = A.shape[0]
276    type_test = A[:, 0] + B[:, 0] + C[0, :] + D + 0.0
277    num = numpy.empty((nout, num_states + 1), type_test.dtype)
278    for k in range(nout):
279        Ck = atleast_2d(C[k, :])
280        num[k] = poly(A - dot(B, Ck)) + (D[k] - 1) * den
281
282    return num, den
283
284
285def zpk2ss(z, p, k):
286    """Zero-pole-gain representation to state-space representation
287
288    Parameters
289    ----------
290    z, p : sequence
291        Zeros and poles.
292    k : float
293        System gain.
294
295    Returns
296    -------
297    A, B, C, D : ndarray
298        State space representation of the system, in controller canonical
299        form.
300
301    """
302    return tf2ss(*zpk2tf(z, p, k))
303
304
305def ss2zpk(A, B, C, D, input=0):
306    """State-space representation to zero-pole-gain representation.
307
308    A, B, C, D defines a linear state-space system with `p` inputs,
309    `q` outputs, and `n` state variables.
310
311    Parameters
312    ----------
313    A : array_like
314        State (or system) matrix of shape ``(n, n)``
315    B : array_like
316        Input matrix of shape ``(n, p)``
317    C : array_like
318        Output matrix of shape ``(q, n)``
319    D : array_like
320        Feedthrough (or feedforward) matrix of shape ``(q, p)``
321    input : int, optional
322        For multiple-input systems, the index of the input to use.
323
324    Returns
325    -------
326    z, p : sequence
327        Zeros and poles.
328    k : float
329        System gain.
330
331    """
332    return tf2zpk(*ss2tf(A, B, C, D, input=input))
333
334
335def cont2discrete(system, dt, method="zoh", alpha=None):
336    """
337    Transform a continuous to a discrete state-space system.
338
339    Parameters
340    ----------
341    system : a tuple describing the system or an instance of `lti`
342        The following gives the number of elements in the tuple and
343        the interpretation:
344
345            * 1: (instance of `lti`)
346            * 2: (num, den)
347            * 3: (zeros, poles, gain)
348            * 4: (A, B, C, D)
349
350    dt : float
351        The discretization time step.
352    method : str, optional
353        Which method to use:
354
355            * gbt: generalized bilinear transformation
356            * bilinear: Tustin's approximation ("gbt" with alpha=0.5)
357            * euler: Euler (or forward differencing) method ("gbt" with alpha=0)
358            * backward_diff: Backwards differencing ("gbt" with alpha=1.0)
359            * zoh: zero-order hold (default)
360            * foh: first-order hold (*versionadded: 1.3.0*)
361            * impulse: equivalent impulse response (*versionadded: 1.3.0*)
362
363    alpha : float within [0, 1], optional
364        The generalized bilinear transformation weighting parameter, which
365        should only be specified with method="gbt", and is ignored otherwise
366
367    Returns
368    -------
369    sysd : tuple containing the discrete system
370        Based on the input type, the output will be of the form
371
372        * (num, den, dt)   for transfer function input
373        * (zeros, poles, gain, dt)   for zeros-poles-gain input
374        * (A, B, C, D, dt) for state-space system input
375
376    Notes
377    -----
378    By default, the routine uses a Zero-Order Hold (zoh) method to perform
379    the transformation. Alternatively, a generalized bilinear transformation
380    may be used, which includes the common Tustin's bilinear approximation,
381    an Euler's method technique, or a backwards differencing technique.
382
383    The Zero-Order Hold (zoh) method is based on [1]_, the generalized bilinear
384    approximation is based on [2]_ and [3]_, the First-Order Hold (foh) method
385    is based on [4]_.
386
387    Examples
388    --------
389    We can transform a continuous state-space system to a discrete one:
390
391    >>> import matplotlib.pyplot as plt
392    >>> from scipy.signal import cont2discrete, lti, dlti, dstep
393
394    Define a continuous state-space system.
395
396    >>> A = np.array([[0, 1],[-10., -3]])
397    >>> B = np.array([[0],[10.]])
398    >>> C = np.array([[1., 0]])
399    >>> D = np.array([[0.]])
400    >>> l_system = lti(A, B, C, D)
401    >>> t, x = l_system.step(T=np.linspace(0, 5, 100))
402    >>> fig, ax = plt.subplots()
403    >>> ax.plot(t, x, label='Continuous', linewidth=3)
404
405    Transform it to a discrete state-space system using several methods.
406
407    >>> dt = 0.1
408    >>> for method in ['zoh', 'bilinear', 'euler', 'backward_diff', 'foh', 'impulse']:
409    ...    d_system = cont2discrete((A, B, C, D), dt, method=method)
410    ...    s, x_d = dstep(d_system)
411    ...    ax.step(s, np.squeeze(x_d), label=method, where='post')
412    >>> ax.axis([t[0], t[-1], x[0], 1.4])
413    >>> ax.legend(loc='best')
414    >>> fig.tight_layout()
415    >>> plt.show()
416
417    References
418    ----------
419    .. [1] https://en.wikipedia.org/wiki/Discretization#Discretization_of_linear_state_space_models
420
421    .. [2] http://techteach.no/publications/discretetime_signals_systems/discrete.pdf
422
423    .. [3] G. Zhang, X. Chen, and T. Chen, Digital redesign via the generalized
424        bilinear transformation, Int. J. Control, vol. 82, no. 4, pp. 741-754,
425        2009.
426        (https://www.mypolyuweb.hk/~magzhang/Research/ZCC09_IJC.pdf)
427
428    .. [4] G. F. Franklin, J. D. Powell, and M. L. Workman, Digital control
429        of dynamic systems, 3rd ed. Menlo Park, Calif: Addison-Wesley,
430        pp. 204-206, 1998.
431
432    """
433    if len(system) == 1:
434        return system.to_discrete()
435    if len(system) == 2:
436        sysd = cont2discrete(tf2ss(system[0], system[1]), dt, method=method,
437                             alpha=alpha)
438        return ss2tf(sysd[0], sysd[1], sysd[2], sysd[3]) + (dt,)
439    elif len(system) == 3:
440        sysd = cont2discrete(zpk2ss(system[0], system[1], system[2]), dt,
441                             method=method, alpha=alpha)
442        return ss2zpk(sysd[0], sysd[1], sysd[2], sysd[3]) + (dt,)
443    elif len(system) == 4:
444        a, b, c, d = system
445    else:
446        raise ValueError("First argument must either be a tuple of 2 (tf), "
447                         "3 (zpk), or 4 (ss) arrays.")
448
449    if method == 'gbt':
450        if alpha is None:
451            raise ValueError("Alpha parameter must be specified for the "
452                             "generalized bilinear transform (gbt) method")
453        elif alpha < 0 or alpha > 1:
454            raise ValueError("Alpha parameter must be within the interval "
455                             "[0,1] for the gbt method")
456
457    if method == 'gbt':
458        # This parameter is used repeatedly - compute once here
459        ima = np.eye(a.shape[0]) - alpha*dt*a
460        ad = linalg.solve(ima, np.eye(a.shape[0]) + (1.0-alpha)*dt*a)
461        bd = linalg.solve(ima, dt*b)
462
463        # Similarly solve for the output equation matrices
464        cd = linalg.solve(ima.transpose(), c.transpose())
465        cd = cd.transpose()
466        dd = d + alpha*np.dot(c, bd)
467
468    elif method == 'bilinear' or method == 'tustin':
469        return cont2discrete(system, dt, method="gbt", alpha=0.5)
470
471    elif method == 'euler' or method == 'forward_diff':
472        return cont2discrete(system, dt, method="gbt", alpha=0.0)
473
474    elif method == 'backward_diff':
475        return cont2discrete(system, dt, method="gbt", alpha=1.0)
476
477    elif method == 'zoh':
478        # Build an exponential matrix
479        em_upper = np.hstack((a, b))
480
481        # Need to stack zeros under the a and b matrices
482        em_lower = np.hstack((np.zeros((b.shape[1], a.shape[0])),
483                              np.zeros((b.shape[1], b.shape[1]))))
484
485        em = np.vstack((em_upper, em_lower))
486        ms = linalg.expm(dt * em)
487
488        # Dispose of the lower rows
489        ms = ms[:a.shape[0], :]
490
491        ad = ms[:, 0:a.shape[1]]
492        bd = ms[:, a.shape[1]:]
493
494        cd = c
495        dd = d
496
497    elif method == 'foh':
498        # Size parameters for convenience
499        n = a.shape[0]
500        m = b.shape[1]
501
502        # Build an exponential matrix similar to 'zoh' method
503        em_upper = linalg.block_diag(np.block([a, b]) * dt, np.eye(m))
504        em_lower = zeros((m, n + 2 * m))
505        em = np.block([[em_upper], [em_lower]])
506
507        ms = linalg.expm(em)
508
509        # Get the three blocks from upper rows
510        ms11 = ms[:n, 0:n]
511        ms12 = ms[:n, n:n + m]
512        ms13 = ms[:n, n + m:]
513
514        ad = ms11
515        bd = ms12 - ms13 + ms11 @ ms13
516        cd = c
517        dd = d + c @ ms13
518
519    elif method == 'impulse':
520        if not np.allclose(d, 0):
521            raise ValueError("Impulse method is only applicable"
522                             "to strictly proper systems")
523
524        ad = linalg.expm(a * dt)
525        bd = ad @ b * dt
526        cd = c
527        dd = c @ b * dt
528
529    else:
530        raise ValueError("Unknown transformation method '%s'" % method)
531
532    return ad, bd, cd, dd, dt
533