bnavigator · Apr 29, 2021 · Mar 27, 2021 · Mar 28, 2021 · Mar 28, 2021 · Mar 28, 2021
diff --git a/control/freqplot.py b/control/freqplot.py
@@ -56,7 +56,7 @@
 from .xferfcn import TransferFunction
 from . import config
 
-__all__ = ['bode_plot', 'nyquist_plot', 'gangof4_plot',
+__all__ = ['bode_plot', 'nyquist_plot', 'gangof4_plot', 'singular_values_plot',
            'bode', 'nyquist', 'gangof4']
 
 # Default values for module parameter variables
@@ -172,7 +172,7 @@ def bode_plot(syslist, omega=None,
     >>> mag, phase, omega = bode(sys)
 
     """
-    # Make a copy of the kwargs dictonary since we will modify it
+    # Make a copy of the kwargs dictionary since we will modify it
     kwargs = dict(kwargs)
 
     # Check to see if legacy 'Plot' keyword was used
@@ -1039,14 +1039,183 @@ def gangof4_plot(P, C, omega=None, **kwargs):
 
     plt.tight_layout()
 
+#
+# Singular value plot
+#
+
+
+# Default values for Bode plot configuration variables
+_singular_values_plot_default = {
+    'singular_values_plot.dB': False,           # Plot singular values in dB
+    'singular_values_plot.deg': True,           # Plot phase in degrees
+    'singular_values_plot.Hz': False,           # Plot frequency in Hertz
+    'singular_values_plot.grid': True,          # Turn on grid for gain and phase
+}
+
+
+def singular_values_plot(syslist, omega=None,
+                         plot=True, omega_limits=None, omega_num=None,
+                         *args, **kwargs):
+    """Singular value plot for a system
+
+    Plots a Singular Value plot for the system over a (optional) frequency range.
+
+    Parameters
+    ----------
+    syslist : linsys
+        List of linear systems (single system is OK).
+    omega : array_like
+        List of frequencies in rad/sec to be used for frequency response.
+    plot : bool
+        If True (default), generate the singular values plot.
+    omega_limits : array_like of two values
+        Limits of the frequency vector to generate.
+        If Hz=True the limits are in Hz otherwise in rad/s.
+    omega_num : int
+        Number of samples to plot.
+        Default value (1000) set by config.defaults['freqplot.number_of_samples'].
+    dB : bool
+        If True, plot result in dB.
+        Default value (False) set by config.defaults['singular_values_plot.dB'].
+    Hz : bool
+        If True, plot frequency in Hz (omega must be provided in rad/sec).
+        Default value (False) set by config.defaults['singular_values_plot.Hz']
 
+    Returns
+    -------
+    sigma : ndarray (or list of ndarray if len(syslist) > 1))
+        singular values
+    omega : ndarray (or list of ndarray if len(syslist) > 1))
+        frequency in rad/sec
+
+    Other Parameters
+    ----------------
+    grid : bool
+        If True, plot grid lines on gain and phase plots.  Default is set by
+        `config.defaults['singular_values_plot.grid']`.
+
+    Examples
+    --------
+    >>> den = [75, 1]
+    >>> sys = ct.tf([[[87.8], [-86.4]], [[108.2], [-109.6]]], [[den, den], [den, den]])
+    >>> omega = np.logspace(-4, 1, 1000)
+    >>> sigma, omega = singular_values_plot(sys)
+
+    """
+    # Make a copy of the kwargs dictionary since we will modify it
+    kwargs = dict(kwargs)
+
+    # Get values for params (and pop from list to allow keyword use in plot)
+    dB = config._get_param('singular_values_plot', 'dB', kwargs, singular_values_plot, pop=True)
+    Hz = config._get_param('singular_values_plot', 'Hz', kwargs, _bode_defaults, pop=True)
+    grid = config._get_param('singular_values_plot', 'grid', kwargs, _bode_defaults, pop=True)
+    plot = config._get_param('singular_values_plot', 'grid', plot, True)
+
+    # If argument was a singleton, turn it into a tuple
+    if not hasattr(syslist, '__iter__'):
+        syslist = (syslist,)
+
+    # Decide whether to go above Nyquist frequency
+    omega_range_given = True if omega is not None else False
+
+    if omega is None:
+        omega_num = config._get_param(
+            'freqplot', 'number_of_samples', omega_num)
+        if omega_limits is None:
+            # Select a default range if none is provided
+            omega = _default_frequency_range(syslist, number_of_samples=omega_num)
+        else:
+            omega_range_given = True
+            omega_limits = np.asarray(omega_limits)
+            if len(omega_limits) != 2:
+                raise ValueError("len(omega_limits) must be 2")
+            if Hz:
+                omega_limits *= 2. * math.pi
+            omega = np.logspace(np.log10(omega_limits[0]),
+                                np.log10(omega_limits[1]), num=omega_num,
+                                endpoint=True)
+    else:
+        omega = np.atleast_1d(omega)
+
+    if plot:
+        fig = plt.gcf()
+        ax_sigma = None
+
+        # Get the current axes if they already exist
+        for ax in fig.axes:
+            if ax.get_label() == 'control-sigma':
+                ax_sigma = ax
+
+        # If no axes present, create them from scratch
+        if ax_sigma is None:
+            plt.clf()
+            ax_sigma = plt.subplot(111, label='control-sigma')
+
+        color_cycle = plt.rcParams['axes.prop_cycle'].by_key()['color']
+
+    sigmas, omegas, nyquistfrqs = [], [], []
+    for idx_sys, sys in enumerate(syslist):
+        omega_sys = np.asarray(omega)
+        if sys.isdtime(strict=True):
+            nyquistfrq = math.pi / sys.dt
+            if not omega_range_given:
+                # limit up to and including nyquist frequency
+                omega_sys = np.hstack((
+                    omega_sys[omega_sys < nyquistfrq], nyquistfrq))
+            omega_complex = np.exp(1j * omega * sys.dt)
+        else:
+            nyquistfrq = None
+            omega_complex = 1j*omega
+
+        fresp = sys(omega_complex, squeeze=False)
+
+        fresp = fresp.transpose((2, 0, 1))
+        sigma = np.linalg.svd(fresp, compute_uv=False)
+
+        sigmas.append(sigma.transpose())  # return shape is "channel first"
+        omegas.append(omega_sys)
+        nyquistfrqs.append(nyquistfrq)
+
+        if plot:
+            color = color_cycle[idx_sys % len(color_cycle)]
+
+            nyquistfrq_plot = None
+            if Hz:
+                omega_plot = omega_sys / (2. * math.pi)
+                if nyquistfrq:
+                    nyquistfrq_plot = nyquistfrq / (2. * math.pi)
+            else:
+                omega_plot = omega_sys
+                if nyquistfrq:
+                    nyquistfrq_plot = nyquistfrq
+            sigma_plot = sigma
+
+            if dB:
+                ax_sigma.semilogx(omega_plot, 20 * np.log10(sigma_plot), color=color, *args, **kwargs)
+            else:
+                ax_sigma.loglog(omega_plot, sigma_plot, color=color, *args, **kwargs)
+
+            if nyquistfrq_plot is not None:
+                ax_sigma.axvline(x=nyquistfrq_plot, color=color)
+
+    # Add a grid to the plot + labeling
+    if plot:
+        ax_sigma.grid(grid, which='both')
+        ax_sigma.set_ylabel("Singular Values (dB)" if dB else "Singular Values")
+        ax_sigma.set_xlabel("Frequency (Hz)" if Hz else "Frequency (rad/sec)")
+
+    if len(syslist) == 1:
+        return sigmas[0], omegas[0]
+    else:
+        return sigmas, omegas
 #
 # Utility functions
 #
 # This section of the code contains some utility functions for
 # generating frequency domain plots
 #
 
+
 # Compute reasonable defaults for axes
 def _default_frequency_range(syslist, Hz=None, number_of_samples=None,
                              feature_periphery_decades=None):

diff --git a/control/tests/freqresp_test.py b/control/tests/freqresp_test.py
@@ -16,7 +16,7 @@
 from control.statesp import StateSpace
 from control.xferfcn import TransferFunction
 from control.matlab import ss, tf, bode, rss
-from control.freqplot import bode_plot, nyquist_plot
+from control.freqplot import bode_plot, nyquist_plot, singular_values_plot
 from control.tests.conftest import slycotonly
 
 pytestmark = pytest.mark.usefixtures("mplcleanup")
@@ -39,6 +39,7 @@ def ss_mimo():
     D = np.array([[0, 0]])
     return StateSpace(A, B, C, D)
 
+
 def test_freqresp_siso(ss_siso):
     """Test SISO frequency response"""
     omega = np.linspace(10e-2, 10e2, 1000)
@@ -508,3 +509,84 @@ def test_dcgain_consistency():
 
     sys_ss = ctrl.tf2ss(sys_tf)
     np.testing.assert_almost_equal(sys_ss.dcgain(), -1)
+
+
+# Testing of the singular_value_plot_function
+class TSys:
+    """Struct of test system"""
+    def __init__(self, sys=None, call_kwargs=None):
+        self.sys = sys
+        self.kwargs = call_kwargs if call_kwargs else {}
+
+    def __repr__(self):
+        """Show system when debugging"""
+        return self.sys.__repr__()
+
+
+@pytest.fixture
+def ss_mimo_t():
+    A = np.diag([-1/75.0, -1/75.0])
+    B = np.array([[87.8, -86.4],
+                  [108.2, -109.6]])/75.0
+    C = np.eye(2)
+    D = np.zeros((2, 2))
+    T = TSys(ss(A, B, C, D))
+    T.omega = 0.0
+    T.sigma = np.array([[197.20868123], [1.39141948]])
+    return T
+
+
+@pytest.fixture
+def ss_miso_t():
+    A = np.diag([-1 / 75.0])
+    B = np.array([[87.8, -86.4]]) / 75.0
+    C = np.array([[1]])
+    D = np.zeros((1, 2))
+    T = TSys(ss(A, B, C, D))
+    T.omega = 0.0
+    T.sigma = np.array([[123.1819792]])
+    return T
+
+
+@pytest.fixture
+def ss_simo_t():
+    A = np.diag([-1 / 75.0])
+    B = np.array([[1.0]]) / 75.0
+    C = np.array([[87.8], [108.2]])
+    D = np.zeros((2, 1))
+    T = TSys(ss(A, B, C, D))
+    T.omega = 0.0
+    T.sigma = np.array([[139.34159465]])
+    return T
+
+
+@pytest.fixture
+def ss_siso_t():
+    A = np.diag([-1 / 75.0])
+    B = np.array([[1.0]]) / 75.0
+    C = np.array([[87.8]])
+    D = np.zeros((1, 1))
+    T = TSys(ss(A, B, C, D))
+    T.omega = 0.0
+    T.sigma = np.array([[87.8]])
+    return T
+
+
+@pytest.fixture
+def tsystem(request, ss_mimo_t, ss_miso_t, ss_simo_t, ss_siso_t):
+
+    systems = {"ss_mimo": ss_mimo_t,
+               "ss_miso": ss_miso_t,
+               "ss_simo": ss_simo_t,
+               "ss_siso": ss_siso_t
+               }
+    return systems[request.param]
+
+
+@pytest.mark.parametrize("tsystem", ["ss_mimo", "ss_miso", "ss_simo", "ss_siso"], indirect=["tsystem"])
+def test_singular_values_plot(tsystem):
+    sys = tsystem.sys
+    omega = tsystem.omega
+    sigma_check = tsystem.sigma
+    sigma, omega = singular_values_plot(sys, omega, plot=False)
+    np.testing.assert_almost_equal(sigma, sigma_check)