Aluode/PerceptionLabPortable
0
1"""
2ltisys -- a collection of functions to convert linear time invariant systems
3from one representation to another.
4"""
5
6import numpy as np
7from numpy import (r_, eye, atleast_2d, poly, dot,
8 asarray, 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 = np.hstack((np.zeros((num.shape[0], K - M), dtype=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 (B.size == 0) and (C.size == 0):
270 num = np.ravel(D)
271 if (D.size == 0) and (A.size == 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 = np.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 References
388 ----------
389 .. [1] https://en.wikipedia.org/wiki/Discretization#Discretization_of_linear_state_space_models
390
391 .. [2] http://techteach.no/publications/discretetime_signals_systems/discrete.pdf
392
393 .. [3] G. Zhang, X. Chen, and T. Chen, Digital redesign via the generalized
394 bilinear transformation, Int. J. Control, vol. 82, no. 4, pp. 741-754,
395 2009.
396 (https://www.mypolyuweb.hk/~magzhang/Research/ZCC09_IJC.pdf)
397
398 .. [4] G. F. Franklin, J. D. Powell, and M. L. Workman, Digital control
399 of dynamic systems, 3rd ed. Menlo Park, Calif: Addison-Wesley,
400 pp. 204-206, 1998.
401
402 Examples
403 --------
404 We can transform a continuous state-space system to a discrete one:
405
406 >>> import numpy as np
407 >>> import matplotlib.pyplot as plt
408 >>> from scipy.signal import cont2discrete, lti, dlti, dstep
409
410 Define a continuous state-space system.
411
412 >>> A = np.array([[0, 1],[-10., -3]])
413 >>> B = np.array([[0],[10.]])
414 >>> C = np.array([[1., 0]])
415 >>> D = np.array([[0.]])
416 >>> l_system = lti(A, B, C, D)
417 >>> t, x = l_system.step(T=np.linspace(0, 5, 100))
418 >>> fig, ax = plt.subplots()
419 >>> ax.plot(t, x, label='Continuous', linewidth=3)
420
421 Transform it to a discrete state-space system using several methods.
422
423 >>> dt = 0.1
424 >>> for method in ['zoh', 'bilinear', 'euler', 'backward_diff', 'foh', 'impulse']:
425 ... d_system = cont2discrete((A, B, C, D), dt, method=method)
426 ... s, x_d = dstep(d_system)
427 ... ax.step(s, np.squeeze(x_d), label=method, where='post')
428 >>> ax.axis([t[0], t[-1], x[0], 1.4])
429 >>> ax.legend(loc='best')
430 >>> fig.tight_layout()
431 >>> plt.show()
432
433 """
434 if len(system) == 1:
435 return system.to_discrete()
436 if len(system) == 2:
437 sysd = cont2discrete(tf2ss(system[0], system[1]), dt, method=method,
438 alpha=alpha)
439 return ss2tf(sysd[0], sysd[1], sysd[2], sysd[3]) + (dt,)
440 elif len(system) == 3:
441 sysd = cont2discrete(zpk2ss(system[0], system[1], system[2]), dt,
442 method=method, alpha=alpha)
443 return ss2zpk(sysd[0], sysd[1], sysd[2], sysd[3]) + (dt,)
444 elif len(system) == 4:
445 a, b, c, d = system
446 else:
447 raise ValueError("First argument must either be a tuple of 2 (tf), "
448 "3 (zpk), or 4 (ss) arrays.")
449
450 if method == 'gbt':
451 if alpha is None:
452 raise ValueError("Alpha parameter must be specified for the "
453 "generalized bilinear transform (gbt) method")
454 elif alpha < 0 or alpha > 1:
455 raise ValueError("Alpha parameter must be within the interval "
456 "[0,1] for the gbt method")
457
458 if method == 'gbt':
459 # This parameter is used repeatedly - compute once here
460 ima = np.eye(a.shape[0]) - alpha*dt*a
461 ad = linalg.solve(ima, np.eye(a.shape[0]) + (1.0-alpha)*dt*a)
462 bd = linalg.solve(ima, dt*b)
463
464 # Similarly solve for the output equation matrices
465 cd = linalg.solve(ima.transpose(), c.transpose())
466 cd = cd.transpose()
467 dd = d + alpha*np.dot(c, bd)
468
469 elif method == 'bilinear' or method == 'tustin':
470 return cont2discrete(system, dt, method="gbt", alpha=0.5)
471
472 elif method == 'euler' or method == 'forward_diff':
473 return cont2discrete(system, dt, method="gbt", alpha=0.0)
474
475 elif method == 'backward_diff':
476 return cont2discrete(system, dt, method="gbt", alpha=1.0)
477
478 elif method == 'zoh':
479 # Build an exponential matrix
480 em_upper = np.hstack((a, b))
481
482 # Need to stack zeros under the a and b matrices
483 em_lower = np.hstack((np.zeros((b.shape[1], a.shape[0])),
484 np.zeros((b.shape[1], b.shape[1]))))
485
486 em = np.vstack((em_upper, em_lower))
487 ms = linalg.expm(dt * em)
488
489 # Dispose of the lower rows
490 ms = ms[:a.shape[0], :]
491
492 ad = ms[:, 0:a.shape[1]]
493 bd = ms[:, a.shape[1]:]
494
495 cd = c
496 dd = d
497
498 elif method == 'foh':
499 # Size parameters for convenience
500 n = a.shape[0]
501 m = b.shape[1]
502
503 # Build an exponential matrix similar to 'zoh' method
504 em_upper = linalg.block_diag(np.block([a, b]) * dt, np.eye(m))
505 em_lower = zeros((m, n + 2 * m))
506 em = np.block([[em_upper], [em_lower]])
507
508 ms = linalg.expm(em)
509
510 # Get the three blocks from upper rows
511 ms11 = ms[:n, 0:n]
512 ms12 = ms[:n, n:n + m]
513 ms13 = ms[:n, n + m:]
514
515 ad = ms11
516 bd = ms12 - ms13 + ms11 @ ms13
517 cd = c
518 dd = d + c @ ms13
519
520 elif method == 'impulse':
521 if not np.allclose(d, 0):
522 raise ValueError("Impulse method is only applicable "
523 "to strictly proper systems")
524
525 ad = linalg.expm(a * dt)
526 bd = ad @ b * dt
527 cd = c
528 dd = c @ b * dt
529
530 else:
531 raise ValueError(f"Unknown transformation method '{method}'")
532
533 return ad, bd, cd, dd, dt
534