Implement dcgain outside of matlab module

cwrowley · cwrowley · commit c51d665748dd · 2015-04-26T19:16:07.000-04:00
The dcgain function is now implemented as a method of the StateSpace and
TransferFunction classes.  There is also a routine lti.dcgain(sys) that
simply calls sys.dcgain().

Note that the DC gain is now returned as a numpy array (not a matrix),
and furthermore, single dimensions are removed with np.squeeze.  This is
a change from the previous implementation in matlab.dcgain().  The new
convention makes it easier to deal with SISO systems (a common case),
since instead of referring to a gain as g[0][0], you can refer to it
simply as g.  This seems preferable to me than returning a
np.matrix.  (The new convention is also more consistent with Matlab.)
diff --git a/control/lti.py b/control/lti.py
@@ -15,7 +15,7 @@
 from numpy import absolute, real
 
 __all__ = ['issiso', 'timebase', 'timebaseEqual', 'isdtime', 'isctime',
-           'pole', 'zero', 'damp', 'evalfr', 'freqresp']
+           'pole', 'zero', 'damp', 'evalfr', 'freqresp', 'dcgain']
 
 class LTI:
     """LTI is a parent class to linear time-invariant (LTI) system objects.
@@ -89,6 +89,11 @@ def damp(self):
         Z = -real(poles)/wn
         return wn, Z, poles
 
+    def dcgain(self):
+        """Return the zero-frequency gain"""
+        raise NotImplementedError("dcgain not implemented for %s objects" %
+                                  str(self.__class__))
+
 # Test to see if a system is SISO
 def issiso(sys, strict=False):
     if isinstance(sys, (int, float, complex)) and not strict:
@@ -215,11 +220,8 @@ def pole(sys):
     See Also
     --------
     zero
-
-    Notes
-    -----
-    This function is a wrapper for StateSpace.pole and
-    TransferFunction.pole.
+    TransferFunction.pole
+    StateSpace.pole
 
     """
 
@@ -243,16 +245,13 @@ def zero(sys):
     Raises
     ------
     NotImplementedError
-        when called on a TransferFunction object or a MIMO StateSpace object
+        when called on a MIMO system
 
     See Also
     --------
     pole
-
-    Notes
-    -----
-    This function is a wrapper for StateSpace.zero and
-    TransferFunction.zero.
+    StateSpace.zero
+    TransferFunction.zero
 
     """
 
@@ -391,3 +390,14 @@ def freqresp(sys, omega):
     """
 
     return sys.freqresp(omega)
+
+def dcgain(sys):
+    """Return the zero-frequency (or DC) gain of the given system
+
+    Returns
+    -------
+    gain : ndarray
+        The zero-frequency gain, or np.nan if the system has a pole
+        at the origin
+    """
+    return sys.dcgain()
diff --git a/control/matlab/__init__.py b/control/matlab/__init__.py
@@ -90,7 +90,7 @@
 
 # Import functions specific to Matlab compatibility package
 from .timeresp import *
-from .plots import *
+from .wrappers import *
 
 __doc__ += r"""
 The following tables give an overview of the module ``control.matlab``.
@@ -382,57 +382,3 @@
 ==  ==========================  ============================================
 
 """
-
-def dcgain(*args):
-    '''
-    Compute the gain of the system in steady state.
-
-    The function takes either 1, 2, 3, or 4 parameters:
-
-    Parameters
-    ----------
-    A, B, C, D: array-like
-        A linear system in state space form.
-    Z, P, k: array-like, array-like, number
-        A linear system in zero, pole, gain form.
-    num, den: array-like
-        A linear system in transfer function form.
-    sys: LTI (StateSpace or TransferFunction)
-        A linear system object.
-
-    Returns
-    -------
-    gain: matrix
-        The gain of each output versus each input:
-        :math:`y = gain \cdot u`
-
-    Notes
-    -----
-    This function is only useful for systems with invertible system
-    matrix ``A``.
-
-    All systems are first converted to state space form. The function then
-    computes:
-
-    .. math:: gain = - C \cdot A^{-1} \cdot B + D
-    '''
-    #Convert the parameters to state space form
-    if len(args) == 4:
-        A, B, C, D = args
-        sys = ss(A, B, C, D)
-    elif len(args) == 3:
-        Z, P, k = args
-        A, B, C, D = zpk2ss(Z, P, k)
-        sys = ss(A, B, C, D)
-    elif len(args) == 2:
-        num, den = args
-        sys = tf2ss(num, den)
-    elif len(args) == 1:
-        sys, = args
-        sys = ss(sys)
-    else:
-        raise ValueError("Function ``dcgain`` needs either 1, 2, 3 or 4 "
-                         "arguments.")
-    #gain = - C * A**-1 * B + D
-    gain = sys.D - sys.C * sys.A.I * sys.B
-    return gain
diff --git a/control/matlab/wrappers.py b/control/matlab/wrappers.py
@@ -1,10 +1,13 @@
 """
-Plotting routines for the Matlab compatibility module
+Wrappers for the Matlab compatibility module
 """
 
 import numpy as np
