Skip to content

Commit 4f168cd

Browse files
RoboX2020RoboX2020
committed
Fix stability_margins missing a phase crossover at the Nyquist frequency
For a discrete-time loop whose phase reaches -180 degrees exactly at w = pi/dt, stability_margins returned gm = inf under every method. For real coefficients L(-1) is always real, so this is a common case: L(z) = 1/(z-1) has a phase crossover at z = -1 with gm = 2, but 'poly', 'frd' and 'best' all reported gm = inf. - _z_filter kept only unit-circle roots with 0 <= arg(z) < pi, dropping the root at z = -1. Now also keep roots whose argument is exactly +pi or exactly -pi (the root-finder returns an exact imaginary zero for a real root, with either sign) and report the endpoint frequency via abs(arg). No tolerance is applied to the angle, so a genuine root near, but not at, -1 keeps its computed frequency and its lower-half conjugate stays excluded, exactly as before. - The 'frd' path detects phase crossovers as sign changes inside the frequency grid, which excludes w = pi/dt for discrete-time systems. When a transfer function is converted to FRD, evaluate L(-1) and count it as a phase crossover if it is real and negative and pi/dt >= epsw. (Other FRD-detected crossings ignore epsw; that pre-existing behavior is unchanged.) This also makes phase_crossover_frequencies include the real-axis intersection at w = pi/dt for discrete-time systems, matching the w = 0 endpoint that was already returned; one existing test reference is updated accordingly. Adds regression tests: gm at the Nyquist endpoint under all three methods, a loop with L(-1) > 0 that must keep gm = inf, and a _z_filter unit test covering the exact endpoint and a nearby conjugate root pair. The new tests fail at baseline and pass with the fix. Fixes #1267
1 parent bfe9380 commit 4f168cd

2 files changed

Lines changed: 78 additions & 3 deletions

File tree

‎control/margins.py‎

Lines changed: 24 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -102,8 +102,12 @@ def _z_filter(z, dt, eps):
102102
# |z| == 1 with some float precision tolerance
103103
z = z[np.abs(np.abs(z) - 1.) < eps]
104104
zarg = np.angle(z)
105-
zidx = (0 <= zarg) * (zarg < np.pi)
106-
omega = zarg[zidx] / dt
105+
# also keep the Nyquist endpoint z = -1: its argument is exactly pi,
106+
# or exactly -pi when the root carries a negative imaginary zero.
107+
# No tolerance is applied to the angle: a genuine root near, but not
108+
# at, -1 must keep its computed frequency. abs() maps -pi to +pi.
109+
zidx = ((0 <= zarg) * (zarg < np.pi)) | (np.abs(zarg) == np.pi)
110+
omega = np.abs(zarg[zidx]) / dt
107111
return z[zidx], omega
108112

109113

@@ -292,6 +296,12 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'):
292296
raise ControlMIMONotImplemented(
293297
"Can only do margins for SISO system")
294298

299+
# response at the Nyquist frequency w = pi/dt, if it can be evaluated
300+
# exactly: the FRD frequency grid excludes this endpoint, so a phase
301+
# crossover there would otherwise be missed
302+
nyquist_dt = None
303+
nyquist_resp = None
304+
295305
if method == 'frd':
296306
# convert to FRD if we got a transfer function
297307
if isinstance(sys, xferfcn.TransferFunction):
@@ -300,6 +310,8 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'):
300310
sys = frdata.FRD(sys, omega_sys)
301311
else:
302312
omega_sys = omega_sys[omega_sys < np.pi / sys.dt]
313+
nyquist_resp = sys(-1) # L(exp(j pi))
314+
nyquist_dt = sys.dt
303315
sys = frdata.FRD(sys, omega_sys, smooth=True)
304316
elif method == 'best':
305317
# convert to FRD if anticipated numerical issues
@@ -310,6 +322,8 @@ def stability_margins(sysdata, returnall=False, epsw=0.0, method='best'):
310322
stacklevel=2)
311323
omega_sys = freqplot._default_frequency_range(sys)
312324
omega_sys = omega_sys[omega_sys < np.pi / sys.dt]
325+
nyquist_resp = sys(-1) # L(exp(j pi))
326+
nyquist_dt = sys.dt
313327
sys = frdata.FRD(sys, omega_sys, smooth=True)
314328
elif method != 'poly':
315329
raise ValueError("method " + method + " unknown")
@@ -382,6 +396,14 @@ def _dstab(w):
382396
for i in widx])
383397
w180_resp = sys(1j * w_180)
384398

