Merge pull request #729 from roryyorke/rory/linfnorm

murrayrm · web-flow · commit dd95f35c8529 · 2022-04-27T07:04:38.000-07:00
ENH: add `linform` to compute linear system L-infinity norm
diff --git a/control/statesp.py b/control/statesp.py
@@ -55,6 +55,7 @@
 from numpy.linalg import solve, eigvals, matrix_rank
 from numpy.linalg.linalg import LinAlgError
 import scipy as sp
+import scipy.linalg
 from scipy.signal import cont2discrete
 from scipy.signal import StateSpace as signalStateSpace
 from warnings import warn
@@ -65,7 +66,12 @@
 from . import config
 from copy import deepcopy
 
-__all__ = ['StateSpace', 'tf2ss', 'ssdata']
+try:
+    from slycot import ab13dd
+except ImportError:
+    ab13dd = None
+
+__all__ = ['StateSpace', 'tf2ss', 'ssdata', 'linfnorm']
 
 # Define module default parameter values
 _statesp_defaults = {
@@ -1895,3 +1901,61 @@ def ssdata(sys):
     """
     ss = _convert_to_statespace(sys)
     return ss.A, ss.B, ss.C, ss.D
+
+
+def linfnorm(sys, tol=1e-10):
+    """L-infinity norm of a linear system
+
+    Parameters
+    ----------
+    sys : LTI (StateSpace or TransferFunction)
+      system to evalute L-infinity norm of
+    tol : real scalar
+      tolerance on norm estimate
+
+    Returns
+    -------
+    gpeak : non-negative scalar
+      L-infinity norm
+    fpeak : non-negative scalar
+      Frequency, in rad/s, at which gpeak occurs
+
+    For stable systems, the L-infinity and H-infinity norms are equal;
+    for unstable systems, the H-infinity norm is infinite, while the
+    L-infinity norm is finite if the system has no poles on the
+    imaginary axis.
+
+    See also
+    --------
+    slycot.ab13dd : the Slycot routine linfnorm that does the calculation
+    """
+
+    if ab13dd is None:
+        raise ControlSlycot("Can't find slycot module 'ab13dd'")
+
+    a, b, c, d = ssdata(_convert_to_statespace(sys))
+    e = np.eye(a.shape[0])
+
+    n = a.shape[0]
+    m = b.shape[1]
+    p = c.shape[0]
+
+    if n == 0:
+        # ab13dd doesn't accept empty A, B, C, D;
+        # static gain case is easy enough to compute
+        gpeak = scipy.linalg.svdvals(d)[0]
+        # max svd is constant with freq; arbitrarily choose 0 as peak
+        fpeak = 0
+        return gpeak, fpeak
+
+    dico = 'C' if sys.isctime() else 'D'
+    jobe = 'I'
+    equil = 'S'
+    jobd = 'Z' if all(0 == d.flat) else 'D'
+
+    gpeak, fpeak = ab13dd(dico, jobe, equil, jobd, n, m, p, a, e, b, c, d, tol)
+
+    if dico=='D':
+        fpeak /= sys.dt
+
+    return gpeak, fpeak
diff --git a/control/tests/statesp_test.py b/control/tests/statesp_test.py
@@ -19,13 +19,15 @@
 from control.dtime import sample_system
 from control.lti import evalfr
 from control.statesp import StateSpace, _convert_to_statespace, tf2ss, \
-    _statesp_defaults, _rss_generate
+    _statesp_defaults, _rss_generate, linfnorm
 from control.iosys import ss, rss, drss
 from control.tests.conftest import ismatarrayout, slycotonly
 from control.xferfcn import TransferFunction, ss2tf
 
+
 from .conftest import editsdefaults
 
+
 class TestStateSpace:
     """Tests for the StateSpace class."""
 
@@ -1107,3 +1109,52 @@ def test_latex_repr_testsize(editsdefaults):
 
     gstatic = ss([], [], [], 1)
     assert gstatic._repr_latex_() is None
+
+
+class TestLinfnorm:
+    # these are simple tests; we assume ab13dd is correct
+    # python-control specific behaviour is:
+    #   - checking for continuous- and discrete-time
+    #   - scaling fpeak for discrete-time
+    #   - handling static gains
+
+    # the underdamped gpeak and fpeak are found from
+    #   gpeak = 1/(2*zeta*(1-zeta**2)**0.5)
+    #   fpeak = wn*(1-2*zeta**2)**0.5
+    @pytest.fixture(params=[
+        ('static', ct.tf, ([1.23],[1]), 1.23, 0),
+        ('underdamped', ct.tf, ([100],[1, 2*0.5*10, 100]), 1.1547005, 7.0710678),
+        ])
+    def ct_siso(self, request):
+        name, systype, sysargs, refgpeak, reffpeak = request.param
+        return systype(*sysargs), refgpeak, reffpeak
+
+    @pytest.fixture(params=[
+        ('underdamped', ct.tf, ([100],[1, 2*0.5*10, 100]), 1e-4, 1.1547005, 7.0710678),
+        ])
+    def dt_siso(self, request):
+        name, systype, sysargs, dt, refgpeak, reffpeak = request.param
+        return ct.c2d(systype(*sysargs), dt), refgpeak, reffpeak
+
+    @slycotonly
+    def test_linfnorm_ct_siso(self, ct_siso):
+        sys, refgpeak, reffpeak = ct_siso
+        gpeak, fpeak = linfnorm(sys)
+        np.testing.assert_allclose(gpeak, refgpeak)
+        np.testing.assert_allclose(fpeak, reffpeak)
+
+    @slycotonly
+    def test_linfnorm_dt_siso(self, dt_siso):
+        sys, refgpeak, reffpeak = dt_siso
+        gpeak, fpeak = linfnorm(sys)
+        # c2d pole-mapping has round-off
+        np.testing.assert_allclose(gpeak, refgpeak)
+        np.testing.assert_allclose(fpeak, reffpeak)
+
+    @slycotonly
+    def test_linfnorm_ct_mimo(self, ct_siso):
+        siso, refgpeak, reffpeak = ct_siso
+        sys = ct.append(siso, siso)
+        gpeak, fpeak = linfnorm(sys)
+        np.testing.assert_allclose(gpeak, refgpeak)
+        np.testing.assert_allclose(fpeak, reffpeak)