+from ..statesp import ss
+from ..xferfcn import tf
+from scipy.signal import zpk2tf
 
-__all__ = ['bode', 'ngrid']
+__all__ = ['bode', 'ngrid', 'dcgain']
 
 def bode(*args, **keywords):
     """Bode plot of the frequency response
@@ -103,3 +106,54 @@ def bode(*args, **keywords):
 def ngrid():
     return nichols_grid()
 ngrid.__doc__ = nichols_grid.__doc__
+
+def dcgain(*args):
+    '''
+    Compute the gain of the system in steady state.
+
+    The function takes either 1, 2, 3, or 4 parameters:
+
+    Parameters
+    ----------
+    A, B, C, D: array-like
+        A linear system in state space form.
+    Z, P, k: array-like, array-like, number
+        A linear system in zero, pole, gain form.
+    num, den: array-like
+        A linear system in transfer function form.
+    sys: LTI (StateSpace or TransferFunction)
+        A linear system object.
+
+    Returns
+    -------
+    gain: ndarray
+        The gain of each output versus each input:
+        :math:`y = gain \cdot u`
+
+    Notes
+    -----
+    This function is only useful for systems with invertible system
+    matrix ``A``.
+
+    All systems are first converted to state space form. The function then
+    computes:
+
+    .. math:: gain = - C \cdot A^{-1} \cdot B + D
+    '''
+    #Convert the parameters to state space form
+    if len(args) == 4:
+        A, B, C, D = args
+        return ss(A, B, C, D).dcgain()
+    elif len(args) == 3:
+        Z, P, k = args
+        num, den = zpk2tf(Z, P, k)
+        return tf(num, den).dcgain()
+    elif len(args) == 2:
+        num, den = args
+        return tf(num, den).dcgain()
+    elif len(args) == 1:
+        sys, = args
+        return sys.dcgain()
+    else:
+        raise ValueError("Function ``dcgain`` needs either 1, 2, 3 or 4 "
+                         "arguments.")
diff --git a/control/statesp.py b/control/statesp.py
@@ -50,6 +50,7 @@
 $Id$
 """
 
+import numpy as np
 from numpy import all, angle, any, array, asarray, concatenate, cos, delete, \
     dot, empty, exp, eye, matrix, ones, pi, poly, poly1d, roots, shape, sin, \
     zeros, squeeze
@@ -591,6 +592,27 @@ def sample(self, Ts, method='zoh', alpha=None):
         Ad, Bd, C, D, dt = cont2discrete(sys, Ts, method, alpha)
         return StateSpace(Ad, Bd, C, D, dt)
 
+    def dcgain(self):
+        """Return the zero-frequency gain
+
+        The zero-frequency gain of a state-space system is given by:
+
+        .. math: G(0) = - C A^{-1} B + D
+
+        Returns
+        -------
+        gain : ndarray
+            The zero-frequency gain, or np.nan if the system has a pole
+            at the origin
+        """
+        try:
+            gain = np.asarray(self.D -
+                              self.C.dot(np.linalg.solve(self.A, self.B)))
+        except LinAlgError:
+            # zero eigenvalue: singular matrix
+            return np.nan
+        return np.squeeze(gain)
+
 
 # TODO: add discrete time check
 def _convertToStateSpace(sys, **kw):
