2
# Author: Travis Oliphant 2001
5
from filter_design import tf2zpk, zpk2tf, normalize
6
from Numeric import product, zeros, concatenate, \
7
array, dot, transpose, arange, ones, Float
9
import scipy.interpolate as interpolate
10
import scipy.integrate as integrate
11
import scipy.linalg as linalg
12
from scipy_base import r_, c_, eye, real, atleast_1d, atleast_2d, poly, \
13
squeeze, diag, asarray
14
from Matrix import Matrix as Mat
17
"""Transfer function to state-space representation.
21
num, den -- sequences representing the numerator and denominator polynomials.
25
A, B, C, D -- state space representation of the system.
27
# Controller canonical state-space representation.
28
# if M+1 = len(num) and K+1 = len(den) then we must have M <= K
29
# states are found by asserting that X(s) = U(s) / D(s)
30
# then Y(s) = N(s) * X(s)
32
# A, B, C, and D follow quite naturally.
34
num, den = normalize(num, den) # Strips zeros, checks arrays
37
num = asarray([num],num.typecode())
41
raise ValueError, "Improper transfer function."
42
if (M == 0 or K == 0): # Null system
43
return array([],Float), array([], Float), array([], Float), \
46
# pad numerator to have same number of columns has denominator
47
num = c_[zeros((num.shape[0],K-M),num.typecode()), num]
55
return array([], Float), array([], Float), array([], Float), D
57
frow = -array([den[1:]])
58
A = r_[frow, eye(K-2, K-1)]
60
C = num[:,1:] - num[:,0] * den[1:]
63
def none_to_empty(arg):
69
def abcd_normalize(A=None, B=None, C=None, D=None):
70
"""Check state-space matrices and ensure they are rank-2.
72
A, B, C, D = map(none_to_empty, (A, B, C, D))
73
A, B, C, D = map(atleast_2d, (A, B, C, D))
75
if ((len(A.shape) > 2) or (len(B.shape) > 2) or \
76
(len(C.shape) > 2) or (len(D.shape) > 2)):
77
raise ValueError, "A, B, C, D arrays can be no larger than rank-2."
84
if (MC == 0) and (NC == 0) and (MD != 0) and (NA != 0):
87
if (MB == 0) and (NB == 0) and (MA != 0) and (ND != 0):
90
if (MD == 0) and (ND == 0) and (MC != 0) and (NB != 0):
93
if (MA == 0) and (NA == 0) and (MB != 0) and (NC != 0):
98
raise ValueError, "A must be square."
100
raise ValueError, "A and B must have the same number of rows."
102
raise ValueError, "A and C must have the same number of columns."
104
raise ValueError, "C and D must have the same number of rows."
106
raise ValueErrro, "B and D must have the same number of columns."
110
def ss2tf(A, B, C, D, input=0):
111
"""State-space to transfer function.
115
A, B, C, D -- state-space representation of linear system.
116
input -- For multiple-input systems, the input to use.
120
num, den -- Numerator and denominator polynomials (as sequences)
123
# transfer function is C (sI - A)**(-1) B + D
124
A, B, C, D = map(asarray, (A, B, C, D))
125
# Check consistency and
126
# make them all rank-2 arrays
127
A, B, C, D = abcd_normalize(A, B, C, D)
131
raise ValueError, "System does not have the input specified."
133
# make MOSI from possibly MOMI system.
136
B.shape = (B.shape[0],1)
142
if (product(B.shape) == 0) and (product(C.shape) == 0):
143
num = Numeric.ravel(D)
144
if (product(D.shape) == 0) and (product(A.shape) == 0):
149
num_states = A.shape[0]
150
type_test = A[:,0] + B[:,0] + C[0,:] + D
151
num = Numeric.zeros((nout, num_states+1),type_test.typecode())
152
for k in range(nout):
153
Ck = atleast_2d(C[k,:])
154
num[k] = poly(A - dot(B,Ck)) + (D[k]-1)*den
159
"""Zero-pole-gain representation to state-space representation
163
z, p, k -- zeros, poles (sequences), and gain of system
167
A, B, C, D -- state-space matrices.
169
return tf2ss(*zpk2tf(z,p,k))
171
def ss2zpk(A,B,C,D,input=0):
172
"""State-space representation to zero-pole-gain representation.
176
A, B, C, D -- state-space matrices.
177
input -- for multiple-input systems, the input to use.
181
z, p, k -- zeros and poles in sequences and gain constant.
183
return tf2zpk(*ss2tf(A,B,C,D,input=input))
186
"""Linear Time Invariant class which simplifies representation.
188
def __init__(self,*args,**kwords):
189
"""Initialize the LTI system using either:
190
(numerator, denominator)
192
(A, B, C, D) -- state-space.
195
if N == 2: # Numerator denominator transfer function input
196
self.__dict__['num'], self.__dict__['den'] = normalize(*args)
197
self.__dict__['zeros'], self.__dict__['poles'], \
198
self.__dict__['gain'] = tf2zpk(*args)
199
self.__dict__['A'], self.__dict__['B'], \
200
self.__dict__['C'], \
201
self.__dict__['D'] = tf2ss(*args)
203
if len(self.num.shape) > 1:
204
self.outputs = self.num.shape[0]
207
elif N == 3: # Zero-pole-gain form
208
self.__dict__['zeros'], self.__dict__['poles'], \
209
self.__dict__['gain'] = args
210
self.__dict__['num'], self.__dict__['den'] = zpk2tf(*args)
211
self.__dict__['A'], self.__dict__['B'], \
212
self.__dict__['C'], \
213
self.__dict__['D'] = zpk2ss(*args)
215
if len(self.zeros.shape) > 1:
216
self.outputs = self.zeros.shape[0]
219
elif N == 4: # State-space form
220
self.__dict__['A'], self.__dict__['B'], \
221
self.__dict__['C'], \
222
self.__dict__['D'] = abcd_normalize(*args)
223
self.__dict__['zeros'], self.__dict__['poles'], \
224
self.__dict__['gain'] = ss2zpk(*args)
225
self.__dict__['num'], self.__dict__['den'] = ss2tf(*args)
226
self.inputs = B.shape[-1]
227
self.outputs = C.shape[0]
229
raise ValueError, "Needs 2, 3, or 4 arguments."
231
def __setattr__(self, attr, val):
232
if attr in ['num','den']:
233
self.__dict__[attr] = val
234
self.__dict__['zeros'], self.__dict__['poles'], \
235
self.__dict__['gain'] = \
236
tf2zpk(self.num, self.den)
237
self.__dict__['A'], self.__dict__['B'], \
238
self.__dict__['C'], \
239
self.__dict__['D'] = \
240
tf2ss(self.num, self.den)
241
elif attr in ['zeros', 'poles', 'gain']:
242
self.__dict__[attr] = val
243
self.__dict__['num'], self.__dict__['den'] = \
245
self.poles, self.gain)
246
self.__dict__['A'], self.__dict__['B'], \
247
self.__dict__['C'], \
248
self.__dict__['D'] = \
250
self.poles, self.gain)
251
elif attr in ['A', 'B', 'C', 'D']:
252
self.__dict__[attr] = val
253
self.__dict__['zeros'], self.__dict__['poles'], \
254
self.__dict__['gain'] = \
255
ss2zpk(self.A, self.B,
257
self.__dict__['num'], self.__dict__['den'] = \
258
ss2tf(self.A, self.B,
261
self.__dict__[attr] = val
263
def impulse(self, X0=None, T=None, N=None):
264
return impulse(self, X0=X0, T=T, N=N)
266
def step(self, X0=None, T=None, N=None):
267
return step(self, X0=X0, T=T, N=N)
269
def output(self, U, T, X0=None):
270
return lsim(self, U, T, X0=X0)
273
def lsim2(system, U, T, X0=None):
274
"""Simulate output of a continuous-time linear system, using ODE solver.
278
system -- an instance of the LTI class or a tuple describing the
279
system. The following gives the number of elements in
280
the tuple and the interpretation.
282
3 (zeros, poles, gain)
284
U -- an input array describing the input at each time T
285
(linear interpolation is assumed between given times).
286
If there are multiple inputs, then each column of the
287
rank-2 array represents an input.
288
T -- the time steps at which the input is defined and at which
289
the output is desired.
290
X0 -- (optional, default=0) the initial conditions on the state vector.
292
Outputs: (T, yout, xout)
294
T -- the time values for the output.
295
yout -- the response of the system.
296
xout -- the time-evolution of the state-vector.
298
# system is an lti system or a sequence
300
# 3 (zeros, poles, gain)
302
# describing the system
303
# U is an input vector at times T
304
# if system describes multiple outputs
305
# then U can be a rank-2 array with the number of columns
306
# being the number of inputs
308
# rather than use lsim, use direct integration and matrix-exponential.
309
if isinstance(system, lti):
315
if len(U.shape) == 1:
316
U.shape = (U.shape[0],1)
318
if len(T.shape) != 1:
319
raise ValueError, "T must be a rank-1 array."
321
raise ValueError, "U must have the same number of rows as elements in T."
322
if sU[1] != sys.inputs:
323
raise ValueError, "System does not define that many inputs."
326
X0 = zeros(sys.B.shape[0],sys.A.typecode())
328
# for each output point directly integrate assume zero-order hold
329
# or linear interpolation.
331
ufunc = interpolate.linear_1d(T, U, axis=0, bounds_error=0, fill_value=0)
333
def fprime(x, t, sys, ufunc):
334
return dot(sys.A,x) + squeeze(dot(sys.B,ufunc([t])))
336
xout = integrate.odeint(fprime, X0, T, args=(sys, ufunc))
337
yout = dot(sys.C,transpose(xout)) + dot(sys.D,transpose(U))
338
return T, squeeze(transpose(yout)), xout
341
def lsim(system, U, T, X0=None, interp=1):
342
"""Simulate output of a continuous-time linear system.
346
system -- an instance of the LTI class or a tuple describing the
347
system. The following gives the number of elements in
348
the tuple and the interpretation.
350
3 (zeros, poles, gain)
352
U -- an input array describing the input at each time T
353
(interpolation is assumed between given times).
354
If there are multiple inputs, then each column of the
355
rank-2 array represents an input.
356
T -- the time steps at which the input is defined and at which
357
the output is desired.
358
X0 -- (optional, default=0) the initial conditions on the state vector.
359
interp -- linear (1) or zero-order hold (0) interpolation
361
Outputs: (T, yout, xout)
363
T -- the time values for the output.
364
yout -- the response of the system.
365
xout -- the time-evolution of the state-vector.
367
# system is an lti system or a sequence
369
# 3 (zeros, poles, gain)
371
# describing the system
372
# U is an input vector at times T
373
# if system describes multiple inputs
374
# then U can be a rank-2 array with the number of columns
375
# being the number of inputs
376
if isinstance(system, lti):
382
if len(U.shape) == 1:
383
U.shape = (U.shape[0],1)
385
if len(T.shape) != 1:
386
raise ValueError, "T must be a rank-1 array."
388
raise ValueError, "U must have the same number of rows as elements in T."
389
if sU[1] != sys.inputs:
390
raise ValueError, "System does not define that many inputs."
393
X0 = zeros(sys.B.shape[0],sys.A.typecode())
395
xout = zeros((len(T),sys.B.shape[0]),sys.A.typecode())
398
AT, BT = transpose(sys.A), transpose(sys.B)
400
lam, v = linalg.eig(A)
403
GT = dot(dot(vti,diag(Numeric.exp(dt*lam))),vt).astype(xout.typecode())
404
ATm1 = linalg.inv(AT)
405
ATm2 = dot(ATm1,ATm1)
406
I = eye(A.shape[0],typecode=A.typecode())
408
F1T = dot(dot(BT,GTmI),ATm1)
410
F2T = dot(BT,dot(GTmI,ATm2)/dt - ATm1)
412
for k in xrange(1,len(T)):
416
GT = dot(dot(vti,diag(Numeric.exp(dt*lam))),vt).astype(xout.typecode())
418
F1T = dot(dot(BT,GTmI),ATm1)
420
F2T = dot(BT,dot(GTmI,ATm2)/dt - ATm1)
422
xout[k] = dot(xout[k-1],GT) + dot(U[k-1],F1T)
424
xout[k] = xout[k] + dot((U[k]-U[k-1]),F2T)
426
yout = squeeze(dot(U,transpose(sys.D))) + squeeze(dot(xout,transpose(sys.C)))
427
return T, squeeze(yout), squeeze(xout)
430
def impulse(system, X0=None, T=None, N=None):
431
"""Impulse response of continuous-time system.
435
system -- an instance of the LTI class or a tuple with 2, 3, or 4
436
elements representing (num, den), (zero, pole, gain), or
437
(A, B, C, D) representation of the system.
438
X0 -- (optional, default = 0) inital state-vector.
439
T -- (optional) time points (autocomputed if not given).
440
N -- (optional) number of time points to autocompute (100 if not given).
444
T -- output time points,
445
yout -- impulse response of system (except possible singularities at 0).
447
if isinstance(system, lti):
458
vals = linalg.eigvals(sys.A)
459
tc = 1.0/min(abs(real(vals)))
460
T = arange(0,7*tc,7*tc / float(N))
461
h = zeros(T.shape, sys.A.typecode())
462
s,v = linalg.eig(sys.A)
465
for k in range(len(h)):
466
es = diag(Numeric.exp(s*T[k]))
467
eA = (dot(dot(v,es),vi)).astype(h.typecode())
468
h[k] = squeeze(dot(dot(C,eA),B))
471
def step(system, X0=None, T=None, N=None):
472
"""Step response of continuous-time system.
476
system -- an instance of the LTI class or a tuple with 2, 3, or 4
477
elements representing (num, den), (zero, pole, gain), or
478
(A, B, C, D) representation of the system.
479
X0 -- (optional, default = 0) inital state-vector.
480
T -- (optional) time points (autocomputed if not given).
481
N -- (optional) number of time points to autocompute (100 if not given).
485
T -- output time points,
486
yout -- step response of system.
488
if isinstance(system, lti):
495
vals = linalg.eigvals(sys.A)
496
tc = 1.0/min(abs(real(vals)))
497
T = arange(0,7*tc,7*tc / float(N))
498
U = ones(T.shape, sys.A.typecode())
499
vals = lsim(sys, U, T, X0=X0)
500
return vals[0], vals[1]
added
removed
Lib/signal/ltisys.py
