Implement c2d without slycot (fixes #23)

cwrowley · cwrowley · commit 03c663703d21 · 2014-08-12T23:36:54.000-04:00
Conversion from continuous-time to discrete-time are now implemented as methods of the TransferFunction or StateSpace classes. The function sample_system() is still provided, and takes either a state-space system or transfer function as an argument. The implementations simply call the routine `cont2discrete` from `scipy.signal` This new organization into methods avoids some messy type checking in sample_system(), and also avoids some unnecessary conversions between state-space systems and transfer functions, which fixes #23.
diff --git a/control/dtime.py b/control/dtime.py
@@ -5,7 +5,6 @@
 Routines in this module:
 
 sample_system()
-_c2dmatched()
 """
 
 """Copyright (c) 2012 by California Institute of Technology
@@ -47,16 +46,10 @@
 
 """
 
-from scipy.signal import zpk2tf, tf2zpk
-import numpy as np
-from cmath import exp
-from warnings import warn
 from .lti import isctime
-from .statesp import StateSpace, _convertToStateSpace
-from .xferfcn import TransferFunction, _convertToTransferFunction
 
 # Sample a continuous time system
-def sample_system(sysc, Ts, method='matched'):
+def sample_system(sysc, Ts, method='zoh', alpha=None):
     """Convert a continuous time system to discrete time
 
     Creates a discrete time system from a continuous time system by
@@ -78,11 +71,8 @@ def sample_system(sysc, Ts, method='matched'):
 
     Notes
     -----
-    1. The conversion methods 'tustin' and 'zoh' require the
-       cont2discrete() function, including in SciPy 0.10.0 and above.
-
-    2. Additional methods 'foh' and 'impulse' are planned for future
-       implementation.
+    See `TransferFunction.sample` and `StateSpace.sample` for
+    further details.
 
     Examples
     --------
@@ -94,79 +84,4 @@ def sample_system(sysc, Ts, method='matched'):
     if not isctime(sysc):
         raise ValueError("First argument must be continuous time system")
 
-    # If we are passed a state space system, convert to transfer function first
-    if isinstance(sysc, StateSpace) and method == 'zoh':
-        try:
-            # try with slycot routine
-            from slycot import mb05nd
-            F, H = mb05nd(sysc.A, Ts)
-            return StateSpace(F, H*sysc.B, sysc.C, sysc.D, Ts)
-        except ImportError:
-            if sysc.inputs != 1 or sysc.outputs != 1:
-                raise TypeError(
-                    "mb05nd not found in slycot, or slycot not installed")
-
-    # TODO: implement MIMO version for other than ZOH state-space
-    if (sysc.inputs != 1 or sysc.outputs != 1):
-        raise NotImplementedError("MIMO implementation not available")
-
-    # SISO state-space, with other than ZOH, or failing slycot import,
-    # is handled by conversion to TF
-    if isinstance(sysc, StateSpace):
-        warn("sample_system: converting to transfer function")
-        sysc = _convertToTransferFunction(sysc)
-
-    # Decide what to do based on the methods available
-    if method == 'matched':
-        sysd = _c2dmatched(sysc, Ts)
-
-    elif method == 'tustin':
-        try:
-            from scipy.signal import cont2discrete
-            sys = [sysc.num[0][0], sysc.den[0][0]]
-            scipySysD = cont2discrete(sys, Ts, method='bilinear')
-            sysd = TransferFunction(scipySysD[0][0], scipySysD[1], Ts)
-        except ImportError:
-            raise TypeError("cont2discrete not found in scipy.signal; upgrade to v0.10.0+")
-
-    elif method == 'zoh':
-        try:
-            from scipy.signal import cont2discrete
-            sys = [sysc.num[0][0], sysc.den[0][0]]
-            scipySysD = cont2discrete(sys, Ts, method='zoh')
-            sysd = TransferFunction(scipySysD[0][0],scipySysD[1], Ts)
-        except ImportError:
-            raise TypeError("cont2discrete not found in scipy.signal; upgrade to v0.10.0+")
-
-    elif method == 'foh' or method == 'impulse':
-        raise ValueError("Method not developed yet")
-
-    else:
-        raise ValueError("Invalid discretization method: %s" % method)
-
-    # TODO: Convert back into the input form
-    # Set sampling time
-    return sysd
-
-# c2d function contributed by Benjamin White, Oct 2012
-def _c2dmatched(sysC, Ts):
-    # Pole-zero match method of continuous to discrete time conversion
-    szeros, spoles, sgain = tf2zpk(sysC.num[0][0], sysC.den[0][0])
-    zzeros = [0] * len(szeros)
-    zpoles = [0] * len(spoles)
-    pregainnum = [0] * len(szeros)
-    pregainden = [0] * len(spoles)
-    for idx, s in enumerate(szeros):
-        sTs = s*Ts
-        z = exp(sTs)
-        zzeros[idx] = z
-        pregainnum[idx] = 1-z
-    for idx, s in enumerate(spoles):
-        sTs = s*Ts
-        z = exp(sTs)
-        zpoles[idx] = z
-        pregainden[idx] = 1-z
-    zgain = np.multiply.reduce(pregainnum)/np.multiply.reduce(pregainden)
-    gain = sgain/zgain
-    sysDnum, sysDden = zpk2tf(zzeros, zpoles, gain)
-    return TransferFunction(sysDnum, sysDden, Ts)
+    return sysc.sample(Ts, method, alpha)
diff --git a/control/statesp.py b/control/statesp.py
@@ -83,7 +83,7 @@
 from numpy.random import rand, randn
 from numpy.linalg import inv, det, solve
 from numpy.linalg.linalg import LinAlgError
-from scipy.signal import lti
+from scipy.signal import lti, cont2discrete
 # from exceptions import Exception
 import warnings
 from .lti import Lti, timebase, timebaseEqual, isdtime
@@ -576,6 +576,52 @@ def __getitem__(self, indices):
                 self.C[i,:],
                 self.D[i,j], self.dt)
 
