diff --git a/control/modelsimp.py b/control/modelsimp.py index 06c3d350d..4b72e2bb9 100644 --- a/control/modelsimp.py +++ b/control/modelsimp.py @@ -48,8 +48,9 @@ from .iosys import isdtime, isctime from .statesp import StateSpace from .statefbk import gram +from .timeresp import TimeResponseData -__all__ = ['hsvd', 'balred', 'modred', 'era', 'markov', 'minreal'] +__all__ = ['hsvd', 'balred', 'modred', 'eigensys_realization', 'markov', 'minreal', 'era'] # Hankel Singular Value Decomposition @@ -368,38 +369,129 @@ def minreal(sys, tol=None, verbose=True): return sysr -def era(YY, m, n, nin, nout, r): - """Calculate an ERA model of order `r` based on the impulse-response data +def _block_hankel(Y, m, n): + """Create a block Hankel matrix from impulse response""" + q, p, _ = Y.shape + YY = Y.transpose(0,2,1) # transpose for reshape + + H = np.zeros((q*m,p*n)) + + for r in range(m): + # shift and add row to Hankel matrix + new_row = YY[:,r:r+n,:] + H[q*r:q*(r+1),:] = new_row.reshape((q,p*n)) + + return H + + +def eigensys_realization(arg, r, m=None, n=None, dt=True, transpose=False): + r"""eigensys_realization(YY, r) + + Calculate an ERA model of order `r` based on the impulse-response data `YY`. - .. note:: This function is not implemented yet. + This function computes a discrete time system + + .. math:: + + x[k+1] &= A x[k] + B u[k] \\\\ + y[k] &= C x[k] + D u[k] + + for a given impulse-response data (see [1]_). + + The function can be called with 2 arguments: + + * ``sysd, S = eigensys_realization(data, r)`` + * ``sysd, S = eigensys_realization(YY, r)`` + + where `data` is a `TimeResponseData` object, `YY` is a 1D or 3D + array, and r is an integer. Parameters ---------- - YY: array - `nout` x `nin` dimensional impulse-response data - m: integer - Number of rows in Hankel matrix - n: integer - Number of columns in Hankel matrix - nin: integer - Number of input variables - nout: integer - Number of output variables - r: integer - Order of model + YY : array_like + Impulse response from which the StateSpace model is estimated, 1D + or 3D array. + data : TimeResponseData + Impulse response from which the StateSpace model is estimated. + r : integer + Order of model. + m : integer, optional + Number of rows in Hankel matrix. Default is 2*r. + n : integer, optional + Number of columns in Hankel matrix. Default is 2*r. + dt : True or float, optional + True indicates discrete time with unspecified sampling time and a + positive float is discrete time with the specified sampling time. + It can be used to scale the StateSpace model in order to match the + unit-area impulse response of python-control. Default is True. + transpose : bool, optional + Assume that input data is transposed relative to the standard + :ref:`time-series-convention`. For TimeResponseData this parameter + is ignored. Default is False. Returns ------- - sys: StateSpace - A reduced order model sys=ss(Ar,Br,Cr,Dr) + sys : StateSpace + A reduced order model sys=StateSpace(Ar,Br,Cr,Dr,dt). + S : array + Singular values of Hankel matrix. Can be used to choose a good r + value. + + References + ---------- + .. [1] Samet Oymak and Necmiye Ozay, Non-asymptotic Identification of + LTI Systems from a Single Trajectory. + https://arxiv.org/abs/1806.05722 Examples -------- - >>> rsys = era(YY, m, n, nin, nout, r) # doctest: +SKIP + >>> T = np.linspace(0, 10, 100) + >>> _, YY = ct.impulse_response(ct.tf([1], [1, 0.5], True), T) + >>> sysd, _ = ct.eigensys_realization(YY, r=1) + >>> T = np.linspace(0, 10, 100) + >>> response = ct.impulse_response(ct.tf([1], [1, 0.5], True), T) + >>> sysd, _ = ct.eigensys_realization(response, r=1) """ - raise NotImplementedError('This function is not implemented yet.') + if isinstance(arg, TimeResponseData): + YY = np.array(arg.outputs, ndmin=3) + if arg.transpose: + YY = np.transpose(YY) + else: + YY = np.array(arg, ndmin=3) + if transpose: + YY = np.transpose(YY) + + q, p, l = YY.shape + + if m is None: + m = 2*r + if n is None: + n = 2*r + + if m*q < r or n*p < r: + raise ValueError("Hankel parameters are to small") + + if (l-1) < m+n: + raise ValueError("not enough data for requested number of parameters") + + H = _block_hankel(YY[:,:,1:], m, n+1) # Hankel matrix (q*m, p*(n+1)) + Hf = H[:,:-p] # first p*n columns of H + Hl = H[:,p:] # last p*n columns of H + + U,S,Vh = np.linalg.svd(Hf, True) + Ur =U[:,0:r] + Vhr =Vh[0:r,:] + + # balanced realizations + Sigma_inv = np.diag(1./np.sqrt(S[0:r])) + Ar = Sigma_inv @ Ur.T @ Hl @ Vhr.T @ Sigma_inv + Br = Sigma_inv @ Ur.T @ Hf[:,0:p]*dt # dt scaling for unit-area impulse + Cr = Hf[0:q,:] @ Vhr.T @ Sigma_inv + Dr = YY[:,:,0] + + return StateSpace(Ar,Br,Cr,Dr,dt), S def markov(Y, U, m=None, transpose=False): @@ -556,3 +648,6 @@ def markov(Y, U, m=None, transpose=False): # Return the first m Markov parameters return H if transpose else np.transpose(H) + +# Function aliases +era = eigensys_realization diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py index 49c2afd58..dc50ce963 100644 --- a/control/tests/modelsimp_test.py +++ b/control/tests/modelsimp_test.py @@ -7,10 +7,10 @@ import pytest -from control import StateSpace, forced_response, tf, rss, c2d +from control import StateSpace, impulse_response, step_response, forced_response, tf, rss, c2d from control.exception import ControlMIMONotImplemented from control.tests.conftest import slycotonly -from control.modelsimp import balred, hsvd, markov, modred +from control.modelsimp import balred, hsvd, markov, modred, eigensys_realization class TestModelsimp: @@ -111,6 +111,85 @@ def testMarkovResults(self, k, m, n): # for k=5, m=n=10: 0.015 % np.testing.assert_allclose(Mtrue, Mcomp, rtol=1e-6, atol=1e-8) + def testERASignature(self): + + # test siso + # Katayama, Subspace Methods for System Identification + # Example 6.1, Fibonacci sequence + H_true = np.array([0.,1.,1.,2.,3.,5.,8.,13.,21.,34.]) + + # A realization of fibonacci impulse response + A = np.array([[0., 1.],[1., 1.,]]) + B = np.array([[1.],[1.,]]) + C = np.array([[1., 0.,]]) + D = np.array([[0.,]]) + + T = np.arange(0,10,1) + sysd_true = StateSpace(A,B,C,D,True) + ir_true = impulse_response(sysd_true,T=T) + + # test TimeResponseData + sysd_est, _ = eigensys_realization(ir_true,r=2) + ir_est = impulse_response(sysd_est, T=T) + _, H_est = ir_est + + np.testing.assert_allclose(H_true, H_est, rtol=1e-6, atol=1e-8) + + # test ndarray + _, YY_true = ir_true + sysd_est, _ = eigensys_realization(YY_true,r=2) + ir_est = impulse_response(sysd_est, T=T) + _, H_est = ir_est + + np.testing.assert_allclose(H_true, H_est, rtol=1e-6, atol=1e-8) + + # test mimo + # Mechanical Vibrations: Theory and Application, SI Edition, 1st ed. + # Figure 6.5 / Example 6.7 + # m q_dd + c q_d + k q = f + m1, k1, c1 = 1., 4., 1. + m2, k2, c2 = 2., 2., 1. + k3, c3 = 6., 2. + + A = np.array([ + [0., 0., 1., 0.], + [0., 0., 0., 1.], + [-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1], + [(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2] + ]) + B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]]) + C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]]) + D = np.zeros((2,2)) + + sys = StateSpace(A, B, C, D) + + dt = 0.1 + T = np.arange(0,10,dt) + sysd_true = sys.sample(dt, method='zoh') + ir_true = impulse_response(sysd_true, T=T) + + # test TimeResponseData + sysd_est, _ = eigensys_realization(ir_true,r=4,dt=dt) + + step_true = step_response(sysd_true) + step_est = step_response(sysd_est) + + np.testing.assert_allclose(step_true.outputs, + step_est.outputs, + rtol=1e-6, atol=1e-8) + + # test ndarray + _, YY_true = ir_true + sysd_est, _ = eigensys_realization(YY_true,r=4,dt=dt) + + step_true = step_response(sysd_true, T=T) + step_est = step_response(sysd_est, T=T) + + np.testing.assert_allclose(step_true.outputs, + step_est.outputs, + rtol=1e-6, atol=1e-8) + + def testModredMatchDC(self): #balanced realization computed in matlab for the transfer function: # num = [1 11 45 32], den = [1 15 60 200 60] diff --git a/doc/control.rst b/doc/control.rst index efd643d8a..30ae1a03b 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -137,7 +137,7 @@ Model simplification tools balred hsvd modred - era + eigensys_realization markov Nonlinear system support diff --git a/doc/era_msd.py b/doc/era_msd.py new file mode 120000 index 000000000..0cf6a5282 --- /dev/null +++ b/doc/era_msd.py @@ -0,0 +1 @@ +../examples/era_msd.py \ No newline at end of file diff --git a/doc/era_msd.rst b/doc/era_msd.rst new file mode 100644 index 000000000..de702406e --- /dev/null +++ b/doc/era_msd.rst @@ -0,0 +1,15 @@ +ERA example, mass spring damper system +-------------------------------------- + +Code +.... +.. literalinclude:: era_msd.py + :language: python + :linenos: + + +Notes +..... + +1. The environment variable `PYCONTROL_TEST_EXAMPLES` is used for +testing to turn off plotting of the outputs.0 \ No newline at end of file diff --git a/doc/examples.rst b/doc/examples.rst index 21364157e..25e161159 100644 --- a/doc/examples.rst +++ b/doc/examples.rst @@ -35,6 +35,7 @@ other sources. kincar-flatsys mrac_siso_mit mrac_siso_lyapunov + era_msd Jupyter notebooks ================= diff --git a/examples/era_msd.py b/examples/era_msd.py new file mode 100644 index 000000000..101933435 --- /dev/null +++ b/examples/era_msd.py @@ -0,0 +1,63 @@ +# era_msd.py +# Johannes Kaisinger, 4 July 2024 +# +# Demonstrate estimation of State Space model from impulse response. +# SISO, SIMO, MISO, MIMO case + +import numpy as np +import matplotlib.pyplot as plt +import os + +import control as ct + +# set up a mass spring damper system (2dof, MIMO case) +# Mechanical Vibrations: Theory and Application, SI Edition, 1st ed. +# Figure 6.5 / Example 6.7 +# m q_dd + c q_d + k q = f +m1, k1, c1 = 1., 4., 1. +m2, k2, c2 = 2., 2., 1. +k3, c3 = 6., 2. + +A = np.array([ + [0., 0., 1., 0.], + [0., 0., 0., 1.], + [-(k1+k2)/m1, (k2)/m1, -(c1+c2)/m1, c2/m1], + [(k2)/m2, -(k2+k3)/m2, c2/m2, -(c2+c3)/m2] +]) +B = np.array([[0.,0.],[0.,0.],[1/m1,0.],[0.,1/m2]]) +C = np.array([[1.0, 0.0, 0.0, 0.0],[0.0, 1.0, 0.0, 0.0]]) +D = np.zeros((2,2)) + +xixo_list = ["SISO","SIMO","MISO","MIMO"] +xixo = xixo_list[3] # choose a system for estimation +match xixo: + case "SISO": + sys = ct.StateSpace(A, B[:,0], C[0,:], D[0,0]) + case "SIMO": + sys = ct.StateSpace(A, B[:,:1], C, D[:,:1]) + case "MISO": + sys = ct.StateSpace(A, B, C[:1,:], D[:1,:]) + case "MIMO": + sys = ct.StateSpace(A, B, C, D) + + +dt = 0.1 +sysd = sys.sample(dt, method='zoh') +response = ct.impulse_response(sysd) +response.plot() +plt.show() + +sysd_est, _ = ct.eigensys_realization(response,r=4,dt=dt) + +step_true = ct.step_response(sysd) +step_true.sysname="H_true" +step_est = ct.step_response(sysd_est) +step_est.sysname="H_est" + +step_true.plot(title=xixo) +step_est.plot(color='orange',linestyle='dashed') + +plt.show() + +if 'PYCONTROL_TEST_EXAMPLES' not in os.environ: + plt.show() \ No newline at end of file