diff --git a/control/margins.py b/control/margins.py index 57e825c65..1a8700989 100644 --- a/control/margins.py +++ b/control/margins.py @@ -292,6 +292,12 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'): raise ControlMIMONotImplemented( "Can only do margins for SISO system") + # response at the Nyquist frequency w = pi/dt, if it can be evaluated + # exactly: the FRD frequency grid excludes this endpoint, so a phase + # crossover there would otherwise be missed + nyquist_dt = None + nyquist_resp = None + if method == 'frd': # convert to FRD if we got a transfer function if isinstance(sys, xferfcn.TransferFunction): @@ -300,6 +306,8 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'): sys = frdata.FRD(sys, omega_sys) else: omega_sys = omega_sys[omega_sys < np.pi / sys.dt] + nyquist_resp = sys(-1) # L(exp(j pi)) + nyquist_dt = sys.dt sys = frdata.FRD(sys, omega_sys, smooth=True) elif method == 'best': # convert to FRD if anticipated numerical issues @@ -310,6 +318,8 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'): stacklevel=2) omega_sys = freqplot._default_frequency_range(sys) omega_sys = omega_sys[omega_sys < np.pi / sys.dt] + nyquist_resp = sys(-1) # L(exp(j pi)) + nyquist_dt = sys.dt sys = frdata.FRD(sys, omega_sys, smooth=True) elif method != 'poly': raise ValueError("method " + method + " unknown") @@ -335,6 +345,18 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'): z, w_180 = _poly_z_real_crossing(*zargs, epsw=epsw) w180_resp = sys(z) + # the phase can also cross -180 degrees exactly at the Nyquist + # frequency w = pi/dt: _z_filter excludes arg = pi, so the + # polynomial search never returns z = -1. Check it directly + # instead of widening the filter, which is shared with the + # gain-crossover search and must not admit off-circle roots + # whose argument happens to be pi. + if np.pi / sys.dt >= epsw: + resp = np.asarray(sys(-1), dtype=complex) + if np.isclose(resp.imag, 0) and resp.real < 0: + w_180 = np.append(w_180, np.pi / sys.dt) + w180_resp = np.append(w180_resp, resp.real) + # phase margin z, wc = _poly_z_mag1_crossing(*zargs, epsw=epsw) wc_resp = sys(z) @@ -382,6 +404,14 @@ def _dstab(w): for i in widx]) w180_resp = sys(1j * w_180) + # the phase can also cross -180 degrees exactly at the Nyquist + # frequency w = pi/dt, which the frequency grid does not include + if nyquist_dt is not None and np.pi / nyquist_dt >= epsw: + resp = np.asarray(nyquist_resp, dtype=complex) + if np.isclose(resp.imag, 0) and resp.real < 0: + w_180 = np.append(w_180, np.pi / nyquist_dt) + w180_resp = np.append(w180_resp, resp.real) + # Find all crossings, note that this depends on omega having # a correct range widx = np.where(np.diff(np.sign(_mod(sys.omega))))[0] @@ -467,6 +497,12 @@ def phase_crossover_frequencies(sys): z, omega = _poly_z_real_crossing(*zargs, epsw=0.) gains = np.real(sys(z, warn_infinite=False)) + # the Nyquist plot also intersects the real axis at w = pi/dt + # (the response at z = -1 is real for real coefficients); the + # polynomial search never returns z = -1, so add it directly + omega = np.append(omega, np.pi / tf.dt) + gains = np.append(gains, np.real(tf(-1))) + return omega, gains diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index c8be4ee6c..5b8cdbb28 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -7,6 +7,8 @@ BG, 16 Nov 2020 -- pick from gh-438 and add discrete test """ +import warnings + import numpy as np import pytest from numpy import inf, nan @@ -114,7 +116,7 @@ def test_margin_3input(tsys): (([1], [1, 1]), [0.], [1.]), (([2], [1, 3, 3, 1]), [1.732, 0.], [-0.25, 2.]), ((np.array([3, 11, 3]) * 1e-4, [1., -2.7145, 2.4562, -0.7408], .1), - [1.6235, 0.], [-0.28598, 1.88889]), + [1.6235, 0., 10*np.pi], [-0.28598, 1.88889, 7.23432e-05]), (([200.0], [1.0, 21.0, 20.0, 0.0]), [4.47213595, 0], [-0.47619048, inf]), ]) @@ -149,6 +151,57 @@ def test_mag_phase_omega(): marg2 = np.array(out2)[ind] assert_allclose(marg1, marg2, atol=1.5e-3) +@pytest.mark.parametrize("method", ["poly", "frd", "best"]) +@pytest.mark.parametrize( + "num, den, gm_ref", + [([1.], [1., -1.], 2.), + ([1.5], [1., -0.5], 1.), + ([0.4], [1., 0.2], 2.)]) +def test_stability_margins_discrete_nyquist(num, den, gm_ref, method): + """Phase crossover exactly at the Nyquist frequency w = pi/dt. + + For real coefficients L(-1) is always real, so a loop with L(-1) < 0 + has a phase crossover at w = pi/dt with gain margin -1/L(-1). + """ + L = TransferFunction(num, den, 1.) + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + gm, pm, sm, wpc, wgc, wms = stability_margins(L, method=method) + assert_allclose(gm, gm_ref, rtol=1e-6) + assert_allclose(wpc, np.pi / L.dt, rtol=1e-6) + + +@pytest.mark.parametrize("method", ["poly", "frd", "best"]) +def test_stability_margins_discrete_nyquist_positive(method): + """L(-1) > 0: no phase crossover at the Nyquist frequency.""" + L = TransferFunction([0.4], [1., 2.], 1.) + assert L(-1).real > 0 + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + gm = stability_margins(L, method=method)[0] + assert gm == inf + + +def test_stability_margins_moving_average_phase(): + """No gain crossover at w = pi/dt when |L(-1)| = 0. + + Regression test for the review on gh-1268: for an even-length moving + average the |L| = 1 polynomial has off-circle roots with argument + exactly pi, which must not be accepted as a gain crossover at the + Nyquist frequency (that reported pm = 0 for a loop whose true phase + margin is about 70 degrees). + """ + N = 16 + num = np.r_[0.0, np.full(N, 1.0 / N)] + L = TransferFunction(num, np.polysub(np.r_[1.0, np.zeros(N)], num), 1.) + assert abs(complex(L(-1))) < 1e-12 + with warnings.catch_warnings(): + warnings.simplefilter("ignore") + gm, pm, sm, wpc, wgc, wms = stability_margins(L, method="poly") + assert wpc != pytest.approx(np.pi) or gm == inf + assert pm > 0 and wgc != pytest.approx(np.pi) + + def test_frd(): """Test FrequencyResonseData margins"""