murrayrm · Aug 6, 2024 · Jul 3, 2024 · Jul 4, 2024 · Jul 4, 2024 · Jul 4, 2024
diff --git a/control/modelsimp.py b/control/modelsimp.py
@@ -43,11 +43,11 @@
 # External packages and modules
 import numpy as np
 import warnings
-from .exception import ControlSlycot, ControlMIMONotImplemented, \
-    ControlDimension
+from .exception import ControlSlycot, ControlArgument, ControlDimension
 from .iosys import isdtime, isctime
 from .statesp import StateSpace
 from .statefbk import gram
+from .timeresp import TimeResponseData
 
 __all__ = ['hsvd', 'balred', 'modred', 'era', 'markov', 'minreal']
 
@@ -402,9 +402,11 @@ def era(YY, m, n, nin, nout, r):
     raise NotImplementedError('This function is not implemented yet.')
 
 
-def markov(Y, U, m=None, transpose=False):
-    """Calculate the first `m` Markov parameters [D CB CAB ...]
-    from input `U`, output `Y`.
+def markov(*args, m=None, transpose=False, dt=True, truncate=False):
+    """markov(Y, U, [, m])
+
+    Calculate the first `m` Markov parameters [D CB CAB ...]
+    from data
 
     This function computes the Markov parameters for a discrete time system
 
@@ -418,6 +420,16 @@ def markov(Y, U, m=None, transpose=False):
     the input data is less than the desired number of Markov parameters (a
     warning message is generated in this case).
 
+    The function can be called with either 1, 2 or 3 arguments:
+
+    * ``H = markov(response)``
+    * ``H = markov(respnose, m)``
+    * ``H = markov(Y, U)``
+    * ``H = markov(Y, U, m)``
+
+    where `response` is an `TimeResponseData` object, and `Y`, `U`, are 1D or 2D
+    array and m is an integer.
+
     Parameters
     ----------
     Y : array_like
@@ -427,11 +439,22 @@ def markov(Y, U, m=None, transpose=False):
         time points.
     U : array_like
         Input data, arranged in the same way as `Y`.
+    data : TimeResponseData
+        Response data from which the Markov parameters where estimated.
+        Input and output data must be 1D or 2D array.
     m : int, optional
         Number of Markov parameters to output.  Defaults to len(U).
+    dt : True of float, optional
+        True indicates discrete time with unspecified sampling time,
+        positive number is discrete time with specified sampling time.It
+        can be used to scale the markov parameters in order to match the
+        impulse response of this library. Default is True.
+    truncate : bool, optional
+        Do not use first m equation for least least squares. Default is False.
     transpose : bool, optional
         Assume that input data is transposed relative to the standard
-        :ref:`time-series-convention`. Default value is False.
+        :ref:`time-series-convention`. For TimeResponseData this parameter
+        is ignored. Default is False.
 
     Returns
     -------
@@ -445,15 +468,6 @@ def markov(Y, U, m=None, transpose=False):
        and experiments. Journal of Guidance Control and Dynamics, 16(2),
        320-329, 2012. http://doi.org/10.2514/3.21006
 
-    Notes
-    -----
-    Currently only works for SISO systems.
-
-    This function does not currently comply with the Python Control Library
-    :ref:`time-series-convention` for representation of time series data.
-    Use `transpose=False` to make use of the standard convention (this
-    will be updated in a future release).
-
     Examples
     --------
     >>> T = np.linspace(0, 10, 100)