diff --git a/control/tests/lti_test.py b/control/tests/lti_test.py
@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+
+import unittest
+import numpy as np
+from control.lti import *
+from control.xferfcn import tf
+import numpy as np
+
+class TestUtils(unittest.TestCase):
+    def test_pole(self):
+        sys = tf(126, [-1, 42])
+        np.testing.assert_equal(sys.pole(), 42)
+        np.testing.assert_equal(pole(sys), 42)
+
+    def test_zero(self):
+        sys = tf([-1, 42], [1, 10])
+        np.testing.assert_equal(sys.zero(), 42)
+        np.testing.assert_equal(zero(sys), 42)
+
+    def test_damp(self):
+        zeta = 0.1
+        wn = 42
+        p = -wn * zeta + 1j * wn * np.sqrt(1 - zeta**2)
+        sys = tf(1, [1, 2 * zeta * wn, wn**2])
+        expected = ([wn, wn], [zeta, zeta], [p, p.conjugate()])
+        np.testing.assert_equal(sys.damp(), expected)
+        np.testing.assert_equal(damp(sys), expected)
+
+    def test_dcgain(self):
+        sys = tf(84, [1, 2])
+        np.testing.assert_equal(sys.dcgain(), 42)
+        np.testing.assert_equal(dcgain(sys), 42)
diff --git a/control/tests/matlab_test.py b/control/tests/matlab_test.py
@@ -283,7 +283,7 @@ def testDcgain(self):
 
         #All gain values must be approximately equal to the known gain
         np.testing.assert_array_almost_equal(
-            [gain_abcd[0,0], gain_zpk[0,0], gain_numden[0,0], gain_sys_ss[0,0],
+            [gain_abcd, gain_zpk, gain_numden, gain_sys_ss,
              gain_sim],
             [59, 59, 59, 59, 59])
 
diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py
@@ -10,7 +10,6 @@
 from control.statesp import StateSpace, _convertToStateSpace
 from control.xferfcn import TransferFunction
 
-
 class TestStateSpace(unittest.TestCase):
     """Tests for the StateSpace class."""
 
@@ -224,6 +223,17 @@ def testArrayAccessSS(self):
 
         assert sys1.dt == sys1_11.dt
 
+    def test_dcgain(self):
+        sys = StateSpace(-2.,6.,5.,0)
+        np.testing.assert_equal(sys.dcgain(), 15.)
+
+        sys2 = StateSpace(-2, [6., 4.], [[5.],[7.],[11]], np.zeros((3,2)))
+        expected = np.array([[15., 10.], [21., 14.], [33., 22.]])
+        np.testing.assert_array_equal(sys2.dcgain(), expected)
+
+        sys3 = StateSpace(0., 1., 1., 0.)
+        np.testing.assert_equal(sys3.dcgain(), np.nan)
+
 class TestRss(unittest.TestCase):
     """These are tests for the proper functionality of statesp.rss."""
 
diff --git a/control/tests/test_control_matlab.py b/control/tests/test_control_matlab.py
@@ -119,8 +119,8 @@ def test_dcgain_2(self):
         # print('gain_sim:', gain_sim)
 
         #All gain values must be approximately equal to the known gain
-        assert_array_almost_equal([gain_abcd[0,0],   gain_zpk[0,0],
-                                   gain_numden[0,0], gain_sys_ss[0,0], gain_sim],
+        assert_array_almost_equal([gain_abcd,   gain_zpk,
+                                   gain_numden, gain_sys_ss, gain_sim],
                                   [0.026948, 0.026948, 0.026948, 0.026948,
                                    0.026948],
                                   decimal=6)
diff --git a/control/tests/xferfcn_test.py b/control/tests/xferfcn_test.py
@@ -510,6 +510,22 @@ def testMatrixMult(self):
         np.testing.assert_array_almost_equal(H.num[1][0], H2.num[0][0])
         np.testing.assert_array_almost_equal(H.den[1][0], H2.den[0][0])
 
+    def test_dcgain(self):
+        sys = TransferFunction(6, 3)
+        np.testing.assert_equal(sys.dcgain(), 2)
+
+        sys2 = TransferFunction(6, [1, 3])
+        np.testing.assert_equal(sys2.dcgain(), 2)
+
+        sys3 = TransferFunction(6, [1, 0])
+        np.testing.assert_equal(sys3.dcgain(), np.inf)
+
+        num = [[[15], [21], [33]], [[10], [14], [22]]]
+        den = [[[1, 3], [2, 3], [3, 3]], [[1, 5], [2, 7], [3, 11]]]
+        sys4 = TransferFunction(num, den)
+        expected = [[5, 7, 11], [2, 2, 2]]
+        np.testing.assert_array_equal(sys4.dcgain(), expected)
+
 def suite():
     return unittest.TestLoader().loadTestsFromTestCase(TestXferFcn)
 
diff --git a/control/xferfcn.py b/control/xferfcn.py
@@ -943,6 +943,32 @@ def sample(self, Ts, method='zoh', alpha=None):
         numd, dend, dt = cont2discrete(sys, Ts, method, alpha)
         return TransferFunction(numd[0,:], dend, dt)
 
+    def dcgain(self):
+        """Return the zero-frequency (or DC) gain
+
+        For a transfer function G(s), the DC gain is G(0)
+
+        Returns
+        -------
+        gain : ndarray
+            The zero-frequency gain
+        """
+        gain = np.empty((self.outputs, self.inputs), dtype=float)
+        for i in range(self.outputs):
+            for j in range(self.inputs):
+                num = self.num[i][j][-1]
+                den = self.den[i][j][-1]
+                if den:
+                    gain[i][j] = num / den
+                else:
+                    if num:
+                        # numerator nonzero: infinite gain
+                        gain[i][j] = np.inf
+                    else:
+                        # numerator is zero too: give up
+                        gain[i][j] = np.nan
+        return np.squeeze(gain)
+
 # c2d function contributed by Benjamin White, Oct 2012
 def _c2dmatched(sysC, Ts):
     # Pole-zero match method of continuous to discrete time conversion
