Skip to content

Speed up forced_response/lsim #51

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
murrayrm merged 2 commits into from
Mar 29, 2015
Merged

Speed up forced_response/lsim #51

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f90798e
Ignore generated documentation
cwrowley Mar 23, 2015
d7d278b
Speed up forced_response (lsim)
cwrowley Mar 23, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions .gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,3 +7,4 @@ control/version.py
build.log
*.egg-info/
.coverage
doc/_build
34 changes: 34 additions & 0 deletions control/tests/timeresp_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -161,6 +161,40 @@ def test_forced_response(self):
_t, yout, _xout = forced_response(self.mimo_ss1, t, u, x0)
np.testing.assert_array_almost_equal(yout, youttrue, decimal=4)

def test_lsim_double_integrator(self):
# Note: scipy.signal.lsim fails if A is not invertible
A = np.mat("0. 1.;0. 0.")
B = np.mat("0.; 1.")
C = np.mat("1. 0.")
D = 0.
sys = StateSpace(A, B, C, D)

def check(u, x0, xtrue):
_t, yout, xout = forced_response(sys, t, u, x0)
np.testing.assert_array_almost_equal(xout, xtrue, decimal=6)
ytrue = np.squeeze(np.asarray(C.dot(xtrue)))
np.testing.assert_array_almost_equal(yout, ytrue, decimal=6)

# test with zero input
npts = 10
t = np.linspace(0, 1, npts)
u = np.zeros_like(t)
x0 = np.array([2., 3.])
xtrue = np.zeros((2, npts))
xtrue[0, :] = x0[0] + t * x0[1]
xtrue[1, :] = x0[1]
check(u, x0, xtrue)

# test with step input
u = np.ones_like(t)
xtrue = np.array([0.5 * t**2, t])
x0 = np.array([0., 0.])
check(u, x0, xtrue)

# test with linear input
u = t
xtrue = np.array([1./6. * t**3, 0.5 * t**2])
check(u, x0, xtrue)

def suite():
return unittest.TestLoader().loadTestsFromTestCase(TestTimeresp)
Expand Down
65 changes: 39 additions & 26 deletions control/timeresp.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -249,8 +249,7 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, **keywords):
LTI system to simulate

T: array-like
Time steps at which the input is defined, numbers must be (strictly
monotonic) increasing.
Time steps at which the input is defined; values must be evenly spaced.

U: array-like or number, optional
Input array giving input at each time `T` (default = 0).
Expand Down Expand Up @@ -298,6 +297,7 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False, **keywords):
# d_type = A.dtype
n_states = A.shape[0]
n_inputs = B.shape[1]
n_outputs = C.shape[0]

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

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

xout = np.zeros((n_states, n_steps))
xout[:, 0] = X0
yout = np.zeros((n_outputs, n_steps))

# Separate out the discrete and continuous time cases
if isctime(sys):
# Solve the differential equation, copied from scipy.signal.ltisys.
dot, squeeze, = np.dot, np.squeeze # Faster and shorter code

# Faster algorithm if U is zero
if U is None or (isinstance(U, (int, float)) and U == 0):
# Function that computes the time derivative of the linear system
def f_dot(x, _t):
return dot(A, x)

xout = sp.integrate.odeint(f_dot, X0, T, **keywords)
yout = dot(C, xout.T)
# Solve using matrix exponential
expAdt = sp.linalg.expm(A * dt)
for i in range(1, n_steps):
xout[:, i] = dot(expAdt, xout[:, i-1])
yout = dot(C, xout)

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

# Create a callable that uses linear interpolation to
# calculate the input at any time.
compute_u = \
sp.interpolate.interp1d(T, U, kind='linear', copy=False,
axis=-1, bounds_error=False,
fill_value=0)

# Function that computes the time derivative of the linear system
def f_dot(x, t):
return dot(A, x) + squeeze(dot(B, compute_u([t])))

xout = sp.integrate.odeint(f_dot, X0, T, **keywords)
yout = dot(C, xout.T) + dot(D, U)
# Algorithm: to integrate from time 0 to time dt, with linear
# interpolation between inputs u(0) = u0 and u(dt) = u1, we solve
# xdot = A x + B u, x(0) = x0
# udot = (u1 - u0) / dt, u(0) = u0.
#
# Solution is
# [ x(dt) ] [ A*dt B*dt 0 ] [ x0 ]
# [ u(dt) ] = exp [ 0 0 I ] [ u0 ]
# [u1 - u0] [ 0 0 0 ] [u1 - u0]

M = np.bmat([[A * dt, B * dt, np.zeros((n_states, n_inputs))],
[np.zeros((n_inputs, n_states + n_inputs)),
np.identity(n_inputs)],
[np.zeros((n_inputs, n_states + 2 * n_inputs))]])
expM = sp.linalg.expm(M)
Ad = expM[:n_states, :n_states]
Bd1 = expM[:n_states, n_states+n_inputs:]
Bd0 = expM[:n_states, n_states:n_states + n_inputs] - Bd1

for i in range(1, n_steps):
xout[:, i] = (dot(Ad, xout[:, i-1]) + dot(Bd0, U[:, i-1]) +
dot(Bd1, U[:, i]))
yout = dot(C, xout) + dot(D, U)

yout = squeeze(yout)
xout = xout.T
xout = squeeze(xout)

else:
# Discrete time simulation using signal processing toolbox
Expand Down