8000 Merge pull request #51 from cwrowley/lsim · basicmachines/python-control@4f6165a · GitHub
[go: up one dir, main page]
Skip to content

Commit 4f6165a

Browse files
murrayrmmurrayrm
committed
Merge pull request python-control#51 from cwrowley/lsim
Speed up forced_response/lsim
2 parents 5ac5100 + d7d278b commit 4f6165a

File tree

3 files changed

+74
-26
lines changed

3 files changed

+74
-26
lines changed

‎.gitignore

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,3 +7,4 @@ control/version.py
77
build.log
88
*.egg-info/
99
.coverage
10+
doc/_build

‎control/tests/timeresp_test.py

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -161,6 +161,40 @@ def test_forced_response(self):
161161
_t, yout, _xout = forced_response(self.mimo_ss1, t, u, x0)
162162
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)
163163

164+
def test_lsim_double_integrator(self):
165+
# Note: scipy.signal.lsim fails if A is not invertible
166+
A = np.mat("0. 1.;0. 0.")
167+
B = np.mat("0.; 1.")
168+
C = np.mat("1. 0.")
169+
D = 0.
170+
sys = StateSpace(A, B, C, D)
171+
172+
def check(u, x0, xtrue):
173+
_t, yout, xout = forced_response(sys, t, u, x0)
174+
np.testing.assert_array_almost_equal(xout, xtrue, decimal=6)
175+
ytrue = np.squeeze(np.asarray(C.dot(xtrue)))
176+
np.testing.assert_array_almost_equal(yout, ytrue, decimal=6)
177+
178+
# test with zero input
179+
npts = 10
180+
t = np.linspace(0, 1, npts)
181+
u = np.zeros_like(t)
182+
x0 = np.array([2., 3.])
183+
xtrue = np.zeros((2, npts))
184+
xtrue[0, :] = x0[0] + t * x0[1]
185+
xtrue[1, :] = x0[1]
186+
check(u, x0, xtrue)
187+
188+
# test with step input
189+
u = np.ones_like(t)
190+
xtrue = np.array([0.5 * t**2, t])
191+
x0 = np.array([0., 0.])
192+
check(u, x0, xtrue)
193+
194+
# test with linear input
195+
u = t
196+
xtrue = np.array([1./6. * t**3, 0.5 * t**2])
197+
check(u, x0, xtrue)
164198

165199
def suite():
166200
return unittest.TestLoader().loadTestsFromTestCase(TestTimeresp)

‎control/timeresp.py

Lines changed: 39 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -249,8 +249,7 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, **keywords):
249249
LTI system to simulate
250250
251251
T: array-like
252-
Time steps at which the input is defined, numbers must be (strictly
253-
monotonic) increasing.
252+
Time steps at which the input is defined; values must be evenly spaced.
254253
255254
U: array-like or number, optional
256255
Input array giving input at each time `T` (default = 0).
@@ -298,6 +297,7 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, **keywords):
298297
# d_type = A.dtype
299298
n_states = A.shape[0]
300299
n_inputs = B.shape[1]
300+
n_outputs = C.shape[0]
301301

302302
# Set and/or check time vector in discrete time case
303303
if isdtime(sys, strict=True):
@@ -323,28 +323,31 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, **keywords):
323323
T = _check_convert_array(T, [('any',), (1, 'any')],
324324
'Parameter ``T``: ', squeeze=True,
325325
transpose=transpose)
326-
if not all(T[1:] - T[:-1] > 0):
327-
raise ValueError('Parameter ``T``: time values must be '
328-
'(strictly monotonic) increasing numbers.')
326+
dt = T[1] - T[0]
327+
if not np.allclose(T[1:] - T[:-1], dt):
328+
raise ValueError('Parameter ``T``: time values must be equally spaced.')
329329
n_steps = len(T) # number of simulation steps
330330