+    def sample(self, Ts, method='zoh', alpha=None):
+        """Convert a continuous time system to discrete time
+
+        Creates a discrete-time system from a continuous-time system by
+        sampling.  Multiple methods of conversion are supported.
+
+        Parameters
+        ----------
+        Ts : float
+            Sampling period
+        method :  {"gbt", "bilinear", "euler", "backward_diff", "zoh"}
+            Which method to use:
+
+               * gbt: generalized bilinear transformation
+               * bilinear: Tustin's approximation ("gbt" with alpha=0.5)
+               * euler: Euler (or forward differencing) method ("gbt" with alpha=0)
+               * backward_diff: Backwards differencing ("gbt" with alpha=1.0)
+               * zoh: zero-order hold (default)
+
+        alpha : float within [0, 1]
+            The generalized bilinear transformation weighting parameter, which
+            should only be specified with method="gbt", and is ignored otherwise
+
+        Returns
+        -------
+        sysd : StateSpace system
+            Discrete time system, with sampling rate Ts
+
+        Notes
+        -----
+        Uses the command 'cont2discrete' from scipy.signal
+
+        Examples
+        --------
+        >>> sys = StateSpace(0, 1, 1, 0)
+        >>> sysd = sys.sample(0.5, method='bilinear')
+
+        """
+        if not self.isctime():
+            raise ValueError("System must be continuous time system")
+
+        sys = (self.A, self.B, self.C, self.D)
+        Ad, Bd, C, D, dt = cont2discrete(sys, Ts, method, alpha)
+        return StateSpace(Ad, Bd, C, D, dt)
+
+
 # TODO: add discrete time check
 def _convertToStateSpace(sys, **kw):
     """Convert a system to state space form (if needed).
diff --git a/control/tests/discrete_test.py b/control/tests/discrete_test.py
@@ -259,29 +259,53 @@ def testSimulation(self):
         tout, yout, xout = forced_response(self.siso_ss2d, T, U, 0)
         tout, yout, xout = forced_response(self.siso_ss3d, T, U, 0)
 
-    @unittest.skip("skipping test_sample_system: not implemented for MIMO")
     def test_sample_system(self):
         # Make sure we can convert various types of systems
-        for sysc in (self.siso_ss1, self.siso_ss1c, self.siso_tf1c):
-            sysd = sample_system(sysc, 1, method='matched')
+        for sysc in (self.siso_tf1, self.siso_tf1c,
+                     self.siso_ss1, self.siso_ss1c,
+                     self.mimo_ss1, self.mimo_ss1c):
+            for method in ("zoh", "bilinear", "euler", "backward_diff"):
+                sysd = sample_system(sysc, 1, method=method)
+                self.assertEqual(sysd.dt, 1)
+
+        # Check "matched", defined only for SISO transfer functions
+        for sysc in (self.siso_tf1, self.siso_tf1c):
+            sysd = sample_system(sysc, 1, method="matched")
             self.assertEqual(sysd.dt, 1)
 
-            sysd = sample_system(sysc, 1, method='tustin')
-            self.assertEqual(sysd.dt, 1)
-
-            sysd = sample_system(sysc, 1, method='zoh')
-            self.assertEqual(sysd.dt, 1)
-            # TODO: put in other generic checks
-
-        for sysc in (self.mimo_ss1, self.mimo_ss1c):
-            sysd = sample_system(sysc, 1, method='zoh')
-            self.assertEqual(sysd.dt, 1)
-
-        # TODO: check results of converstion
-
         # Check errors
         self.assertRaises(ValueError, sample_system, self.siso_ss1d, 1)
         self.assertRaises(ValueError, sample_system, self.siso_ss1, 1, 'unknown')
+
+    def test_sample_ss(self):
+        # double integrators, two different ways
+        sys1 = StateSpace([[0.,1.],[0.,0.]], [[0.],[1.]], [[1.,0.]], 0.)
+        sys2 = StateSpace([[0.,0.],[1.,0.]], [[1.],[0.]], [[0.,1.]], 0.)
+        I = np.eye(2)
+        for sys in (sys1, sys2):
+            for h in (0.1, 0.5, 1, 2):
+                Ad = I + h * sys.A
+                Bd = h * sys.B + 0.5 * h**2 * (sys.A * sys.B)
+                sysd = sample_system(sys, h, method='zoh')
+                np.testing.assert_array_almost_equal(sysd.A, Ad)
+                np.testing.assert_array_almost_equal(sysd.B, Bd)
+                np.testing.assert_array_almost_equal(sysd.C, sys.C)
+                np.testing.assert_array_almost_equal(sysd.D, sys.D)
+                self.assertEqual(sysd.dt, h)
+
+    def test_sample_tf(self):
+        # double integrator
+        sys = TransferFunction(1, [1,0,0])
+        for h in (0.1, 0.5, 1, 2):
+            numd_expected = 0.5 * h**2 * np.array([1.,1.])
+            dend_expected = np.array([1.,-2.,1.])
+            sysd = sample_system(sys, h, method='zoh')
+            self.assertEqual(sysd.dt, h)
+            numd = sysd.num[0][0]
+            dend = sysd.den[0][0]
+            np.testing.assert_array_almost_equal(numd, numd_expected)
+            np.testing.assert_array_almost_equal(dend, dend_expected)
+
     def test_discrete_bode(self):
         # Create a simple discrete time system and check the calculation
         sys = TransferFunction([1], [1, 0.5], 1)
diff --git a/control/xferfcn.py b/control/xferfcn.py
@@ -76,7 +76,7 @@
 
 Author: Richard M. Murray
 Date: 24 May 09
-Revised: Kevin K. Chewn, Dec 10
+Revised: Kevin K. Chen, Dec 10
 
 $Id$
 
@@ -86,7 +86,8 @@
 from numpy import angle, any, array, empty, finfo, insert, ndarray, ones, \
     polyadd, polymul, polyval, roots, sort, sqrt, zeros, squeeze, exp, pi, \
     where, delete, real, poly, poly1d
-from scipy.signal import lti
+import numpy as np
+from scipy.signal import lti, tf2zpk, zpk2tf, cont2discrete
 from copy import deepcopy
 from warnings import warn
 from .lti import Lti, timebaseEqual, timebase, isdtime
@@ -928,6 +929,78 @@ def _common_den(self, imag_tol=None):
 
         return num, den
 
+    def sample(self, Ts, method='zoh', alpha=None):
+        """Convert a continuous-time system to discrete time
+
+        Creates a discrete-time system from a continuous-time system by
+        sampling.  Multiple methods of conversion are supported.
+
+        Parameters
+        ----------
+        Ts : float
+            Sampling period
+        method :  {"gbt", "bilinear", "euler", "backward_diff", "zoh", "matched"}
+            Which method to use:
+
+               * gbt: generalized bilinear transformation
+               * bilinear: Tustin's approximation ("gbt" with alpha=0.5)
+               * euler: Euler (or forward differencing) method ("gbt" with alpha=0)
+               * backward_diff: Backwards differencing ("gbt" with alpha=1.0)
+               * zoh: zero-order hold (default)
+
+        alpha : float within [0, 1]
+            The generalized bilinear transformation weighting parameter, which
+            should only be specified with method="gbt", and is ignored otherwise
+
+        Returns
+        -------
+        sysd : StateSpace system
+            Discrete time system, with sampling rate Ts
+
+        Notes
+        -----
+        1. Available only for SISO systems
+
+        2. Uses the command `cont2discrete` from `scipy.signal`
+
+        Examples
+        --------
+        >>> sys = TransferFunction(1, [1,1])
+        >>> sysd = sys.sample(0.5, method='bilinear')
+
+        """
+        if not self.isctime():
+            raise ValueError("System must be continuous time system")
+        if not self.issiso():
+            raise NotImplementedError("MIMO implementation not available")
+        if method == "matched":
+            return _c2dmatched(self, Ts)
+        sys = (self.num[0][0], self.den[0][0])
+        numd, dend, dt = cont2discrete(sys, Ts, method, alpha)
+        return TransferFunction(numd[0,:], dend, dt)
+
+# c2d function contributed by Benjamin White, Oct 2012
+def _c2dmatched(sysC, Ts):
+    # Pole-zero match method of continuous to discrete time conversion
+    szeros, spoles, sgain = tf2zpk(sysC.num[0][0], sysC.den[0][0])
+    zzeros = [0] * len(szeros)
+    zpoles = [0] * len(spoles)
+    pregainnum = [0] * len(szeros)
+    pregainden = [0] * len(spoles)
+    for idx, s in enumerate(szeros):
+        sTs = s*Ts
+        z = exp(sTs)
+        zzeros[idx] = z
+        pregainnum[idx] = 1-z
+    for idx, s in enumerate(spoles):
+        sTs = s*Ts
+        z = exp(sTs)
+        zpoles[idx] = z
+        pregainden[idx] = 1-z
+    zgain = np.multiply.reduce(pregainnum)/np.multiply.reduce(pregainden)
+    gain = sgain/zgain
+    sysDnum, sysDden = zpk2tf(zzeros, zpoles, gain)
+    return TransferFunction(sysDnum, sysDden, Ts)
 
 # Utility function to convert a transfer function polynomial to a string
 # Borrowed from poly1d library
