fixed discrete-time simulation time output

murrayrm · murrayrm · commit 11b7797b3d9f · 2019-04-09T22:11:42.000-07:00
diff --git a/control/tests/timeresp_test.py b/control/tests/timeresp_test.py
@@ -45,6 +45,14 @@ def setUp(self):
                       "0. 9. ")
         self.mimo_ss1 = StateSpace(A, B, C, D)
 
+        # Create discrete time systems
+        self.siso_dtf1 = TransferFunction([1], [1, 1, 0.25], True)
+        self.siso_dtf2 = TransferFunction([1], [1, 1, 0.25], 0.2)
+        self.siso_dss1 = tf2ss(self.siso_dtf1)
+        self.siso_dss2 = tf2ss(self.siso_dtf2)
+        self.mimo_dss1 = StateSpace(A, B, C, D, True)
+        self.mimo_dss2 = c2d(self.mimo_ss1, 0.2)
+
     def test_step_response(self):
         # Test SISO system
         sys = self.siso_ss1
@@ -320,6 +328,123 @@ def test_step_robustness(self):
         t2, y2 = step_response(sys2, input=0)
         np.testing.assert_array_almost_equal(y1, y2)
 
+    def test_time_vector(self):
+        "Unit test: https://github.com/python-control/python-control/issues/239"
+        # Discrete time simulations with specified time vectors
+        Tin1 = np.arange(0, 5, 1)       # matches dtf1, dss1; multiple of 0.2
+        Tin2 = np.arange(0, 5, 0.2)     # matches dtf2, dss2
+        Tin3 = np.arange(0, 5, 0.5)     # incompatible with 0.2
+
+        # Initial conditions to use for the different systems
+        siso_x0 = [1, 2]
+        mimo_x0 = [1, 2, 3, 4]
+
+        #
+        # Easy cases: make sure that output sample time matches input
+        #
+        # No timebase in system => output should match input
+        #
+        # Initial response
+        tout, yout = initial_response(self.siso_dtf1, Tin2, siso_x0)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Impulse response
+        tout, yout = impulse_response(self.siso_dtf1, Tin2)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Step response
+        tout, yout = step_response(self.siso_dtf1, Tin2)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Forced response with specified time vector
+        tout, yout, xout = forced_response(self.siso_dtf1, Tin2, np.sin(Tin2))
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Forced response with no time vector, no sample time (should use 1)
+        tout, yout, xout = forced_response(self.siso_dtf1, None, np.sin(Tin1))
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin1)
+
+        # MIMO forced response
+        tout, yout, xout = forced_response(self.mimo_dss1, Tin1, 
+                                           (np.sin(Tin1), np.cos(Tin1)),
+                                           mimo_x0)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        self.assertEqual(np.shape(tout), np.shape(yout[1,:]))
+        np.testing.assert_array_equal(tout, Tin1)
+
+        # Matching timebase in system => output should match input
+        #
+        # Initial response
+        tout, yout = initial_response(self.siso_dtf2, Tin2, siso_x0)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Impulse response
+        tout, yout = impulse_response(self.siso_dtf2, Tin2)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Step response
+        tout, yout = step_response(self.siso_dtf2, Tin2)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Forced response
+        tout, yout, xout = forced_response(self.siso_dtf2, Tin2, np.sin(Tin2))
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Forced response with no time vector, use sample time
+        tout, yout, xout = forced_response(self.siso_dtf2, None, np.sin(Tin2))
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin2)
+
+        # Compatible timebase in system => output should match input
+        #
+        # Initial response
+        tout, yout = initial_response(self.siso_dtf2, Tin1, siso_x0)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin1)
+
+        # Impulse response
+        tout, yout = impulse_response(self.siso_dtf2, Tin1)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin1)
+
+        # Step response
+        tout, yout = step_response(self.siso_dtf2, Tin1)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin1)
+
+        # Forced response
+        tout, yout, xout = forced_response(self.siso_dtf2, Tin1, np.sin(Tin1))
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        np.testing.assert_array_equal(tout, Tin1)
+
+        #
+        # Interpolation of the input (to match scipy.signal.dlsim)
+        #
+        # Initial response
+        tout, yout, xout = forced_response(self.siso_dtf2, Tin1,
+                                           np.sin(Tin1), interpolate=True)
+        self.assertEqual(np.shape(tout), np.shape(yout[0,:]))
+        self.assertTrue(np.allclose(tout[1:] - tout[:-1],  self.siso_dtf2.dt))
+
+        #
+        # Incompatible cases: make sure an error is thrown
+        #
+        # System timebase and given time vector are incompatible
+        #
+        # Initial response
+        with self.assertRaises(Exception) as context:
+            tout, yout = initial_response(self.siso_dtf2, Tin3, siso_x0)
+        self.assertTrue(isinstance(context.exception, ValueError))
+
 def suite():
     return unittest.TestLoader().loadTestsFromTestCase(TestTimeresp)
 
