Speed up forced_response (lsim)

cwrowley · cwrowley · commit d7d278ba6072 · 2015-03-22T20:46:55.000-04:00
Fixes #48 The old algorithm called a general-purpose ODE integrator at each timestep, and this could be extremely slow (hundreds of times slower than scipy.signal.lsim). This new algorithm evaluates the matrix exponential once, and uses it to advance the simulation at each step. Importantly, the old algorithm would work for variable timesteps, but the new version assumes a fixed timestep (as does Matlab's lsim). This is the usual case, and it is the price one pays for increased speed. Note that this algorithm is the same idea as the algorithm used in scipy.signal.lsim, but the scipy version requires inverting the `A` matrix, and thus does not work if the system has a pole at the origin. The algorithm used here works even if there is a pole at the origin. This commit also adds a test for this case (the double integrator).
diff --git a/control/tests/timeresp_test.py b/control/tests/timeresp_test.py
@@ -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)
diff --git a/control/timeresp.py b/control/timeresp.py
@@ -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).
@@ -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):
@@ -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:
@@ -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
