fix #523: finding z for |H(z)|=1 computed the wrong polynomials

bnavigator · bnavigator · commit 96cc1245b922 · 2021-01-30T01:46:01.000+01:00
diff --git a/control/margins.py b/control/margins.py
@@ -156,13 +156,14 @@ def _poly_z_real_crossing(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw):
     return z, w
 
 
-def _poly_z_mag1_crossing(num, den, num_inv, den_inv, p_q, dt, epsw):
+def _poly_z_mag1_crossing(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw):
     # |H(z)| = 1, H(z)*H(1/z)=1, num(z)*num(1/z) == den(z)*den(1/z)
-    p1 = np.polymul(num, num_inv)
-    p2 = np.polymul(den, den_inv)
+    p1 = np.polymul(num, num_inv_zp)
+    p2 = np.polymul(den, den_inv_zq)
     if p_q < 0:
+        # * z**(-p_q)
         x = [1] + [0] * (-p_q)
-        p2 = np.polymul(p2, x)
+        p1 = np.polymul(p1, x)
     z = np.roots(np.polysub(p1, p2))
     eps = np.finfo(float).eps**(1 / len(p2))
     z, w = _z_filter(z, dt, eps)
@@ -171,7 +172,7 @@ def _poly_z_mag1_crossing(num, den, num_inv, den_inv, p_q, dt, epsw):
     return z, w
 
 
-def _poly_z_wstab(num, den, num_inv, den_inv, p_q, dt, epsw):
+def _poly_z_wstab(num, den, num_inv_zp, den_inv_zq, p_q, dt, epsw):
     # Stability margin: Minimum distance to -1
 
     # TODO: Find a way to solve for z or omega analytically with given
diff --git a/control/tests/margin_test.py b/control/tests/margin_test.py
@@ -336,17 +336,19 @@ def test_zmore_stability_margins(tsys_zmore):
 
 @pytest.mark.parametrize(
     'cnum, cden, dt,'
-    'ref,'
-    'rtol',
-    [([2], [1, 3, 2, 0], 1e-2,  # gh-465
-      (2.9558, 32.8170, 0.43584, 1.4037, 0.74953, 0.97079),
-      0.1  # very crude tolerance, because the gradients are not great
-      ),
-     ([2], [1, 3, 3, 1], .1,  # 2/(s+1)**3
-      [3.4927, 69.9996, 0.5763, 1.6283, 0.7631, 1.2019],
-      1e-3)])
-def test_stability_margins_discrete(cnum, cden, dt, ref, rtol):
+    'ref,',
+    [( # gh-465
+      [2], [1, 3, 2, 0], 1e-2,  
+      (2.9558, 32.390, 0.43584, 1.4037, 0.74951, 0.97079)),
+     ( # 2/(s+1)**3
+      [2], [1, 3, 3, 1], .1,  
+      [3.4927, 65.4212, 0.5763, 1.6283, 0.76625, 1.2019]),
+     ( # gh-523
+      [1.1 * 4 * np.pi**2], [1, 2 * 0.2 * 2 * np.pi,  4 * np.pi**2], .05,
+      [2.3842, 18.161, 0.26953, 11.712, 8.7478, 9.1504]),
+     ])
+def test_stability_margins_discrete(cnum, cden, dt, ref):
     """Test stability_margins with discrete TF input"""
     tf = TransferFunction(cnum, cden).sample(dt)
     out = stability_margins(tf)
-    assert_allclose(out, ref, rtol=rtol)
+    assert_allclose(out, ref, rtol=1e-4)
