implementation of initial_phase, wrap_phase keywords for bode()

murrayrm · murrayrm · commit 775b03f67240 · 2020-12-31T14:13:49.000-08:00
diff --git a/control/freqplot.py b/control/freqplot.py
@@ -78,6 +78,7 @@
     'bode.deg': True,           # Plot phase in degrees
     'bode.Hz': False,           # Plot frequency in Hertz
     'bode.grid': True,          # Turn on grid for gain and phase
+    'bode.wrap_phase': False,   # Wrap the phase plot at a given value
 }
 
 
@@ -131,7 +132,18 @@ def bode_plot(syslist, omega=None,
     grid : bool
         If True, plot grid lines on gain and phase plots.  Default is set by
         `config.defaults['bode.grid']`.
-
+    initial_phase : float
+        Set the reference phase to use for the lowest frequency.  If set, the
+        initial phase of the Bode plot will be set to the value closest to the
+        value specified.  Default is 180 if wrap_phase is False, 0 if
+        wrap_phase is True.
+    wrap_phase : bool or float
+        If wrap_phase is `False`, then the phase will be unwrapped so that it
+        is continuously increasing or decreasing.  If wrap_phase is `True` the
+        phase will be restricted to the range [-180, 180) (or [:math:`-\\pi`,
+        :math:`\\pi`) radians). If `wrap_phase` is specified as a float, the
+        phase will be offset by 360 degrees if it falls below the specified
+        value.  Default to `False`, set by config.defaults['bode.wrap_phase'].
 
     The default values for Bode plot configuration parameters can be reset
     using the `config.defaults` dictionary, with module name 'bode'.
@@ -171,6 +183,10 @@ def bode_plot(syslist, omega=None,
     grid = config._get_param('bode', 'grid', kwargs, _bode_defaults, pop=True)
     plot = config._get_param('bode', 'grid', plot, True)
     margins = config._get_param('bode', 'margins', margins, False)
+    wrap_phase = config._get_param(
+        'bode', 'wrap_phase', kwargs, _bode_defaults, pop=True)
+    initial_phase = config._get_param(
+        'bode', 'initial_phase', kwargs, None, pop=True)
 
     # If argument was a singleton, turn it into a list
     if not getattr(syslist, '__iter__', False):
@@ -209,11 +225,47 @@ def bode_plot(syslist, omega=None,
                 # TODO: What distance to the Nyquist frequency is appropriate?
             else:
                 nyquistfrq = None
+
             # Get the magnitude and phase of the system
             mag_tmp, phase_tmp, omega_sys = sys.freqresp(omega_sys)
             mag = np.atleast_1d(np.squeeze(mag_tmp))
             phase = np.atleast_1d(np.squeeze(phase_tmp))
-            phase = unwrap(phase)
+
+            #
+            # Post-process the phase to handle initial value and wrapping
+            #
+
+            if initial_phase is None:
+                # Start phase in the range 0 to -360 w/ initial phase = -180
+                # If wrap_phase is true, use 0 instead (phase \in (-pi, pi])
+                initial_phase = -math.pi if wrap_phase is not True else 0
+            elif isinstance(initial_phase, (int, float)):
+                # Allow the user to override the default calculation
+                if deg:
+                    initial_phase = initial_phase/180. * math.pi
+            else:
+                raise ValueError("initial_phase must be a number.")
+
+            # Shift the phase if needed
+            if abs(phase[0] - initial_phase) > math.pi:
+                phase -= 2*math.pi * \
+                    round((phase[0] - initial_phase) / (2*math.pi))
+
+            # Phase wrapping
+            if wrap_phase is False:
+                phase = unwrap(phase)   # unwrap the phase
+            elif wrap_phase is True:
+                pass                    # default calculation OK
+            elif isinstance(wrap_phase, (int, float)):
+                phase = unwrap(phase)   # unwrap the phase first
+                if deg:
+                    wrap_phase *= math.pi/180.
+
+                # Shift the phase if it is below the wrap_phase
+                phase += 2*math.pi * np.maximum(
+                    0, np.ceil((wrap_phase - phase)/(2*math.pi)))
+            else:
+                raise ValueError("wrap_phase must be bool or float.")
 
             mags.append(mag)
             phases.append(phase)
@@ -270,7 +322,9 @@ def bode_plot(syslist, omega=None,
                                                label='control-bode-phase',
                                                sharex=ax_mag)
 
+                #
                 # Magnitude plot
+                #
                 if dB:
                     pltline = ax_mag.semilogx(omega_plot, 20 * np.log10(mag),
                                               *args, **kwargs)
@@ -285,19 +339,22 @@ def bode_plot(syslist, omega=None,
                 ax_mag.grid(grid and not margins, which='both')
                 ax_mag.set_ylabel("Magnitude (dB)" if dB else "Magnitude")
 
+                #
                 # Phase plot
-                if deg:
-                    phase_plot = phase * 180. / math.pi
-                else:
-                    phase_plot = phase
+                #
+                phase_plot = phase * 180. / math.pi if deg else phase
+
+                # Plot the data
                 ax_phase.semilogx(omega_plot, phase_plot, *args, **kwargs)
 
                 # Show the phase and gain margins in the plot
                 if margins:
+                    # Compute stability margins for the system
                     margin = stability_margins(sys)
-                    gm, pm, Wcg, Wcp = \
-                        margin[0], margin[1], margin[3], margin[4]
-                    # TODO: add some documentation describing why this is here
+                    gm, pm, Wcg, Wcp = (margin[i] for i in (0, 1, 3, 4))
+
+                    # Figure out sign of the phase at the first gain crossing
+                    # (needed if phase_wrap is True)
                     phase_at_cp = phases[0][(np.abs(omegas[0] - Wcp)).argmin()]
                     if phase_at_cp >= 0.:
                         phase_limit = 180.
@@ -307,6 +364,7 @@ def bode_plot(syslist, omega=None,
                     if Hz:
                         Wcg, Wcp = Wcg/(2*math.pi), Wcp/(2*math.pi)
 
+                    # Draw lines at gain and phase limits
                     ax_mag.axhline(y=0 if dB else 1, color='k', linestyle=':',
                                    zorder=-20)
                     ax_phase.axhline(y=phase_limit if deg else
@@ -315,6 +373,7 @@ def bode_plot(syslist, omega=None,
                     mag_ylim = ax_mag.get_ylim()
                     phase_ylim = ax_phase.get_ylim()
 
+                    # Annotate the phase margin (if it exists)
                     if pm != float('inf') and Wcp != float('nan'):
                         if dB:
                             ax_mag.semilogx(
@@ -327,7 +386,7 @@ def bode_plot(syslist, omega=None,
 
                         if deg:
                             ax_phase.semilogx(
-                                [Wcp, Wcp], [1e5, phase_limit+pm],
+                                [Wcp, Wcp], [1e5, phase_limit + pm],
                                 color='k', linestyle=':', zorder=-20)
                             ax_phase.semilogx(
                                 [Wcp, Wcp], [phase_limit + pm, phase_limit],
@@ -343,6 +402,7 @@ def bode_plot(syslist, omega=None,
                                              math.radians(phase_limit)],
                                 color='k', zorder=-20)
 
+                    # Annotate the gain margin (if it exists)
                     if gm != float('inf') and Wcg != float('nan'):
                         if dB:
                             ax_mag.semilogx(
@@ -360,11 +420,11 @@ def bode_plot(syslist, omega=None,
 
                         if deg:
                             ax_phase.semilogx(
-                                [Wcg, Wcg], [1e-8, phase_limit],
+                                [Wcg, Wcg], [0, phase_limit],
                                 color='k', linestyle=':', zorder=-20)
                         else:
                             ax_phase.semilogx(
-                                [Wcg, Wcg], [1e-8, math.radians(phase_limit)],
+                                [Wcg, Wcg], [0, math.radians(phase_limit)],
                                 color='k', linestyle=':', zorder=-20)
 
                     ax_mag.set_ylim(mag_ylim)
diff --git a/control/tests/freqresp_test.py b/control/tests/freqresp_test.py
@@ -9,6 +9,7 @@
 import matplotlib.pyplot as plt
 import numpy as np
 from numpy.testing import assert_allclose
+import math
 import pytest
 
 import control as ctrl
@@ -173,7 +174,7 @@ def test_bode_margin(dB, maginfty1, maginfty2, gminv,
                     rtol=1e-5)
 
     phase_to_infinity = (np.array([Wcg, Wcg]),
-                         np.array([1e-8, p0]))
+                         np.array([0, p0]))
     assert_allclose(phase_to_infinity, allaxes[1].lines[4].get_data(),
                     rtol=1e-5)
 
@@ -271,3 +272,68 @@ def test_options(editsdefaults):
     # Make sure we got the right number of points
     assert numpoints1 != numpoints3
     assert numpoints3 == 13
+
+@pytest.mark.parametrize(
+    "TF, initial_phase, default_phase, expected_phase",
+    [pytest.param(ctrl.tf([1], [1, 0]),
+                  None, -math.pi/2, -math.pi/2,         id="order1, default"),
+     pytest.param(ctrl.tf([1], [1, 0]),
+                  180, -math.pi/2, 3*math.pi/2,         id="order1, 180"),
+     pytest.param(ctrl.tf([1], [1, 0, 0]),
+                  None, -math.pi, -math.pi,             id="order2, default"),
+     pytest.param(ctrl.tf([1], [1, 0, 0]),
+                  180, -math.pi, math.pi,               id="order2, 180"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
+                  None, -3*math.pi/2, -3*math.pi/2,     id="order2, default"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
+                  180, -3*math.pi/2, math.pi/2,         id="order2, 180"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]),
+                  None, 0, 0,                           id="order4, default"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]),
+                  180, 0, 0,                            id="order4, 180"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0]),
+                  -360, 0, -2*math.pi,                  id="order4, -360"),
+     ])
+def test_initial_phase(TF, initial_phase, default_phase, expected_phase):
+    # Check initial phase of standard transfer functions
+    mag, phase, omega = ctrl.bode(TF)
+    assert(abs(phase[0] - default_phase) < 0.1)
+
+    # Now reset the initial phase to +180 and see if things work
+    mag, phase, omega = ctrl.bode(TF, initial_phase=initial_phase)
+    assert(abs(phase[0] - expected_phase) < 0.1)
+
+    # Make sure everything works in rad/sec as well
+    # Turn off plotting since that seems to slow things down a lot (?)
+    if initial_phase:
+        mag, phase, omega = ctrl.bode(
+            TF, initial_phase=initial_phase/180. * math.pi, deg=False,
+            plot=False)
+        assert(abs(phase[0] - expected_phase) < 0.1)
+
+
+@pytest.mark.parametrize(
+    "TF, wrap_phase, min_phase, max_phase",
+    [pytest.param(ctrl.tf([1], [1, 0]),
+                  None, -math.pi/2, 0,              id="order1, default"),
+     pytest.param(ctrl.tf([1], [1, 0]),
+                  True, -math.pi, math.pi,          id="order1, True"),
+     pytest.param(ctrl.tf([1], [1, 0]),
+                  -270, -3*math.pi/2, math.pi/2,    id="order1, -270"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
+                  None, -3*math.pi/2, 0,            id="order3, default"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
+                  True, -math.pi, math.pi,          id="order3, True"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0]),
+                  -270, -3*math.pi/2, math.pi/2,    id="order3, -270"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]),
+                  True, -3*math.pi/2, 0,            id="order5, default"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]),
+                  True, -math.pi, math.pi,          id="order5, True"),
+     pytest.param(ctrl.tf([1], [1, 0, 0, 0, 0, 0]),
+                  -270, -3*math.pi/2, math.pi/2,    id="order5, -270"),
+    ])
+def test_phase_wrap(TF, wrap_phase, min_phase, max_phase):
+    mag, phase, omega = ctrl.bode(TF, wrap_phase=wrap_phase)
+    assert(min(phase) >= min_phase)
+    assert(max(phase) <= max_phase)
