murrayrm · Aug 6, 2024 · Jul 4, 2024 · Jul 4, 2024 · Jul 4, 2024 · Jul 4, 2024
diff --git 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,128 @@ 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 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 `response` is an `TimeResponseData` object, and `YY` is 1D or 3D
+    array and r is an integer.
-    where `response` is an `TimeResponseData` object, and `YY` is 1D or 3D
-    array and r is an integer.
+    where `data` is a `TimeResponseData` object, `YY` is a 1D or 3D
+    array, and r is an integer.
-    where `response` is an `TimeResponseData` object, and `YY` is 1D or 3D
-    array and r is an integer.
+    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,
+        positive number is discrete time with specified sampling time. It
-        positive number is discrete time with specified sampling time. It
+        and a positive float means discrete time with the specified sampling time. It
-        positive number is discrete time with specified sampling time. It
+        and a positive float means discrete time with the specified sampling time. It
+        can be used to scale the StateSpace model in order to match the
+        impulse response of this library. 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.')
+    def block_hankel_matrix(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
+
+    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_matrix(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
+    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 +647,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
@@ -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
@@ -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
@@ -0,0 +1 @@
+../examples/era_msd.py
diff --git a/doc/era_msd.rst 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
diff --git 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
@@ -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()
-Original file line number
+Diff line change
@@ Expand Up / @@ -137,7 +137,7 @@ Model simplification tools @@
         balred
         hsvd
         modred
-        era
+        eigensys_realization
         markov
     Nonlinear system support
@@ Expand Down @@