331331
# create X0 if not given, test if X0 has correct shape
332332
X0 = _check_convert_array(X0, [(n_states,), (n_states, 1)],
333333
'Parameter ``X0``: ', squeeze=True)
334334

335+
xout = np.zeros((n_states, n_steps))
336+
xout[:, 0] = X0
337+
yout = np.zeros((n_outputs, n_steps))
338+
335339
# Separate out the discrete and continuous time cases
336340
if isctime(sys):
337341
# Solve the differential equation, copied from scipy.signal.ltisys.
338342
dot, squeeze, = np.dot, np.squeeze # Faster and shorter code
339343

340344
# Faster algorithm if U is zero
341345
if U is None or (isinstance(U, (int, float)) and U == 0):
342-
# Function that computes the time derivative of the linear system
343-
def f_dot(x, _t):
344-
return dot(A, x)
345-
346-
xout = sp.integrate.odeint(f_dot, X0, T, **keywords)
347-
yout = dot(C, xout.T)
346+
# Solve using matrix exponential
347+
expAdt = sp.linalg.expm(A * dt)
348+
for i in range(1, n_steps):
349+
xout[:, i] = dot(expAdt, xout[:, i-1])
350+
yout = dot(C, xout)
348351

349352
# General algorithm that interpolates U in between output points
350353
else:
@@ -354,26 +357,36 @@ def f_dot(x, _t):
354357
U = _check_convert_array(U, legal_shapes,
355358
'Parameter ``U``: ', squeeze=False,
356359
transpose=transpose)
357-
# convert 1D array to D2 array with only one row
360+
# convert 1D array to 2D array with only one row
358361
if len(U.shape) == 1:
359362
U = U.reshape(1, -1) # pylint: disable=E1103
360363

361-
# Create a callable that uses linear interpolation to
362-
# calculate the input at any time.
363-
compute_u = \
364-
sp.interpolate.interp1d(T, U, kind='linear', copy=False,
365-
axis=-1, bounds_error=False,
366-
fill_value=0)
367-
368-
# Function that computes the time derivative of the linear system
369-
def f_dot(x, t):
370-
return dot(A, x) + squeeze(dot(B, compute_u([t])))
371-
372-
xout = sp.integrate.odeint(f_dot, X0, T, **keywords)
373-
yout = dot(C, xout.T) + dot(D, U)
364+
# Algorithm: to integrate from time 0 to time dt, with linear
365+
# interpolation between inputs u(0) = u0 and u(dt) = u1, we solve
366+
# xdot = A x + B u, x(0) = x0
367+
# udot = (u1 - u0) / dt, u(0) = u0.
368+
#
369+
# Solution is
370+
# [ x(dt) ] [ A*dt B*dt 0 ] [ x0 ]
371+
# [ u(dt) ] = exp [ 0 0 I ] [ u0 ]
372+
# [u1 - u0] [ 0 0 0 ] [u1 - u0]
373+
374+
M = np.bmat([[A * dt, B * dt, np.zeros((n_states, n_inputs))],
375+
[np.zeros((n_inputs, n_states + n_inputs)),
376+
np.identity(n_inputs)],
377+
[np.zeros((n_inputs, n_states + 2 * n_inputs))]])
378+
expM = sp.linalg.expm(M)
379+
Ad = expM[:n_states, :n_states]
380+
Bd1 = expM[:n_states, n_states+n_inputs:]
381+
Bd0 = expM[:n_states, n_states:n_states + n_inputs] - Bd1
382+
383+
for i in range(1, n_steps):
384+
xout[:, i] = (dot(Ad, xout[:, i-1]) + dot(Bd0, U[:, i-1]) +
385+
dot(Bd1, U[:, i]))
386+
yout = dot(C, xout) + dot(D, U)
374387

375388
yout = squeeze(yout)
376-
xout = xout.T
389+
xout = squeeze(xout)
377390

378391
else:
379392
# Discrete time simulation using signal processing toolbox

0 commit comments

Comments
 (0)
0