399+
# the phase can also cross -180 degrees exactly at the Nyquist
400+
# frequency w = pi/dt, which the frequency grid does not include
401+
if nyquist_dt is not None and np.pi / nyquist_dt >= epsw:
402+
resp = np.asarray(nyquist_resp, dtype=complex)
403+
if np.isclose(resp.imag, 0) and resp.real < 0:
404+
w_180 = np.append(w_180, np.pi / nyquist_dt)
405+
w180_resp = np.append(w180_resp, resp.real)
406+
385407
# Find all crossings, note that this depends on omega having
386408
# a correct range
387409
widx = np.where(np.diff(np.sign(_mod(sys.omega))))[0]

‎control/tests/margin_test.py‎

Lines changed: 54 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,8 @@
77
BG, 16 Nov 2020 -- pick from gh-438 and add discrete test
88
"""
99

10+
import warnings
11+
1012
import numpy as np
1113
import pytest
1214
from numpy import inf, nan
@@ -114,7 +116,7 @@ def test_margin_3input(tsys):
114116
(([1], [1, 1]), [0.], [1.]),
115117
(([2], [1, 3, 3, 1]), [1.732, 0.], [-0.25, 2.]),
116118
((np.array([3, 11, 3]) * 1e-4, [1., -2.7145, 2.4562, -0.7408], .1),
117-
[1.6235, 0.], [-0.28598, 1.88889]),
119+
[10*np.pi, 1.6235, 0.], [7.23432e-05, -0.28598, 1.88889]),
118120
(([200.0], [1.0, 21.0, 20.0, 0.0]),
119121
[4.47213595, 0], [-0.47619048, inf]),
120122
])
@@ -149,6 +151,57 @@ def test_mag_phase_omega():
149151
marg2 = np.array(out2)[ind]
150152
assert_allclose(marg1, marg2, atol=1.5e-3)
151153

154+
@pytest.mark.parametrize("method", ["poly", "frd", "best"])
155+
@pytest.mark.parametrize(
156+
"num, den, gm_ref",
157+
[([1.], [1., -1.], 2.),
158+
([1.5], [1., -0.5], 1.),
159+
([0.4], [1., 0.2], 2.)])
160+
def test_stability_margins_discrete_nyquist(num, den, gm_ref, method):
161+
"""Phase crossover exactly at the Nyquist frequency w = pi/dt.
162+
163+
For real coefficients L(-1) is always real, so a loop with L(-1) < 0
164+
has a phase crossover at w = pi/dt with gain margin -1/L(-1).
165+
"""
166+
L = TransferFunction(num, den, 1.)
167+
with warnings.catch_warnings():
168+
warnings.simplefilter("ignore")
169+
gm, pm, sm, wpc, wgc, wms = stability_margins(L, method=method)
170+
assert_allclose(gm, gm_ref, rtol=1e-6)
171+
assert_allclose(wpc, np.pi / L.dt, rtol=1e-6)
172+
173+
174+
def test_z_filter_nyquist_endpoint():
175+
"""_z_filter keeps z = -1 exactly, without snapping nearby roots.
176+
177+
A genuine root just below pi keeps its computed frequency, its
178+
lower-half conjugate stays excluded, and -1 is kept whether its
179+
imaginary zero is positive or negative (arg pi or -pi).
180+
"""
181+
from control.margins import _z_filter
182+
# note: the -1 literals must be built with complex() - the expression
183+
# -1 - 0j has a *positive* imaginary zero, so it does not cover arg -pi
184+
zend = [complex(-1., -0.), complex(-1., 0.)]
185+
assert np.angle(zend[0]) == -np.pi
186+
assert np.angle(zend[1]) == np.pi
187+
z = np.array([np.exp(1j * (np.pi - 1e-6)),
188+
np.exp(-1j * (np.pi - 1e-6))] + zend)
189+
zf, omega = _z_filter(z, dt=2.0, eps=0.1)
190+
assert_allclose(np.sort(omega),
191+
np.sort([np.pi / 2, np.pi / 2, (np.pi - 1e-6) / 2]))
192+
193+
194+
@pytest.mark.parametrize("method", ["poly", "frd", "best"])
195+
def test_stability_margins_discrete_nyquist_positive(method):
196+
"""L(-1) > 0: no phase crossover at the Nyquist frequency."""
197+
L = TransferFunction([0.4], [1., 2.], 1.)
198+
assert L(-1).real > 0
199+
with warnings.catch_warnings():
200+
warnings.simplefilter("ignore")
201+
gm = stability_margins(L, method=method)[0]
202+
assert gm == inf
203+
204+
152205

153206
def test_frd():
154207
"""Test FrequencyResonseData margins"""

0 commit comments

Comments
 (0)