Merge branch 'KybernetikJo-implement_era'

murrayrm · murrayrm · commit 5120bcc42674 · 2024-08-05T19:39:49.000-07:00
diff --git a/control/modelsimp.py b/control/modelsimp.py
@@ -49,7 +49,7 @@
 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 +368,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(*args, m=None, transpose=False, dt=None, truncate=False):
@@ -593,3 +684,6 @@ def markov(*args, m=None, transpose=False, dt=None, truncate=False):
 
     # Return the first m Markov parameters
     return H if not transpose else np.transpose(H)
+
+# Function aliases
+era = eigensys_realization
diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py
@@ -6,11 +6,12 @@
 import numpy as np
 import pytest
 
-
-from control import StateSpace, forced_response, impulse_response, tf, rss, c2d, TimeResponseData
+from control import StateSpace, TimeResponseData, c2d, forced_response, \
+    impulse_response, step_response, rss, tf
 from control.exception import ControlArgument, ControlDimension
+from control.modelsimp import balred, eigensys_realization, hsvd, markov, \
+    modred
 from control.tests.conftest import slycotonly
-from control.modelsimp import balred, hsvd, markov, modred
 
 
 class TestModelsimp:
@@ -240,6 +241,85 @@ def testMarkovResults(self, k, m, n):
         np.testing.assert_allclose(Mtrue_scaled, Mcomp_scaled, 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
@@ -36,6 +36,7 @@ other sources.
    mrac_siso_mit
    mrac_siso_lyapunov
    markov
+   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()
