Skip to content

Commit 260eb0f

Browse files
murrayrmmurrayrm
authored
Merge pull request #1241 from gaoflow/fix-matched-c2d-origin-pole-zero
Fix matched c2d/sample returning NaN for a pole or zero at the origin
2 parents 8808fe9 + f80d990 commit 260eb0f

2 files changed

Lines changed: 63 additions & 13 deletions

File tree

‎control/tests/discrete_test.py‎

Lines changed: 33 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -561,3 +561,36 @@ def test_c2d_matched(num, den, dt, method):
561561
for czero in sys_ct.zeros():
562562
zzero = zzeros[(np.abs(zzeros - cmath.exp(czero * dt))).argmin()]
563563
assert cmath.isclose(cmath.exp(czero * dt), zzero)
564+
565+
566+
@pytest.mark.parametrize("num, den", [
567+
([1.], [1., 0.]), # integrator (pole at s = 0)
568+
([1.], [1., 0., 0.]), # double integrator
569+
([2., 5.], [1., 0.]), # PI controller
570+
([1.], [1., 1., 0.]), # type 1 plant, 1/(s(s+1))
571+
([1., 0.], [1., 2., 5.]), # differentiator (zero at s = 0)
572+
([2., 0., 0.], [1., 3., 3., 1.]), # double zero at s = 0
573+
([1., 0.], [1., 0., 0.]), # zero and pole at s = 0
574+
])
575+
@pytest.mark.parametrize("dt", [0.1, 0.5])
576+
def test_c2d_matched_origin(num, den, dt):
577+
# A pole or zero at s = 0 (integrators, PI/PID, type-1/2 plants) used to
578+
# give an all-NaN numerator: the DC-gain match divides by the vanishing
579+
# 1 - z factor of the origin pole/zero (#950, #951).
580+
sys_ct = ct.tf(num, den)
581+
sys_dt = ct.sample_system(sys_ct, dt, method='matched')
582+
assert np.all(np.isfinite(sys_dt.num[0][0]))
583+
assert np.all(np.isfinite(sys_dt.den[0][0]))
584+
# the gain is matched just off the origin, so |G_d(e^jwT)| -> |G_c(jw)|
585+
w = 1e-3 / dt
586+
assert np.isclose(abs(sys_ct(1j * w)),
587+
abs(sys_dt(cmath.exp(1j * w * dt))), rtol=1e-4)
588+
589+
590+
@pytest.mark.parametrize("dt", [0.1, 0.5, 2])
591+
@pytest.mark.parametrize("k", [1, 2, 3])
592+
def test_c2d_matched_integrator(k, dt):
593+
# matched discretization of 1/s**k is the textbook Ts**k / (z - 1)**k
594+
sys_dt = ct.tf([1.], [1.] + [0.] * k).sample(dt, method='matched')
595+
np.testing.assert_allclose(sys_dt.num[0][0], [dt**k])
596+
np.testing.assert_allclose(sys_dt.den[0][0], np.poly([1.] * k))

‎control/xferfcn.py‎

Lines changed: 30 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1334,23 +1334,40 @@ def _c2d_matched(sysC, Ts, **kwargs):
13341334
raise ControlMIMONotImplemented("Not implemented for MIMO systems")
13351335

13361336
# Pole-zero match method of continuous to discrete time conversion
1337-
szeros, spoles, _ = tf2zpk(sysC.num[0][0], sysC.den[0][0])
1337+
szeros, spoles, sgain = tf2zpk(sysC.num[0][0], sysC.den[0][0])
13381338
zzeros = [0] * len(szeros)
13391339
zpoles = [0] * len(spoles)
1340-
pregainnum = [0] * len(szeros)
1341-
pregainden = [0] * len(spoles)
1340+
# The gain is matched at the origin (z = 1). A pole or zero at s = 0 maps
1341+
# to z = 1, so its 1 - z factor vanishes and matching the DC gain there is
1342+
# 0/0 or inf/inf -> a NaN numerator for integrators, PI/PID and other
1343+
# type-1/type-2 systems. Keep the origin factors out of the gain product
1344+
# and restore their scaling through the z - 1 ~ s*Ts limit, which recovers
1345+
# Ts/(z - 1) for 1/s, Ts**2/(z - 1)**2 for 1/s**2, etc. (completes #951).
1346+
origin_zeros = origin_poles = 0
1347+
numgain, dengain = sgain, 1.0
1348+
pregainnum = pregainden = 1.0
13421349
for idx, s in enumerate(szeros):
1343-
sTs = s * Ts
1344-
z = exp(sTs)
1345-
zzeros[idx] = z
1346-
pregainnum[idx] = 1 - z
1350+
zzeros[idx] = exp(s * Ts)
1351+
if s == 0:
1352+
origin_zeros += 1
1353+
else:
1354+
numgain *= -s
1355+
pregainnum *= 1 - zzeros[idx]
13471356
for idx, s in enumerate(spoles):
1348-
sTs = s * Ts
1349-
z = exp(sTs)
1350-
zpoles[idx] = z
1351-
pregainden[idx] = 1 - z
1352-
zgain = np.multiply.reduce(pregainnum) / np.multiply.reduce(pregainden)
1353-
gain = sysC.dcgain() / zgain.real
1357+
zpoles[idx] = exp(s * Ts)
1358+
if s == 0:
1359+
origin_poles += 1
1360+
else:
1361+
dengain *= -s
1362+
pregainden *= 1 - zpoles[idx]
1363+
zgain = pregainnum / pregainden
1364+
if origin_zeros or origin_poles:
1365+
# DC gain of the system with the origin factors divided out, rescaled
1366+
# by Ts**(origin poles - origin zeros) from the z - 1 ~ s*Ts limit
1367+
gain = (numgain / dengain).real \
1368+
* Ts**(origin_poles - origin_zeros) / zgain.real
1369+
else:
1370+
gain = sysC.dcgain() / zgain.real
13541371
sysDnum, sysDden = zpk2tf(zzeros, zpoles, gain)
13551372
return TransferFunction(sysDnum, sysDden, Ts, **kwargs)
13561373

0 commit comments

Comments
 (0)