Merge pull request #997 from wueestry/nyquist_improvements

murrayrm · web-flow · commit 4acc78b8aec4 · 2024-06-30T09:29:51.000-07:00
Add extra information to nyquist plots
diff --git a/control/freqplot.py b/control/freqplot.py
@@ -1060,7 +1060,9 @@ def gen_zero_centered_series(val_min, val_max, period):
     'nyquist.max_curve_magnitude': 20,          # clip large values
     'nyquist.max_curve_offset': 0.02,           # offset of primary/mirror
     'nyquist.start_marker': 'o',                # marker at start of curve
-    'nyquist.start_marker_size': 4,             # size of the maker
+    'nyquist.start_marker_size': 4,             # size of the marker
+    'nyquist.circle_style':                     # style for unit circles
+      {'color': 'black', 'linestyle': 'dashed', 'linewidth': 1}
 }
 
 
@@ -1477,8 +1479,8 @@ def nyquist_response(
 
 def nyquist_plot(
         data, omega=None, plot=None, label_freq=0, color=None, label=None,
-        return_contour=None, title=None, legend_loc='upper right',
-        ax=None, **kwargs):
+        return_contour=None, title=None, legend_loc='upper right', ax=None,
+        unit_circle=False, mt_circles=None, ms_circles=None, **kwargs):
     """Nyquist plot for a system.
 
     Generates a Nyquist plot for the system over a (optional) frequency
@@ -1501,7 +1503,13 @@ def nyquist_plot(
         ``omega_limits``.
     color : string, optional
         Used to specify the color of the line and arrowhead.
-
+    unit_circle : bool, optional
+        If ``True``, display the unit circle, to read gain crossover frequency.
+    mt_circles : array_like, optional
+        Draw circles corresponding to the given magnitudes of sensitivity.
+    ms_circles : array_like, optional
+        Draw circles corresponding to the given magnitudes of complementary
+        sensitivity.
     **kwargs : :func:`matplotlib.pyplot.plot` keyword properties, optional
         Additional keywords (passed to `matplotlib`)
 
@@ -1856,7 +1864,48 @@ def _parse_linestyle(style_name, allow_false=False):
         # Mark the -1 point
         plt.plot([-1], [0], 'r+')
 
-        # Label the frequencies of the points
+        #
+        # Draw circles for gain crossover and sensitivity functions
+        #
+        theta = np.linspace(0, 2*np.pi, 100)
+        cos = np.cos(theta)
+        sin = np.sin(theta)
+        label_pos = 15
+
+        # Display the unit circle, to read gain crossover frequency
+        if unit_circle:
+            plt.plot(cos, sin, **config.defaults['nyquist.circle_style'])
+        
+        # Draw circles for given magnitudes of sensitivity
+        if ms_circles is not None:
+            for ms in ms_circles:
+                pos_x = -1 + (1/ms)*cos
+                pos_y = (1/ms)*sin
+                plt.plot(
+                    pos_x, pos_y, **config.defaults['nyquist.circle_style'])
+                plt.text(pos_x[label_pos], pos_y[label_pos], ms)
+
+        # Draw circles for given magnitudes of complementary sensitivity
+        if mt_circles is not None:
+            for mt in mt_circles:
+                if mt != 1:
+                    ct = -mt**2/(mt**2-1)  # Mt center
+                    rt = mt/(mt**2-1)  # Mt radius
+                    pos_x = ct+rt*cos
+                    pos_y = rt*sin
+                    plt.plot(
+                        pos_x, pos_y,
+                        **config.defaults['nyquist.circle_style'])
+                    plt.text(pos_x[label_pos], pos_y[label_pos], mt)
+                else:
+                    _, _, ymin, ymax = plt.axis()
+                    pos_y = np.linspace(ymin, ymax, 100)
+                    plt.vlines(
+                        -0.5, ymin=ymin, ymax=ymax,
+                        **config.defaults['nyquist.circle_style'])
+                    plt.text(-0.5, pos_y[label_pos], 1)
+
+        # Label the frequencies of the points on the Nyquist curve
         if label_freq:
             ind = slice(None, None, label_freq)
             omega_sys = np.imag(splane_contour[np.real(splane_contour) == 0])
diff --git a/control/tests/nyquist_test.py b/control/tests/nyquist_test.py
@@ -214,6 +214,22 @@ def test_nyquist_arrows(arrows):
     assert _Z(sys) == response.count + _P(sys)
 
 
+def test_sensitivity_circles():
+    A = np.array([
+        [-3.56355873, -1.22980795, -1.5626527 , -0.4626829],
+        [-8.52361371, -3.60331459, -3.71574266, -0.43839201],
+        [-2.50458726, -0.72361335, -1.77795489, -0.4038419],
+        [-0.281183  ,  0.23391825,  0.19096003, -0.9771515]])
+    B = np.array([[-0.], [-1.42827213], [ 0.76806551], [-1.07987454]])
+    C = np.array([[-0.,  0.35557249,  0.35941791, -0.]])
+    D = np.array([[0]])
+    sys1 = ct.ss(A, B, C, D)
+    sys2 = ct.ss(A, B, C, D, dt=0.1)
+    plt.figure()
+    ct.nyquist_plot(sys1, unit_circle=True, mt_circles=[0.9,1,1.1,1.2], ms_circles=[0.9,1,1.1,1.2])
+    ct.nyquist_plot(sys2, unit_circle=True, mt_circles=[0.9,1,1.1,1.2], ms_circles=[0.9,1,1.1,1.2])
+
+
 def test_nyquist_encirclements():
     # Example 14.14: effect of friction in a cart-pendulum system
     s = ct.tf('s')
@@ -518,6 +534,9 @@ def test_nyquist_frd():
     test_nyquist_arrows(3)
     test_nyquist_arrows([0.1, 0.5, 0.9])
 
+    print("Test sensitivity circles")
+    test_sensitivity_circles()
+
     print("Stability checks")
     test_nyquist_encirclements()
 