diff --git a/control/timeresp.py b/control/timeresp.py
@@ -169,7 +169,8 @@ def shape_matches(s_legal, s_actual):
 
 
 # Forced response of a linear system
-def forced_response(sys, T=None, U=0., X0=0., transpose=False):
+def forced_response(sys, T=None, U=0., X0=0., transpose=False,
+                    interpolate=False):
     """Simulate the output of a linear system.
 
     As a convenience for parameters `U`, `X0`:
@@ -200,6 +201,13 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False):
         If True, transpose all input and output arrays (for backward
         compatibility with MATLAB and scipy.signal.lsim)
 
+    interpolate:bool
+        If True and system is a discrete time system, the input will
+        be interpolated between the given time steps and the output
+        will be given at system sampling rate.  Otherwise, only return
+        the output at the times given in `T`.  No effect on continuous
+        time simulations (default = False).
+
     Returns
     -------
     T: array
@@ -236,8 +244,8 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False):
             if U is None:
                 raise ValueError('Parameters ``T`` and ``U`` can\'t both be'
                                  'zero for discrete-time simulation')
-            # Set T to integers with same length as U
-            T = range(len(U))
+            # Set T to equally spaced samples with same length as U
+            T = np.array(range(len(U))) * (1 if sys.dt == True else sys.dt)
         else:
             # Make sure the input vector and time vector have same length
             # TODO: allow interpolation of the input vector
@@ -316,26 +324,53 @@ def forced_response(sys, T=None, U=0., X0=0., transpose=False):
                               dot(Bd1, U[:, i]))
             yout = dot(C, xout) + dot(D, U)
 
+        tout = T
         yout = squeeze(yout)
         xout = squeeze(xout)
 
     else:
+        # Discrete type system => use SciPy signal processing toolbox
+        if (sys.dt != True):
+            # Make sure that the time increment is a multiple of sampling time
+
+            # First make sure that time increment is bigger than sampling time
+            if dt < sys.dt:
+                raise ValueError("Time steps ``T`` must match sampling time")
+
+            # Now check to make sure it is a multiple (with check against
+            # sys.dt because floating point mod can have small errors
+            elif not (np.isclose(dt % sys.dt, 0) or
+                      np.isclose(dt % sys.dt, sys.dt)):
+                raise ValueError("Time steps ``T`` must be multiples of " \
+                                 "sampling time")
+        else:
+            sys.dt = dt         # For unspecified sampling time, use time incr
+
         # Discrete time simulation using signal processing toolbox
         dsys = (A, B, C, D, sys.dt)
+
+        # Use signal processing toolbox for the discrete time simulation
         # Transpose the input to match toolbox convention 
         tout, yout, xout = sp.signal.dlsim(dsys, np.transpose(U), T, X0)
 
+        if not interpolate:
+            # If dt is different from sys.dt, resample the output
+            inc = int(round(dt / sys.dt))
+            tout = T            # Return exact list of time steps
+            yout = yout[::inc,:]
+            xout = xout[::inc,:]
+
         # Transpose the output and state vectors to match local convention
         xout = sp.transpose(xout)
         yout = sp.transpose(yout)
 
     # See if we need to transpose the data back into MATLAB form
     if (transpose):
-        T = np.transpose(T)
+        tout = np.transpose(tout)
         yout = np.transpose(yout)
         xout = np.transpose(xout)
 
-    return T, yout, xout
+    return tout, yout, xout
 
 def _get_ss_simo(sys, input=None, output=None):
     """Return a SISO or SIMO state-space version of sys
