add some omegas for quarter circle around origin poles in nyquist contour

bnavigator · bnavigator · commit a1979e3cf2ed · 2021-05-01T20:17:34.000+02:00
diff --git a/control/freqplot.py b/control/freqplot.py
@@ -620,7 +620,7 @@ def nyquist_plot(syslist, omega=None, plot=True, omega_limits=None,
 
     2. If a continuous-time system contains poles on or near the imaginary
        axis, a small indentation will be used to avoid the pole.  The radius
-       of the indentation is given by `indent_radius` and it is taken the the
+       of the indentation is given by `indent_radius` and it is taken to the
        right of stable poles and the left of unstable poles.  If a pole is
        exactly on the imaginary axis, the `indent_direction` parameter can be
        used to set the direction of indentation.  Setting `indent_direction`
@@ -675,13 +675,11 @@ def nyquist_plot(syslist, omega=None, plot=True, omega_limits=None,
     if not hasattr(syslist, '__iter__'):
         syslist = (syslist,)
 
-    omega, omega_range_given = _determine_omega_vector(syslist,
-                                                       omega,
-                                                       omega_limits,
-                                                       omega_num)
+    omega, omega_range_given = _determine_omega_vector(
+        syslist, omega, omega_limits, omega_num)
     if not omega_range_given:
-        # Replace first point with the origin
-        omega[0] = 0
+        # Start contour at zero frequency
+        omega[0] = 0.
 
     # Go through each system and keep track of the results
     counts, contours = [], []
@@ -713,9 +711,15 @@ def nyquist_plot(syslist, omega=None, plot=True, omega_limits=None,
             contour = 1j * omega_sys
 
         # Bend the contour around any poles on/near the imaginary axis
-        if isinstance(sys, (StateSpace, TransferFunction)) and \
-            sys.isctime() and indent_direction != 'none':
+        if isinstance(sys, (StateSpace, TransferFunction)) \
+                and sys.isctime() and indent_direction != 'none':
             poles = sys.pole()
+            if contour[1].imag > indent_radius \
+                    and 0. in poles and not omega_range_given:
+                # add some points for quarter circle around poles at origin
+                contour = np.concatenate(
+                    (1j * np.linspace(0., indent_radius, 50),
+                     contour[1:]))
             for i, s in enumerate(contour):
                 # Find the nearest pole
                 p = poles[(np.abs(poles - s)).argmin()]
@@ -1185,7 +1189,8 @@ def singular_values_plot(syslist, omega=None,
 
 
 # Determine the frequency range to be used
-def _determine_omega_vector(syslist, omega_in, omega_limits, omega_num):
+def _determine_omega_vector(syslist, omega_in, omega_limits, omega_num,
+                            replace_origin=False):
     """Determine the frequency range for a frequency-domain plot
     according to a standard logic.
 
@@ -1221,7 +1226,6 @@ def _determine_omega_vector(syslist, omega_in, omega_limits, omega_num):
         omega_in or through omega_limits. False if both omega_in
         and omega_limits are None.
     """
-
     omega_range_given = True
 
     if omega_in is None:
@@ -1244,12 +1248,14 @@ def _determine_omega_vector(syslist, omega_in, omega_limits, omega_num):
 
 # Compute reasonable defaults for axes
 def _default_frequency_range(syslist, Hz=None, number_of_samples=None,
-                             feature_periphery_decades=None):
-    """Compute a reasonable default frequency range for frequency
-    domain plots.
+                             feature_periphery_decades=None,
+                             replace_origin=False):
+    """Compute a default frequency range for frequency domain plots.
 
-    Finds a reasonable default frequency range by examining the features
-    (poles and zeros) of the systems in syslist.
+    This code looks at the poles and zeros of all of the systems that
+    we are plotting and sets the frequency range to be one decade above
+    and below the min and max feature frequencies, rounded to the nearest
+    integer.  If no features are found, it returns logspace(-1, 1)
 
     Parameters
     ----------
@@ -1280,12 +1286,6 @@ def _default_frequency_range(syslist, Hz=None, number_of_samples=None,
     >>> omega = _default_frequency_range(sys)
 
     """
-    # This code looks at the poles and zeros of all of the systems that
-    # we are plotting and sets the frequency range to be one decade above
-    # and below the min and max feature frequencies, rounded to the nearest
-    # integer.  It excludes poles and zeros at the origin.  If no features
-    # are found, it turns logspace(-1, 1)
-
     # Set default values for options
     number_of_samples = config._get_param(
         'freqplot', 'number_of_samples', number_of_samples)
@@ -1307,30 +1307,31 @@ def _default_frequency_range(syslist, Hz=None, number_of_samples=None,
                 features_ = np.concatenate((np.abs(sys.pole()),
                                             np.abs(sys.zero())))
                 # Get rid of poles and zeros at the origin
-                features_ = features_[features_ != 0.0]
-                features = np.concatenate((features, features_))
+                toreplace = features_ == 0.0
+                if np.any(toreplace):
+                    features_ = features_[~toreplace]
             elif sys.isdtime(strict=True):
                 fn = math.pi * 1. / sys.dt
                 # TODO: What distance to the Nyquist frequency is appropriate?
                 freq_interesting.append(fn * 0.9)
 
                 features_ = np.concatenate((sys.pole(),
                                             sys.zero()))
-                # Get rid of poles and zeros
-                # * at the origin and real <= 0 & imag==0: log!
+                # Get rid of poles and zeros on the real axis (imag==0)
+                # * origin and real < 0
                 # * at 1.: would result in omega=0. (logaritmic plot!)
-                features_ = features_[
-                    (features_.imag != 0.0) | (features_.real > 0.)]
-                features_ = features_[
-                    np.bitwise_not((features_.imag == 0.0) &
-                                   (np.abs(features_.real - 1.0) < 1.e-10))]
+                toreplace = (features_.imag == 0.0) & (
+                                    (features_.real <= 0.) |
+                                    (np.abs(features_.real - 1.0) < 1.e-10))
+                if np.any(toreplace):
+                    features_ = features_[~toreplace]
                 # TODO: improve
-                features__ = np.abs(np.log(features_) / (1.j * sys.dt))
-                features = np.concatenate((features, features__))
+                features_ = np.abs(np.log(features_) / (1.j * sys.dt))
             else:
                 # TODO
                 raise NotImplementedError(
                     "type of system in not implemented now")
+            features = np.concatenate((features, features_))
         except NotImplementedError:
             pass
 
