diff --git a/control/tests/discrete_test.py b/control/tests/discrete_test.py index 7296c0f31..0bd3fda7c 100644 --- a/control/tests/discrete_test.py +++ b/control/tests/discrete_test.py @@ -561,3 +561,36 @@ def test_c2d_matched(num, den, dt, method): for czero in sys_ct.zeros(): zzero = zzeros[(np.abs(zzeros - cmath.exp(czero * dt))).argmin()] assert cmath.isclose(cmath.exp(czero * dt), zzero) + + +@pytest.mark.parametrize("num, den", [ + ([1.], [1., 0.]), # integrator (pole at s = 0) + ([1.], [1., 0., 0.]), # double integrator + ([2., 5.], [1., 0.]), # PI controller + ([1.], [1., 1., 0.]), # type 1 plant, 1/(s(s+1)) + ([1., 0.], [1., 2., 5.]), # differentiator (zero at s = 0) + ([2., 0., 0.], [1., 3., 3., 1.]), # double zero at s = 0 + ([1., 0.], [1., 0., 0.]), # zero and pole at s = 0 +]) +@pytest.mark.parametrize("dt", [0.1, 0.5]) +def test_c2d_matched_origin(num, den, dt): + # A pole or zero at s = 0 (integrators, PI/PID, type-1/2 plants) used to + # give an all-NaN numerator: the DC-gain match divides by the vanishing + # 1 - z factor of the origin pole/zero (#950, #951). + sys_ct = ct.tf(num, den) + sys_dt = ct.sample_system(sys_ct, dt, method='matched') + assert np.all(np.isfinite(sys_dt.num[0][0])) + assert np.all(np.isfinite(sys_dt.den[0][0])) + # the gain is matched just off the origin, so |G_d(e^jwT)| -> |G_c(jw)| + w = 1e-3 / dt + assert np.isclose(abs(sys_ct(1j * w)), + abs(sys_dt(cmath.exp(1j * w * dt))), rtol=1e-4) + + +@pytest.mark.parametrize("dt", [0.1, 0.5, 2]) +@pytest.mark.parametrize("k", [1, 2, 3]) +def test_c2d_matched_integrator(k, dt): + # matched discretization of 1/s**k is the textbook Ts**k / (z - 1)**k + sys_dt = ct.tf([1.], [1.] + [0.] * k).sample(dt, method='matched') + np.testing.assert_allclose(sys_dt.num[0][0], [dt**k]) + np.testing.assert_allclose(sys_dt.den[0][0], np.poly([1.] * k)) diff --git a/control/xferfcn.py b/control/xferfcn.py index 8e51534d7..ad9ea0554 100644 --- a/control/xferfcn.py +++ b/control/xferfcn.py @@ -1334,23 +1334,40 @@ def _c2d_matched(sysC, Ts, **kwargs): raise ControlMIMONotImplemented("Not implemented for MIMO systems") # Pole-zero match method of continuous to discrete time conversion - szeros, spoles, _ = tf2zpk(sysC.num[0][0], sysC.den[0][0]) + szeros, spoles, sgain = tf2zpk(sysC.num[0][0], sysC.den[0][0]) zzeros = [0] * len(szeros) zpoles = [0] * len(spoles) - pregainnum = [0] * len(szeros) - pregainden = [0] * len(spoles) + # The gain is matched at the origin (z = 1). A pole or zero at s = 0 maps + # to z = 1, so its 1 - z factor vanishes and matching the DC gain there is + # 0/0 or inf/inf -> a NaN numerator for integrators, PI/PID and other + # type-1/type-2 systems. Keep the origin factors out of the gain product + # and restore their scaling through the z - 1 ~ s*Ts limit, which recovers + # Ts/(z - 1) for 1/s, Ts**2/(z - 1)**2 for 1/s**2, etc. (completes #951). + origin_zeros = origin_poles = 0 + numgain, dengain = sgain, 1.0 + pregainnum = pregainden = 1.0 for idx, s in enumerate(szeros): - sTs = s * Ts - z = exp(sTs) - zzeros[idx] = z - pregainnum[idx] = 1 - z + zzeros[idx] = exp(s * Ts) + if s == 0: + origin_zeros += 1 + else: + numgain *= -s + pregainnum *= 1 - zzeros[idx] for idx, s in enumerate(spoles): - sTs = s * Ts - z = exp(sTs) - zpoles[idx] = z - pregainden[idx] = 1 - z - zgain = np.multiply.reduce(pregainnum) / np.multiply.reduce(pregainden) - gain = sysC.dcgain() / zgain.real + zpoles[idx] = exp(s * Ts) + if s == 0: + origin_poles += 1 + else: + dengain *= -s + pregainden *= 1 - zpoles[idx] + zgain = pregainnum / pregainden + if origin_zeros or origin_poles: + # DC gain of the system with the origin factors divided out, rescaled + # by Ts**(origin poles - origin zeros) from the z - 1 ~ s*Ts limit + gain = (numgain / dengain).real \ + * Ts**(origin_poles - origin_zeros) / zgain.real + else: + gain = sysC.dcgain() / zgain.real sysDnum, sysDden = zpk2tf(zzeros, zpoles, gain) return TransferFunction(sysDnum, sysDden, Ts, **kwargs)