@@ -463,77 +477,80 @@ def markov(Y, U, m=None, transpose=False):
 
     """
     # Convert input parameters to 2D arrays (if they aren't already)
-    Umat = np.array(U, ndmin=2)
-    Ymat = np.array(Y, ndmin=2)
 
-    # If data is in transposed format, switch it around
-    if transpose:
-        Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
-
-    # Make sure the system is a SISO system
-    if Umat.shape[0] != 1 or Ymat.shape[0] != 1:
-        raise ControlMIMONotImplemented
+    # Get the system description
+    if (len(args) < 1):
+        raise ControlArgument("not enough input arguments")
+
+    if isinstance(args[0], TimeResponseData):
+        Umat = np.array(args[0].inputs, ndmin=2)
+        Ymat = np.array(args[0].outputs, ndmin=2)
+        transpose = args[0].transpose
+        if args[0].transpose and not args[0].issiso:
+            Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
+        if (len(args) == 2):
+            m = args[1]
+        elif (len(args) > 2):
+            raise ControlArgument("too many positional arguments")
+    else:
+        if (len(args) < 2):
+            raise ControlArgument("not enough input arguments")
+        Umat = np.array(args[1], ndmin=2)
+        Ymat = np.array(args[0], ndmin=2)
+        if transpose:
+            Umat, Ymat = np.transpose(Umat), np.transpose(Ymat)
+        if (len(args) == 3):
+            m = args[2]
+        elif (len(args) > 3):
+            raise ControlArgument("too many positional arguments")
 
     # Make sure the number of time points match
     if Umat.shape[1] != Ymat.shape[1]:
         raise ControlDimension(
             "Input and output data are of differnent lengths")
-    n = Umat.shape[1]
+    l = Umat.shape[1]
 
     # If number of desired parameters was not given, set to size of input data
     if m is None:
-        m = Umat.shape[1]
+        m = l
+
+    t = 0
+    if truncate:
+        t = m
+
+    q = Ymat.shape[0] # number of outputs
+    p = Umat.shape[0] # number of inputs
 
     # Make sure there is enough data to compute parameters
-    if m > n:
+    if m*p > (l-t):
         warnings.warn("Not enough data for requested number of parameters")
 
+    # the algorithm - Construct a matrix of control inputs to invert
     #
-    # Original algorithm (with mapping to standard order)
-    #
-    # RMM note, 24 Dec 2020: This algorithm sets the problem up correctly
-    # until the final column of the UU matrix is created, at which point it
-    # makes some modifications that I don't understand. This version of the
-    # algorithm does not seem to return the actual Markov parameters for a
-    # system.
-    #
-    # # Create the matrix of (shifted) inputs
-    # UU = np.transpose(Umat)
-    # for i in range(1, m-1):
-    #     # Shift previous column down and add a zero at the top
-    #     newCol = np.vstack((0, np.reshape(UU[0:n-1, i-1], (-1, 1))))
-    #     UU = np.hstack((UU, newCol))
-    #
-    # # Shift previous column down and add a zero at the top
-    # Ulast = np.vstack((0, np.reshape(UU[0:n-1, m-2], (-1, 1))))
-    #
-    # # Replace the elements of the last column new values (?)
-    # # Each row gets the sum of the rows above it (?)
-    # for i in range(n-1, 0, -1):
-    #     Ulast[i] = np.sum(Ulast[0:i-1])
-    # UU = np.hstack((UU, Ulast))
-    #
-    # # Solve for the Markov parameters from Y = H @ UU
-    # # H = [[D], [CB], [CAB], ..., [C A^{m-3} B], [???]]
-    # H = np.linalg.lstsq(UU, np.transpose(Ymat))[0]
-    #
-    # # Markov parameters are in rows => transpose if needed
-    # return H if transpose else np.transpose(H)
-
-    #
-    # New algorithm - Construct a matrix of control inputs to invert
+    # (q,l)   = (q,p*m) @ (p*m,l)
+    # YY.T    = H @ UU.T
     #
     # This algorithm sets up the following problem and solves it for
     # the Markov parameters
     #
+    # (l,q)   = (l,p*m) @ (p*m,q) 
+    # YY      = UU @ H.T
+    #
     # [ y(0)   ]   [ u(0)    0       0                 ] [ D           ]
     # [ y(1)   ]   [ u(1)    u(0)    0                 ] [ C B         ]
     # [ y(2)   ] = [ u(2)    u(1)    u(0)              ] [ C A B       ]
     # [  :     ]   [  :      :        :          :     ] [  :          ]
-    # [ y(n-1) ]   [ u(n-1)  u(n-2)  u(n-3) ... u(n-m) ] [ C A^{m-2} B ]
+    # [ y(l-1) ]   [ u(l-1)  u(l-2)  u(l-3) ... u(l-m) ] [ C A^{m-2} B ]
+    #
+    # truncated version t=m, do not use first m equation
+    #
+    # [ y(t)   ]   [ u(t)    u(t-1)  u(t-2)    u(t-m)  ] [ D           ]
+    # [ y(t+1) ]   [ u(t+1)  u(t)    u(t-1)    u(t-m+1)] [ C B         ]
+    # [ y(t+2) ] = [ u(t+2)  u(t+1)  u(t)      u(t-m+2)] [ C B         ]
+    # [  :     ]   [  :      :        :          :     ] [  :          ]
+    # [ y(l-1) ]   [ u(l-1)  u(l-2)  u(l-3) ... u(l-m) ] [ C A^{m-2} B ]
     #
-    # Note: if the number of Markov parameters (m) is less than the size of
-    # the input/output data (n), then this algorithm assumes C A^{j} B = 0
+    # Note: This algorithm assumes C A^{j} B = 0
     # for j > m-2.  See equation (3) in
     #
     #   J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman, Identification
@@ -542,17 +559,26 @@ def markov(Y, U, m=None, transpose=False):
     #   320-329, 2012. http://doi.org/10.2514/3.21006
     #
 
+    # Set up the full problem
     # Create matrix of (shifted) inputs
-    UU = Umat
-    for i in range(1, m):
-        # Shift previous column down and add a zero at the top
-        new_row = np.hstack((0, UU[i-1, 0:-1]))
-        UU = np.vstack((UU, new_row))
-    UU = np.transpose(UU)
+    UUT = np.zeros((p*m,(l)))
+    for i in range(m):
+        # Shift previous column down and keep zeros at the top
+        UUT[i*p:(i+1)*p,i:] = Umat[:,:l-i]
+
+    # Truncate first t=0 or t=m time steps, transpose the problem for lsq
+    YY = Ymat[:,t:].T
+    UU = UUT[:,t:].T
+
+    # Solve for the Markov parameters from  YY = UU @ H.T
+    HT, _, _, _ = np.linalg.lstsq(UU, YY, rcond=None)
+    H = HT.T/dt # scaling
+
+    H = H.reshape(q,m,p) # output, time*input -> output, time, input
+    H = H.transpose(0,2,1) # output, input, time
 
-    # Invert and solve for Markov parameters
-    YY = np.transpose(Ymat)
-    H, _, _, _ = np.linalg.lstsq(UU, YY, rcond=None)
+    if q == 1 and p == 1:
+        H = np.squeeze(H)
 
     # Return the first m Markov parameters
-    return H if transpose else np.transpose(H)
+    return H if not transpose else np.transpose(H)
diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py
@@ -7,8 +7,7 @@
 import pytest
 
 
-from control import StateSpace, forced_response, tf, rss, c2d
-from control.exception import ControlMIMONotImplemented
+from control import StateSpace, forced_response, impulse_response, tf, rss, c2d, TimeResponseData
 from control.tests.conftest import slycotonly
 from control.modelsimp import balred, hsvd, markov, modred
 
@@ -35,34 +34,95 @@ def testHSVD(self):
     def testMarkovSignature(self):
         U = np.array([[1., 1., 1., 1., 1.]])
         Y = U
+        response = TimeResponseData(time=np.arange(U.shape[-1]),
+                                    outputs=Y,
+                                    output_labels='y',
+                                    inputs=U,
+                                    input_labels='u',
+                                    )
+
+        # Basic usage
         m = 3
-        H = markov(Y, U, m, transpose=False)
-        Htrue = np.array([[1., 0., 0.]])
+        Htrue = np.array([1., 0., 0.])
+
+        H = markov(Y, U, m=m, transpose=False)
+        np.testing.assert_array_almost_equal(H, Htrue)
+
+        response.transpose=False
+        H = markov(response, m=m)
         np.testing.assert_array_almost_equal(H, Htrue)
 
         # Make sure that transposed data also works
-        H = markov(np.transpose(Y), np.transpose(U), m, transpose=True)
+        H = markov(Y.T, U.T, m, transpose=True)
+        np.testing.assert_array_almost_equal(H, np.transpose(Htrue))
+
+        response.transpose=True
+        H = markov(response, m)
         np.testing.assert_array_almost_equal(H, np.transpose(Htrue))
+        response.transpose=False
 
         # Generate Markov parameters without any arguments
         H = markov(Y, U, m)
         np.testing.assert_array_almost_equal(H, Htrue)
 
+        H = markov(response, m)
+        np.testing.assert_array_almost_equal(H, Htrue)
+
+        H = markov(Y, U, m=m)
+        np.testing.assert_array_almost_equal(H, Htrue)
+
+        H = markov(response, m=m)
+        np.testing.assert_array_almost_equal(H, Htrue)
+
         # Test example from docstring
+        # TODO: There is a problem here, last markov parameter does not fit
+        # the approximation error could be to big
+        Htrue = np.array([0, 1., -0.5])
         T = np.linspace(0, 10, 100)
         U = np.ones((1, 100))
         T, Y = forced_response(tf([1], [1, 0.5], True), T, U)
-        H = markov(Y, U, 3, transpose=False)
+        H = markov(Y, U, 4, transpose=False)
+        np.testing.assert_array_almost_equal(H[:3], Htrue[:3])
+
+        response = forced_response(tf([1], [1, 0.5], True), T, U)
+        H = markov(response, 4)
+        np.testing.assert_array_almost_equal(H[:3], Htrue[:3])
 
         # Test example from issue #395
         inp = np.array([1, 2])
         outp = np.array([2, 4])
         mrk = markov(outp, inp, 1, transpose=False)
 
-        # Make sure MIMO generates an error
-        U = np.ones((2, 100))   # 2 inputs (Y unchanged, with 1 output)
-        with pytest.raises(ControlMIMONotImplemented):
-            markov(Y, U, m)
+        # Test mimo example
+        # Mechanical Vibrations: Theory and Application, SI Edition, 1st ed.
+        # Figure 6.5 / Example 6.7
+        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.25
+        sysd = sys.sample(dt, method='zoh')
+
+        t = np.arange(0,100,dt)
+        u = np.random.randn(sysd.B.shape[-1], len(t))
+        response = forced_response(sysd, U=u)
+
+        m = 100
+        H = markov(response, m, dt=dt)
+        _, Htrue = impulse_response(sysd, T=dt*(m-1))
+
+        np.testing.assert_array_almost_equal(H, Htrue)
 
     # Make sure markov() returns the right answer
     @pytest.mark.parametrize("k, m, n",
@@ -98,12 +158,14 @@ def testMarkovResults(self, k, m, n):
         Mtrue = np.hstack([Hd.D] + [
             Hd.C @ np.linalg.matrix_power(Hd.A, i) @ Hd.B
             for i in range(m-1)])
+
+        Mtrue = np.squeeze(Mtrue)
 
         # Generate input/output data
         T = np.array(range(n)) * Ts
         U = np.cos(T) + np.sin(T/np.pi)
-        _, Y = forced_response(Hd, T, U, squeeze=True)
-        Mcomp = markov(Y, U, m)
+        response = forced_response(Hd, T, U, squeeze=True)
+        Mcomp = markov(response, m)
 
         # Compare to results from markov()
         # experimentally determined probability to get non matching results

diff --git a/doc/examples.rst b/doc/examples.rst
@@ -35,6 +35,7 @@ other sources.
    kincar-flatsys
    mrac_siso_mit
    mrac_siso_lyapunov
+   markov
 
 Jupyter notebooks
 =================

diff --git a/doc/markov.py b/doc/markov.py
@@ -0,0 +1 @@
+../examples/markov.py