add warning for nyquist_plot() when indent_radius is too large

murrayrm · murrayrm · commit 18e997c037db · 2022-04-15T17:52:10.000-07:00
diff --git a/control/freqplot.py b/control/freqplot.py
@@ -723,35 +723,72 @@ def nyquist_plot(syslist, omega=None, plot=True, omega_limits=None,
                 and indent_direction != 'none':
             if sys.isctime():
                 splane_poles = sys.poles()
+                splane_cl_poles = sys.feedback().poles()
             else:
                 # map z-plane poles to s-plane, ignoring any at the origin
                 # because we don't need to indent for them
                 zplane_poles = sys.poles()
                 zplane_poles = zplane_poles[~np.isclose(abs(zplane_poles), 0.)]
                 splane_poles = np.log(zplane_poles)/sys.dt
 
+                zplane_cl_poles = sys.feedback().poles()
+                zplane_cl_poles = zplane_cl_poles[
+                    ~np.isclose(abs(zplane_poles), 0.)]
+                splane_cl_poles = np.log(zplane_cl_poles)/sys.dt
+
+            #
+            # Check to make sure indent radius is small enough
+            #
+            # If there is a closed loop pole that is near the imaginary access
+            # at a point that is near an open loop pole, it is possible that
+            # indentation might skip or create an extraneous encirclement.
+            # We check for that situation here and generate a warning if that
+            # could happen.
+            #
+            for p_cl in splane_cl_poles:
+                # See if any closed loop poles are near the imaginary axis
+                if abs(p_cl.real) <= indent_radius:
+                    # See if any open loop poles are close to closed loop poles
+                    p_ol = splane_poles[
+                        (np.abs(splane_poles - p_cl)).argmin()]
+
+                    if abs(p_ol - p_cl) <= indent_radius:
+                        warnings.warn(
+                            "indented contour may miss closed loop pole; "
+                            "consider reducing indent_radius to be less than "
+                            f"{abs(p_ol - p_cl):5.2g}", stacklevel=2)
+
+            # See if we should add some frequency points near the origin
             if splane_contour[1].imag > indent_radius \
                     and np.any(np.isclose(abs(splane_poles), 0)) \
                     and not omega_range_given:
                 # add some points for quarter circle around poles at origin
+                # (these will get indented left or right below)
                 splane_contour = np.concatenate(
                     (1j * np.linspace(0., indent_radius, 50),
                     splane_contour[1:]))
+
             for i, s in enumerate(splane_contour):
                 # Find the nearest pole
                 p = splane_poles[(np.abs(splane_poles - s)).argmin()]
+
                 # See if we need to indent around it
                 if abs(s - p) < indent_radius:
+                    # Figure out how much to offset (simple trigonometry)
+                    offset = np.sqrt(indent_radius ** 2 - (s-p).imag ** 2) \
+                        -(s-p).real
+
+                    # Figure out which way to offset the contour point
                     if p.real < 0 or (np.isclose(p.real, 0) \
                             and indent_direction == 'right'):
                         # Indent to the right
-                        splane_contour[i] += \
-                            np.sqrt(indent_radius ** 2 - (s-p).imag ** 2)
+                        splane_contour[i] += offset
+
                     elif p.real > 0 or (np.isclose(p.real, 0) \
                             and indent_direction == 'left'):
                         # Indent to the left
-                        splane_contour[i] -= \
-                            np.sqrt(indent_radius ** 2 - (s-p).imag ** 2)
+                        splane_contour[i] -= offset
+
                     else:
                         ValueError("unknown value for indent_direction")
 
diff --git a/control/tests/nyquist_test.py b/control/tests/nyquist_test.py
@@ -41,6 +41,33 @@ def test_nyquist_basic():
     N_sys = ct.nyquist_plot(sys)
     assert _Z(sys) == N_sys + _P(sys)
 
+    # Previously identified bug
+    #
+    # This example has an open loop pole at -0.06 and a closed loop pole at
+    # 0.06, so if you use an indent_radius of larger than 0.12, then the
+    # encirclements computed by nyquist_plot() will not properly predict
+    # stability.  A new warning messages was added to catch this case.
+    #
+    A = np.array([
+        [-3.56355873, -1.22980795, -1.5626527 , -0.4626829 , -0.16741484],
+        [-8.52361371, -3.60331459, -3.71574266, -0.43839201,  0.41893656],
+        [-2.50458726, -0.72361335, -1.77795489, -0.4038419 ,  0.52451147],
+        [-0.281183  ,  0.23391825,  0.19096003, -0.9771515 ,  0.66975606],
+        [-3.04982852, -1.1091943 , -1.40027242, -0.1974623 , -0.78930791]])
+    B = np.array([[-0.], [-1.42827213], [ 0.76806551], [-1.07987454], [0.]])
+    C = np.array([[-0.,  0.35557249,  0.35941791, -0., -1.42320969]])
+    D = np.array([[0]])
+    sys = ct.ss(A, B, C, D)
+
+    # With a small indent_radius, all should be fine
+    N_sys = ct.nyquist_plot(sys, indent_radius=0.001)
+    assert _Z(sys) == N_sys + _P(sys)
+
+    # With a larger indent_radius, we get a warning message + wrong answer
+    with pytest.warns(UserWarning, match="contour may miss closed loop pole"):
+        N_sys = ct.nyquist_plot(sys, indent_radius=0.2)
+        assert _Z(sys) != N_sys + _P(sys)
+
     # Unstable system
     sys = ct.tf([10], [1, 2, 2, 1])
     N_sys = ct.nyquist_plot(sys)
