503 lines
15 KiB
Python
503 lines
15 KiB
Python
|
"""
|
||
|
|
ltisys -- a collection of functions to convert linear time invariant systems
|
||
|
|
from one representation to another.
|
||
|
|
"""
|
||
|
|
import numpy
|
||
|
|
import numpy as np
|
||
|
|
from numpy import (r_, eye, atleast_2d, poly, dot,
|
||
|
|
asarray, prod, zeros, array, outer)
|
||
|
|
from scipy import linalg
|
||
|
|
|
||
|
|
from .filter_design import tf2zpk, zpk2tf, normalize
|
||
|
|
|
||
|
|
|
||
|
|
__all__ = ['tf2ss', 'abcd_normalize', 'ss2tf', 'zpk2ss', 'ss2zpk',
|
||
|
|
'cont2discrete']
|
||
|
|
|
||
|
|
|
||
|
|
def tf2ss(num, den):
|
||
|
|
r"""Transfer function to state-space representation.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
num, den : array_like
|
||
|
|
Sequences representing the coefficients of the numerator and
|
||
|
|
denominator polynomials, in order of descending degree. The
|
||
|
|
denominator needs to be at least as long as the numerator.
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
A, B, C, D : ndarray
|
||
|
|
State space representation of the system, in controller canonical
|
||
|
|
form.
|
||
|
|
|
||
|
|
Examples
|
||
|
|
--------
|
||
|
|
Convert the transfer function:
|
||
|
|
|
||
|
|
.. math:: H(s) = \frac{s^2 + 3s + 3}{s^2 + 2s + 1}
|
||
|
|
|
||
|
|
>>> num = [1, 3, 3]
|
||
|
|
>>> den = [1, 2, 1]
|
||
|
|
|
||
|
|
to the state-space representation:
|
||
|
|
|
||
|
|
.. math::
|
||
|
|
|
||
|
|
\dot{\textbf{x}}(t) =
|
||
|
|
\begin{bmatrix} -2 & -1 \\ 1 & 0 \end{bmatrix} \textbf{x}(t) +
|
||
|
|
\begin{bmatrix} 1 \\ 0 \end{bmatrix} \textbf{u}(t) \\
|
||
|
|
|
||
|
|
\textbf{y}(t) = \begin{bmatrix} 1 & 2 \end{bmatrix} \textbf{x}(t) +
|
||
|
|
\begin{bmatrix} 1 \end{bmatrix} \textbf{u}(t)
|
||
|
|
|
||
|
|
>>> from scipy.signal import tf2ss
|
||
|
|
>>> A, B, C, D = tf2ss(num, den)
|
||
|
|
>>> A
|
||
|
|
array([[-2., -1.],
|
||
|
|
[ 1., 0.]])
|
||
|
|
>>> B
|
||
|
|
array([[ 1.],
|
||
|
|
[ 0.]])
|
||
|
|
>>> C
|
||
|
|
array([[ 1., 2.]])
|
||
|
|
>>> D
|
||
|
|
array([[ 1.]])
|
||
|
|
"""
|
||
|
|
# Controller canonical state-space representation.
|
||
|
|
# if M+1 = len(num) and K+1 = len(den) then we must have M <= K
|
||
|
|
# states are found by asserting that X(s) = U(s) / D(s)
|
||
|
|
# then Y(s) = N(s) * X(s)
|
||
|
|
#
|
||
|
|
# A, B, C, and D follow quite naturally.
|
||
|
|
#
|
||
|
|
num, den = normalize(num, den) # Strips zeros, checks arrays
|
||
|
|
nn = len(num.shape)
|
||
|
|
if nn == 1:
|
||
|
|
num = asarray([num], num.dtype)
|
||
|
|
M = num.shape[1]
|
||
|
|
K = len(den)
|
||
|
|
if M > K:
|
||
|
|
msg = "Improper transfer function. `num` is longer than `den`."
|
||
|
|
raise ValueError(msg)
|
||
|
|
if M == 0 or K == 0: # Null system
|
||
|
|
return (array([], float), array([], float), array([], float),
|
||
|
|
array([], float))
|
||
|
|
|
||
|
|
# pad numerator to have same number of columns has denominator
|
||
|
|
num = r_['-1', zeros((num.shape[0], K - M), num.dtype), num]
|
||
|
|
|
||
|
|
if num.shape[-1] > 0:
|
||
|
|
D = atleast_2d(num[:, 0])
|
||
|
|
|
||
|
|
else:
|
||
|
|
# We don't assign it an empty array because this system
|
||
|
|
# is not 'null'. It just doesn't have a non-zero D
|
||
|
|
# matrix. Thus, it should have a non-zero shape so that
|
||
|
|
# it can be operated on by functions like 'ss2tf'
|
||
|
|
D = array([[0]], float)
|
||
|
|
|
||
|
|
if K == 1:
|
||
|
|
D = D.reshape(num.shape)
|
||
|
|
|
||
|
|
return (zeros((1, 1)), zeros((1, D.shape[1])),
|
||
|
|
zeros((D.shape[0], 1)), D)
|
||
|
|
|
||
|
|
frow = -array([den[1:]])
|
||
|
|
A = r_[frow, eye(K - 2, K - 1)]
|
||
|
|
B = eye(K - 1, 1)
|
||
|
|
C = num[:, 1:] - outer(num[:, 0], den[1:])
|
||
|
|
D = D.reshape((C.shape[0], B.shape[1]))
|
||
|
|
|
||
|
|
return A, B, C, D
|
||
|
|
|
||
|
|
|
||
|
|
def _none_to_empty_2d(arg):
|
||
|
|
if arg is None:
|
||
|
|
return zeros((0, 0))
|
||
|
|
else:
|
||
|
|
return arg
|
||
|
|
|
||
|
|
|
||
|
|
def _atleast_2d_or_none(arg):
|
||
|
|
if arg is not None:
|
||
|
|
return atleast_2d(arg)
|
||
|
|
|
||
|
|
|
||
|
|
def _shape_or_none(M):
|
||
|
|
if M is not None:
|
||
|
|
return M.shape
|
||
|
|
else:
|
||
|
|
return (None,) * 2
|
||
|
|
|
||
|
|
|
||
|
|
def _choice_not_none(*args):
|
||
|
|
for arg in args:
|
||
|
|
if arg is not None:
|
||
|
|
return arg
|
||
|
|
|
||
|
|
|
||
|
|
def _restore(M, shape):
|
||
|
|
if M.shape == (0, 0):
|
||
|
|
return zeros(shape)
|
||
|
|
else:
|
||
|
|
if M.shape != shape:
|
||
|
|
raise ValueError("The input arrays have incompatible shapes.")
|
||
|
|
return M
|
||
|
|
|
||
|
|
|
||
|
|
def abcd_normalize(A=None, B=None, C=None, D=None):
|
||
|
|
"""Check state-space matrices and ensure they are 2-D.
|
||
|
|
|
||
|
|
If enough information on the system is provided, that is, enough
|
||
|
|
properly-shaped arrays are passed to the function, the missing ones
|
||
|
|
are built from this information, ensuring the correct number of
|
||
|
|
rows and columns. Otherwise a ValueError is raised.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
A, B, C, D : array_like, optional
|
||
|
|
State-space matrices. All of them are None (missing) by default.
|
||
|
|
See `ss2tf` for format.
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
A, B, C, D : array
|
||
|
|
Properly shaped state-space matrices.
|
||
|
|
|
||
|
|
Raises
|
||
|
|
------
|
||
|
|
ValueError
|
||
|
|
If not enough information on the system was provided.
|
||
|
|
|
||
|
|
"""
|
||
|
|
A, B, C, D = map(_atleast_2d_or_none, (A, B, C, D))
|
||
|
|
|
||
|
|
MA, NA = _shape_or_none(A)
|
||
|
|
MB, NB = _shape_or_none(B)
|
||
|
|
MC, NC = _shape_or_none(C)
|
||
|
|
MD, ND = _shape_or_none(D)
|
||
|
|
|
||
|
|
p = _choice_not_none(MA, MB, NC)
|
||
|
|
q = _choice_not_none(NB, ND)
|
||
|
|
r = _choice_not_none(MC, MD)
|
||
|
|
if p is None or q is None or r is None:
|
||
|
|
raise ValueError("Not enough information on the system.")
|
||
|
|
|
||
|
|
A, B, C, D = map(_none_to_empty_2d, (A, B, C, D))
|
||
|
|
A = _restore(A, (p, p))
|
||
|
|
B = _restore(B, (p, q))
|
||
|
|
C = _restore(C, (r, p))
|
||
|
|
D = _restore(D, (r, q))
|
||
|
|
|
||
|
|
return A, B, C, D
|
||
|
|
|
||
|
|
|
||
|
|
def ss2tf(A, B, C, D, input=0):
|
||
|
|
r"""State-space to transfer function.
|
||
|
|
|
||
|
|
A, B, C, D defines a linear state-space system with `p` inputs,
|
||
|
|
`q` outputs, and `n` state variables.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
A : array_like
|
||
|
|
State (or system) matrix of shape ``(n, n)``
|
||
|
|
B : array_like
|
||
|
|
Input matrix of shape ``(n, p)``
|
||
|
|
C : array_like
|
||
|
|
Output matrix of shape ``(q, n)``
|
||
|
|
D : array_like
|
||
|
|
Feedthrough (or feedforward) matrix of shape ``(q, p)``
|
||
|
|
input : int, optional
|
||
|
|
For multiple-input systems, the index of the input to use.
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
num : 2-D ndarray
|
||
|
|
Numerator(s) of the resulting transfer function(s). `num` has one row
|
||
|
|
for each of the system's outputs. Each row is a sequence representation
|
||
|
|
of the numerator polynomial.
|
||
|
|
den : 1-D ndarray
|
||
|
|
Denominator of the resulting transfer function(s). `den` is a sequence
|
||
|
|
representation of the denominator polynomial.
|
||
|
|
|
||
|
|
Examples
|
||
|
|
--------
|
||
|
|
Convert the state-space representation:
|
||
|
|
|
||
|
|
.. math::
|
||
|
|
|
||
|
|
\dot{\textbf{x}}(t) =
|
||
|
|
\begin{bmatrix} -2 & -1 \\ 1 & 0 \end{bmatrix} \textbf{x}(t) +
|
||
|
|
\begin{bmatrix} 1 \\ 0 \end{bmatrix} \textbf{u}(t) \\
|
||
|
|
|
||
|
|
\textbf{y}(t) = \begin{bmatrix} 1 & 2 \end{bmatrix} \textbf{x}(t) +
|
||
|
|
\begin{bmatrix} 1 \end{bmatrix} \textbf{u}(t)
|
||
|
|
|
||
|
|
>>> A = [[-2, -1], [1, 0]]
|
||
|
|
>>> B = [[1], [0]] # 2-D column vector
|
||
|
|
>>> C = [[1, 2]] # 2-D row vector
|
||
|
|
>>> D = 1
|
||
|
|
|
||
|
|
to the transfer function:
|
||
|
|
|
||
|
|
.. math:: H(s) = \frac{s^2 + 3s + 3}{s^2 + 2s + 1}
|
||
|
|
|
||
|
|
>>> from scipy.signal import ss2tf
|
||
|
|
>>> ss2tf(A, B, C, D)
|
||
|
|
(array([[1., 3., 3.]]), array([ 1., 2., 1.]))
|
||
|
|
"""
|
||
|
|
# transfer function is C (sI - A)**(-1) B + D
|
||
|
|
|
||
|
|
# Check consistency and make them all rank-2 arrays
|
||
|
|
A, B, C, D = abcd_normalize(A, B, C, D)
|
||
|
|
|
||
|
|
nout, nin = D.shape
|
||
|
|
if input >= nin:
|
||
|
|
raise ValueError("System does not have the input specified.")
|
||
|
|
|
||
|
|
# make SIMO from possibly MIMO system.
|
||
|
|
B = B[:, input:input + 1]
|
||
|
|
D = D[:, input:input + 1]
|
||
|
|
|
||
|
|
try:
|
||
|
|
den = poly(A)
|
||
|
|
except ValueError:
|
||
|
|
den = 1
|
||
|
|
|
||
|
|
if (prod(B.shape, axis=0) == 0) and (prod(C.shape, axis=0) == 0):
|
||
|
|
num = numpy.ravel(D)
|
||
|
|
if (prod(D.shape, axis=0) == 0) and (prod(A.shape, axis=0) == 0):
|
||
|
|
den = []
|
||
|
|
return num, den
|
||
|
|
|
||
|
|
num_states = A.shape[0]
|
||
|
|
type_test = A[:, 0] + B[:, 0] + C[0, :] + D + 0.0
|
||
|
|
num = numpy.empty((nout, num_states + 1), type_test.dtype)
|
||
|
|
for k in range(nout):
|
||
|
|
Ck = atleast_2d(C[k, :])
|
||
|
|
num[k] = poly(A - dot(B, Ck)) + (D[k] - 1) * den
|
||
|
|
|
||
|
|
return num, den
|
||
|
|
|
||
|
|
|
||
|
|
def zpk2ss(z, p, k):
|
||
|
|
"""Zero-pole-gain representation to state-space representation
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
z, p : sequence
|
||
|
|
Zeros and poles.
|
||
|
|
k : float
|
||
|
|
System gain.
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
A, B, C, D : ndarray
|
||
|
|
State space representation of the system, in controller canonical
|
||
|
|
form.
|
||
|
|
|
||
|
|
"""
|
||
|
|
return tf2ss(*zpk2tf(z, p, k))
|
||
|
|
|
||
|
|
|
||
|
|
def ss2zpk(A, B, C, D, input=0):
|
||
|
|
"""State-space representation to zero-pole-gain representation.
|
||
|
|
|
||
|
|
A, B, C, D defines a linear state-space system with `p` inputs,
|
||
|
|
`q` outputs, and `n` state variables.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
A : array_like
|
||
|
|
State (or system) matrix of shape ``(n, n)``
|
||
|
|
B : array_like
|
||
|
|
Input matrix of shape ``(n, p)``
|
||
|
|
C : array_like
|
||
|
|
Output matrix of shape ``(q, n)``
|
||
|
|
D : array_like
|
||
|
|
Feedthrough (or feedforward) matrix of shape ``(q, p)``
|
||
|
|
input : int, optional
|
||
|
|
For multiple-input systems, the index of the input to use.
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
z, p : sequence
|
||
|
|
Zeros and poles.
|
||
|
|
k : float
|
||
|
|
System gain.
|
||
|
|
|
||
|
|
"""
|
||
|
|
return tf2zpk(*ss2tf(A, B, C, D, input=input))
|
||
|
|
|
||
|
|
|
||
|
|
def cont2discrete(system, dt, method="zoh", alpha=None):
|
||
|
|
"""
|
||
|
|
Transform a continuous to a discrete state-space system.
|
||
|
|
|
||
|
|
Parameters
|
||
|
|
----------
|
||
|
|
system : a tuple describing the system or an instance of `lti`
|
||
|
|
The following gives the number of elements in the tuple and
|
||
|
|
the interpretation:
|
||
|
|
|
||
|
|
* 1: (instance of `lti`)
|
||
|
|
* 2: (num, den)
|
||
|
|
* 3: (zeros, poles, gain)
|
||
|
|
* 4: (A, B, C, D)
|
||
|
|
|
||
|
|
dt : float
|
||
|
|
The discretization time step.
|
||
|
|
method : str, optional
|
||
|
|
Which method to use:
|
||
|
|
|
||
|
|
* gbt: generalized bilinear transformation
|
||
|
|
* bilinear: Tustin's approximation ("gbt" with alpha=0.5)
|
||
|
|
* euler: Euler (or forward differencing) method ("gbt" with alpha=0)
|
||
|
|
* backward_diff: Backwards differencing ("gbt" with alpha=1.0)
|
||
|
|
* zoh: zero-order hold (default)
|
||
|
|
* foh: first-order hold (*versionadded: 1.3.0*)
|
||
|
|
* impulse: equivalent impulse response (*versionadded: 1.3.0*)
|
||
|
|
|
||
|
|
alpha : float within [0, 1], optional
|
||
|
|
The generalized bilinear transformation weighting parameter, which
|
||
|
|
should only be specified with method="gbt", and is ignored otherwise
|
||
|
|
|
||
|
|
Returns
|
||
|
|
-------
|
||
|
|
sysd : tuple containing the discrete system
|
||
|
|
Based on the input type, the output will be of the form
|
||
|
|
|
||
|
|
* (num, den, dt) for transfer function input
|
||
|
|
* (zeros, poles, gain, dt) for zeros-poles-gain input
|
||
|
|
* (A, B, C, D, dt) for state-space system input
|
||
|
|
|
||
|
|
Notes
|
||
|
|
-----
|
||
|
|
By default, the routine uses a Zero-Order Hold (zoh) method to perform
|
||
|
|
the transformation. Alternatively, a generalized bilinear transformation
|
||
|
|
may be used, which includes the common Tustin's bilinear approximation,
|
||
|
|
an Euler's method technique, or a backwards differencing technique.
|
||
|
|
|
||
|
|
The Zero-Order Hold (zoh) method is based on [1]_, the generalized bilinear
|
||
|
|
approximation is based on [2]_ and [3]_, the First-Order Hold (foh) method
|
||
|
|
is based on [4]_.
|
||
|
|
|
||
|
|
References
|
||
|
|
----------
|
||
|
|
.. [1] https://en.wikipedia.org/wiki/Discretization#Discretization_of_linear_state_space_models
|
||
|
|
|
||
|
|
.. [2] http://techteach.no/publications/discretetime_signals_systems/discrete.pdf
|
||
|
|
|
||
|
|
.. [3] G. Zhang, X. Chen, and T. Chen, Digital redesign via the generalized
|
||
|
|
bilinear transformation, Int. J. Control, vol. 82, no. 4, pp. 741-754,
|
||
|
|
2009.
|
||
|
|
(https://www.mypolyuweb.hk/~magzhang/Research/ZCC09_IJC.pdf)
|
||
|
|
|
||
|
|
.. [4] G. F. Franklin, J. D. Powell, and M. L. Workman, Digital control
|
||
|
|
of dynamic systems, 3rd ed. Menlo Park, Calif: Addison-Wesley,
|
||
|
|
pp. 204-206, 1998.
|
||
|
|
|
||
|
|
"""
|
||
|
|
if len(system) == 1:
|
||
|
|
return system.to_discrete()
|
||
|
|
if len(system) == 2:
|
||
|
|
sysd = cont2discrete(tf2ss(system[0], system[1]), dt, method=method,
|
||
|
|
alpha=alpha)
|
||
|
|
return ss2tf(sysd[0], sysd[1], sysd[2], sysd[3]) + (dt,)
|
||
|
|
elif len(system) == 3:
|
||
|
|
sysd = cont2discrete(zpk2ss(system[0], system[1], system[2]), dt,
|
||
|
|
method=method, alpha=alpha)
|
||
|
|
return ss2zpk(sysd[0], sysd[1], sysd[2], sysd[3]) + (dt,)
|
||
|
|
elif len(system) == 4:
|
||
|
|
a, b, c, d = system
|
||
|
|
else:
|
||
|
|
raise ValueError("First argument must either be a tuple of 2 (tf), "
|
||
|
|
"3 (zpk), or 4 (ss) arrays.")
|
||
|
|
|
||
|
|
if method == 'gbt':
|
||
|
|
if alpha is None:
|
||
|
|
raise ValueError("Alpha parameter must be specified for the "
|
||
|
|
"generalized bilinear transform (gbt) method")
|
||
|
|
elif alpha < 0 or alpha > 1:
|
||
|
|
raise ValueError("Alpha parameter must be within the interval "
|
||
|
|
"[0,1] for the gbt method")
|
||
|
|
|
||
|
|
if method == 'gbt':
|
||
|
|
# This parameter is used repeatedly - compute once here
|
||
|
|
ima = np.eye(a.shape[0]) - alpha*dt*a
|
||
|
|
ad = linalg.solve(ima, np.eye(a.shape[0]) + (1.0-alpha)*dt*a)
|
||
|
|
bd = linalg.solve(ima, dt*b)
|
||
|
|
|
||
|
|
# Similarly solve for the output equation matrices
|
||
|
|
cd = linalg.solve(ima.transpose(), c.transpose())
|
||
|
|
cd = cd.transpose()
|
||
|
|
dd = d + alpha*np.dot(c, bd)
|
||
|
|
|
||
|
|
elif method == 'bilinear' or method == 'tustin':
|
||
|
|
return cont2discrete(system, dt, method="gbt", alpha=0.5)
|
||
|
|
|
||
|
|
elif method == 'euler' or method == 'forward_diff':
|
||
|
|
return cont2discrete(system, dt, method="gbt", alpha=0.0)
|
||
|
|
|
||
|
|
elif method == 'backward_diff':
|
||
|
|
return cont2discrete(system, dt, method="gbt", alpha=1.0)
|
||
|
|
|
||
|
|
elif method == 'zoh':
|
||
|
|
# Build an exponential matrix
|
||
|
|
em_upper = np.hstack((a, b))
|
||
|
|
|
||
|
|
# Need to stack zeros under the a and b matrices
|
||
|
|
em_lower = np.hstack((np.zeros((b.shape[1], a.shape[0])),
|
||
|
|
np.zeros((b.shape[1], b.shape[1]))))
|
||
|
|
|
||
|
|
em = np.vstack((em_upper, em_lower))
|
||
|
|
ms = linalg.expm(dt * em)
|
||
|
|
|
||
|
|
# Dispose of the lower rows
|
||
|
|
ms = ms[:a.shape[0], :]
|
||
|
|
|
||
|
|
ad = ms[:, 0:a.shape[1]]
|
||
|
|
bd = ms[:, a.shape[1]:]
|
||
|
|
|
||
|
|
cd = c
|
||
|
|
dd = d
|
||
|
|
|
||
|
|
elif method == 'foh':
|
||
|
|
# Size parameters for convenience
|
||
|
|
n = a.shape[0]
|
||
|
|
m = b.shape[1]
|
||
|
|
|
||
|
|
# Build an exponential matrix similar to 'zoh' method
|
||
|
|
em_upper = linalg.block_diag(np.block([a, b]) * dt, np.eye(m))
|
||
|
|
em_lower = zeros((m, n + 2 * m))
|
||
|
|
em = np.block([[em_upper], [em_lower]])
|
||
|
|
|
||
|
|
ms = linalg.expm(em)
|
||
|
|
|
||
|
|
# Get the three blocks from upper rows
|
||
|
|
ms11 = ms[:n, 0:n]
|
||
|
|
ms12 = ms[:n, n:n + m]
|
||
|
|
ms13 = ms[:n, n + m:]
|
||
|
|
|
||
|
|
ad = ms11
|
||
|
|
bd = ms12 - ms13 + ms11 @ ms13
|
||
|
|
cd = c
|
||
|
|
dd = d + c @ ms13
|
||
|
|
|
||
|
|
elif method == 'impulse':
|
||
|
|
if not np.allclose(d, 0):
|
||
|
|
raise ValueError("Impulse method is only applicable"
|
||
|
|
"to strictly proper systems")
|
||
|
|
|
||
|
|
ad = linalg.expm(a * dt)
|
||
|
|
bd = ad @ b * dt
|
||
|
|
cd = c
|
||
|
|
dd = c @ b * dt
|
||
|
|
|
||
|
|
else:
|
||
|
|
raise ValueError("Unknown transformation method '%s'" % method)
|
||
|
|
|
||
|
|
return ad, bd, cd, dd, dt
|