Skip to content

Commit 7f66cc9

Browse files
committed
Validate discrete gain crossovers on the unit circle
1 parent bfe9380 commit 7f66cc9

2 files changed

Lines changed: 72 additions & 3 deletions

File tree

‎control/margins.py‎

Lines changed: 34 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -134,9 +134,40 @@ def _poly_z_mag1_crossing(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw):
134134
z = np.roots(np.polysub(p1, p2))
135135
eps = np.finfo(float).eps**(1 / len(p2))
136136
z, w = _z_filter(z, dt, eps)
137-
z = z[w > epsw]
138-
w = w[w > epsw]
139-
return z, w
137+
above_cutoff = w > epsw
138+
z = z[above_cutoff]
139+
theta = w[above_cutoff] * dt
140+
141+
# The radial tolerance grows with degree. Verify the gain at the
142+
# candidate frequency on the unit circle before reporting a crossover.
143+
def magnitudes(angle):
144+
point = np.exp(1j * angle)
145+
return np.abs(np.polyval(num, point)), np.abs(np.polyval(den, point))
146+
147+
def magnitude_difference(angle):
148+
num_mag, den_mag = magnitudes(angle)
149+
return num_mag - den_mag
150+
151+
tol = np.sqrt(np.finfo(float).eps)
152+
num_mag, den_mag = magnitudes(theta)
153+
154+
# Refine inaccurate near-circle roots locally using the frequency
155+
# response. The search radius is in radians, independent of degree.
156+
radius = np.sqrt(tol)
157+
near_circle = np.abs(np.abs(z) - 1) < radius
158+
inaccurate = ~np.isclose(num_mag, den_mag, atol=0, rtol=tol)
159+
for i in np.flatnonzero(near_circle & inaccurate):
160+
lo = max(0., theta[i] - radius)
161+
hi = min(np.pi, theta[i] + radius)
162+
ends = magnitude_difference(np.array([lo, hi]))
163+
if np.all(np.isfinite(ends)) and np.sign(ends[0]) != np.sign(ends[1]):
164+
theta[i] = sp.optimize.brentq(magnitude_difference, lo, hi)
165+
num_mag[i], den_mag[i] = magnitudes(theta[i])
166+
167+
w = theta / dt
168+
valid = (den_mag > 0) & np.isfinite(den_mag) & (w > epsw) & (theta < np.pi)
169+
valid &= np.isclose(num_mag, den_mag, atol=0, rtol=tol)
170+
return np.exp(1j * theta[valid]), w[valid]
140171

141172

142173
def _poly_z_wstab(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw):

‎control/tests/margin_test.py‎

Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -373,6 +373,44 @@ def test_stability_margins_discrete(cnum, cden, dt,
373373
out = stability_margins(tf)
374374
assert_allclose(out, ref, rtol=rtol)
375375

376+
377+
@pytest.mark.parametrize("method", ["poly", "best"])
378+
def test_discrete_gain_crossovers_on_unit_circle(method):
379+
n = 17
380+
num = np.full(n, 1 / n)
381+
loop = tf(num, np.r_[1, -num], dt=1)
382+
383+
_, pm, _, _, wgc, _ = stability_margins(loop, method=method, returnall=True)
384+
385+
assert_allclose(wgc, [0.1064535], rtol=1e-6)
386+
assert_allclose(pm, [70.2119], rtol=1e-6)
387+
assert_allclose(np.abs(loop(np.exp(1j * wgc))), 1)
388+
389+
390+
@pytest.mark.parametrize("method", ["poly", "best"])
391+
def test_discrete_gain_below_unity(method):
392+
# |z**8 - 0.5| >= 0.5 on the unit circle, so |L(z)| <= 0.5.
393+
loop = tf([0.25], np.r_[1, np.zeros(7), -0.5], dt=1)
394+
395+
_, pm, _, _, wgc, _ = stability_margins(loop, method=method, returnall=True)
396+
397+
assert len(wgc) == len(pm) == 0
398+
399+
400+
def test_discrete_gain_crossover_refinement():
401+
# For L(z) = k * ((z + 1) / (z - a))**n, solve |L(z)| = 1
402+
# directly in cos(theta) to check the numerically sensitive root.
403+
n, a, k, dt = 10, 0.2, 1000, 0.2
404+
loop = tf(k * np.poly([-1.] * n), np.poly([a] * n), dt=dt)
405+
power = k**(2 / n)
406+
theta = np.arccos((1 + a * a - 2 * power) / (2 * (power + a)))
407+
408+
_, _, _, _, wgc, _ = stability_margins(loop, method="poly", returnall=True)
409+
410+
assert_allclose(wgc * dt, [theta], atol=1e-8, rtol=0)
411+
assert_allclose(np.abs(loop(np.exp(1j * wgc * dt))), 1, rtol=1e-8)
412+
413+
376414
def test_siso_disk_margin():
377415
# Frequencies of interest
378416
omega = np.logspace(-1, 2, 1001)

0 commit comments

Comments
 (0)