From 7f66cc923602adc5d15faf44210ac4678bdd95fa Mon Sep 17 00:00:00 2001 From: galleonli Date: Tue, 10 Nov 2026 13:52:39 -0500 Subject: [PATCH] Validate discrete gain crossovers on the unit circle --- control/margins.py | 37 ++++++++++++++++++++++++++++++++--- control/tests/margin_test.py | 38 ++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+), 3 deletions(-) diff --git a/control/margins.py b/control/margins.py index 57e825c65..25a591a90 100644 --- a/control/margins.py +++ b/control/margins.py @@ -134,9 +134,40 @@ def _poly_z_mag1_crossing(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw): z = np.roots(np.polysub(p1, p2)) eps = np.finfo(float).eps**(1 / len(p2)) z, w = _z_filter(z, dt, eps) - z = z[w > epsw] - w = w[w > epsw] - return z, w + above_cutoff = w > epsw + z = z[above_cutoff] + theta = w[above_cutoff] * dt + + # The radial tolerance grows with degree. Verify the gain at the + # candidate frequency on the unit circle before reporting a crossover. + def magnitudes(angle): + point = np.exp(1j * angle) + return np.abs(np.polyval(num, point)), np.abs(np.polyval(den, point)) + + def magnitude_difference(angle): + num_mag, den_mag = magnitudes(angle) + return num_mag - den_mag + + tol = np.sqrt(np.finfo(float).eps) + num_mag, den_mag = magnitudes(theta) + + # Refine inaccurate near-circle roots locally using the frequency + # response. The search radius is in radians, independent of degree. + radius = np.sqrt(tol) + near_circle = np.abs(np.abs(z) - 1) < radius + inaccurate = ~np.isclose(num_mag, den_mag, atol=0, rtol=tol) + for i in np.flatnonzero(near_circle & inaccurate): + lo = max(0., theta[i] - radius) + hi = min(np.pi, theta[i] + radius) + ends = magnitude_difference(np.array([lo, hi])) + if np.all(np.isfinite(ends)) and np.sign(ends[0]) != np.sign(ends[1]): + theta[i] = sp.optimize.brentq(magnitude_difference, lo, hi) + num_mag[i], den_mag[i] = magnitudes(theta[i]) + + w = theta / dt + valid = (den_mag > 0) & np.isfinite(den_mag) & (w > epsw) & (theta < np.pi) + valid &= np.isclose(num_mag, den_mag, atol=0, rtol=tol) + return np.exp(1j * theta[valid]), w[valid] def _poly_z_wstab(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw): diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py index c8be4ee6c..3f349559b 100644 --- a/control/tests/margin_test.py +++ b/control/tests/margin_test.py @@ -373,6 +373,44 @@ def test_stability_margins_discrete(cnum, cden, dt, out = stability_margins(tf) assert_allclose(out, ref, rtol=rtol) + +@pytest.mark.parametrize("method", ["poly", "best"]) +def test_discrete_gain_crossovers_on_unit_circle(method): + n = 17 + num = np.full(n, 1 / n) + loop = tf(num, np.r_[1, -num], dt=1) + + _, pm, _, _, wgc, _ = stability_margins(loop, method=method, returnall=True) + + assert_allclose(wgc, [0.1064535], rtol=1e-6) + assert_allclose(pm, [70.2119], rtol=1e-6) + assert_allclose(np.abs(loop(np.exp(1j * wgc))), 1) + + +@pytest.mark.parametrize("method", ["poly", "best"]) +def test_discrete_gain_below_unity(method): + # |z**8 - 0.5| >= 0.5 on the unit circle, so |L(z)| <= 0.5. + loop = tf([0.25], np.r_[1, np.zeros(7), -0.5], dt=1) + + _, pm, _, _, wgc, _ = stability_margins(loop, method=method, returnall=True) + + assert len(wgc) == len(pm) == 0 + + +def test_discrete_gain_crossover_refinement(): + # For L(z) = k * ((z + 1) / (z - a))**n, solve |L(z)| = 1 + # directly in cos(theta) to check the numerically sensitive root. + n, a, k, dt = 10, 0.2, 1000, 0.2 + loop = tf(k * np.poly([-1.] * n), np.poly([a] * n), dt=dt) + power = k**(2 / n) + theta = np.arccos((1 + a * a - 2 * power) / (2 * (power + a))) + + _, _, _, _, wgc, _ = stability_margins(loop, method="poly", returnall=True) + + assert_allclose(wgc * dt, [theta], atol=1e-8, rtol=0) + assert_allclose(np.abs(loop(np.exp(1j * wgc * dt))), 1, rtol=1e-8) + + def test_siso_disk_margin(): # Frequencies of interest omega = np.logspace(-1, 2, 1001)