updated markov calculation + new unit tests

murrayrm · murrayrm · commit bb15b8541d2e · 2020-12-26T08:23:36.000-08:00
diff --git a/control/modelsimp.py b/control/modelsimp.py
@@ -46,7 +46,8 @@
 # External packages and modules
 import numpy as np
 import warnings
-from .exception import ControlSlycot, ControlMIMONotImplemented
+from .exception import ControlSlycot, ControlMIMONotImplemented, \
+    ControlDimension
 from .lti import isdtime, isctime
 from .statesp import StateSpace
 from .statefbk import gram
@@ -394,10 +395,22 @@ def era(YY, m, n, nin, nout, r):
     raise NotImplementedError('This function is not implemented yet.')
 
 
-def markov(Y, U, m, transpose=None):
-    """Calculate the first `M` Markov parameters [D CB CAB ...]
+def markov(Y, U, m=None, transpose=None):
+    """Calculate the first `m` Markov parameters [D CB CAB ...]
     from input `U`, output `Y`.
 
+    This function computes the Markov parameters for a discrete time system
+
+    .. math::
+
+        x[k+1] &= A x[k] + B u[k] \\\\
+        y[k] &= C x[k] + D u[k]
+
+    given data for u and y.  The algorithm assumes that that C A^k B = 0 for
+    k > m-2 (see [1]).  Note that the problem is ill-posed if the length of
+    the input data is less than the desired number of Markov parameters (a
+    warning message is generated in this case).
+
     Parameters
     ----------
     Y : array_like
@@ -407,8 +420,8 @@ def markov(Y, U, m, transpose=None):
         time points.
     U : array_like
         Input data, arranged in the same way as `Y`.
-    m : int
-        Number of Markov parameters to output.
+    m : int, optional
+        Number of Markov parameters to output.  Defaults to len(U).
     transpose : bool, optional
         Assume that input data is transposed relative to the standard
         :ref:`time-series-convention`. The default value is true for
@@ -417,7 +430,14 @@ def markov(Y, U, m, transpose=None):
     Returns
     -------
     H : ndarray
-        First m Markov parameters
+        First m Markov parameters, [D CB CAB ...]
+
+    References
+    ----------
+    .. [1] J.-N. Juang, M. Phan, L. G.  Horta, and R. W. Longman,
+       Identification of observer/Kalman filter Markov parameters - Theory
+       and experiments. Journal of Guidance Control and Dynamics, 16(2),
+       320–329, 2012. http://doi.org/10.2514/3.21006
 
     Notes
     -----
@@ -432,8 +452,8 @@ def markov(Y, U, m, transpose=None):
     --------
     >>> T = numpy.linspace(0, 10, 100)
     >>> U = numpy.ones((1, 100))
-    >>> Y = forced_response(tf([1], [1, 1]), T, U)
-    >>> H = markov(Y, U, m, transpose=False)
+    >>> T, Y, _ = forced_response(tf([1], [1, 0.5], True), T, U)
+    >>> H = markov(Y, U, 3, transpose=False)
 
     """
     # Check on the specified format of the input
@@ -449,29 +469,93 @@ def markov(Y, U, m, transpose=None):
     Umat = np.array(U, ndmin=2)
     Ymat = np.array(Y, ndmin=2)
 
-    # Transpose the data from our normal convention to match algorithm
-    # TODO: rewrite the algorithm to use standard conventions
-    if not transpose:
-        Umat = np.transpose(Umat)
-        Ymat = np.transpose(Ymat)
+    # 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[1] != 1 or Ymat.shape[1] != 1:
+    if Umat.shape[0] != 1 or Ymat.shape[0] != 1:
         raise ControlMIMONotImplemented
-    n = Umat.shape[0]
 
-    # Construct a matrix of control inputs to invert
+    # 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]
+
+    # If number of desired parameters was not given, set to size of input data
+    if m is None:
+        m = Umat.shape[1]
+
+    # Make sure there is enough data to compute parameters
+    if m > n:
+        warn.warning("Not enough data for requested number of parameters")
+
+    #
+    # 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
+    #
+    # This algorithm sets up the following problem and solves it for
+    # the Markov parameters
+    #
+    # [ 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 ]
+    #
+    # 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
+    # for j > m-2.  See equation (3) in
+    #
+    #   J.-N. Juang, M. Phan, L. G. Horta, and R. W. Longman, Identification
+    #   of observer/Kalman filter Markov parameters - Theory and
+    #   experiments. Journal of Guidance Control and Dynamics, 16(2),
+    #   320–329, 2012. http://doi.org/10.2514/3.21006
+    #
+
+    # Create matrix of (shifted) inputs
     UU = Umat
-    for i in range(1, m-1):
-        # TODO: second index on UU doesn't seem right; could be neg or pos??
-        newCol = np.vstack((0, np.reshape(UU[0:n-1, i-2], (-1, 1))))
-        UU = np.hstack((UU, newCol))
-    Ulast = np.vstack((0, np.reshape(UU[0:n-1, m-2], (-1, 1))))
-    for i in range(n-1, 0, -1):
-        Ulast[i] = np.sum(Ulast[0:i-1])
-    UU = np.hstack((UU, Ulast))
+    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)
 
     # Invert and solve for Markov parameters
-    H = np.linalg.lstsq(UU, Ymat)[0]
+    YY = np.transpose(Ymat)
+    H, _, _, _ = np.linalg.lstsq(UU, YY, rcond=None)
 
-    return H
+    # Return the first m Markov parameters
+    return H if transpose else np.transpose(H)
diff --git a/control/tests/modelsimp_array_test.py b/control/tests/modelsimp_array_test.py
@@ -49,22 +49,41 @@ def testHSVD(self):
             # Go back to using the normal np.array representation
             control.use_numpy_matrix(False)
 
-    def testMarkov(self):
+    def testMarkovSignature(self):
         U = np.array([[1., 1., 1., 1., 1.]])
         Y = U
-        M = 3
-        H = markov(Y, U, M, transpose=False)
-        Htrue = np.array([[1.], [0.], [0.]])
+        m = 3
+        H = markov(Y, U, m, transpose=False)
+        Htrue = np.array([[1., 0., 0.]])
         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(np.transpose(Y), np.transpose(U), m, transpose=True)
+        np.testing.assert_array_almost_equal( H, np.transpose(Htrue) )
+
+        # Default (in v0.8.4 and below) should be transpose=True (w/ warning)
+        import warnings
+        warnings.simplefilter('always', UserWarning)   # don't supress
+        with warnings.catch_warnings(record=True) as w:
+            # Set up warnings filter to only show warnings in control module
+            warnings.filterwarnings("ignore")
+            warnings.filterwarnings("always", module="control")
+
+            # Generate Markov parameters without any arguments
+            H = markov(np.transpose(Y), np.transpose(U), m)
+            np.testing.assert_array_almost_equal( H, np.transpose(Htrue) )
+
+            # Make sure we got a warning
+            self.assertEqual(len(w), 1)
+            self.assertIn("assumed to be in rows", str(w[-1].message))
+            self.assertIn("change in a future release", str(w[-1].message))
 
         # Test example from docstring
         T = np.linspace(0, 10, 100)
         U = np.ones((1, 100))
-        _, Y, _ = control.forced_response(control.tf([1], [1, 1]), T, U)
-        H = markov(Y, U, M, transpose=False)
+        T, Y, _ = control.forced_response(
+            control.tf([1], [1, 0.5], True), T, U)
+        H = markov(Y, U, 3, transpose=False)
 
         # Test example from issue #395
         inp = np.array([1, 2])
@@ -73,7 +92,47 @@ def testMarkov(self):
 
         # Make sure MIMO generates an error
         U = np.ones((2, 100))   # 2 inputs (Y unchanged, with 1 output)
-        np.testing.assert_raises(ControlMIMONotImplemented, markov, U, Y, M)
+        np.testing.assert_raises(ControlMIMONotImplemented, markov, Y, U, m)
+
+    # Make sure markov() returns the right answer
+    def testMarkovResults(self):
+        #
+        # Test over a range of parameters
+        #
+        # k = order of the system
+        # m = number of Markov parameters
+        # n = size of the data vector
+        #
+        # Values should match exactly for n = m, otherewise you get a
+        # close match but errors due to the assumption that C A^k B =
+        # 0 for k > m-2 (see modelsimp.py).
+        #
+        for k, m, n in \
+            ((2, 2, 2), (2, 5, 5), (5, 2, 2), (5, 5, 5), (5, 10, 10)):
+
+            # Generate stable continuous time system
+            Hc = control.rss(k, 1, 1)
+
+            # Choose sampling time based on fastest time constant / 10
+            w, _ = np.linalg.eig(Hc.A)
+            Ts = np.min(-np.real(w)) / 10.
+
+            # Convert to a discrete time system via sampling
+            Hd = control.c2d(Hc, Ts, 'zoh')
+
+            # Compute the Markov parameters from state space
+            Mtrue = np.hstack([Hd.D] + [np.dot(
+                Hd.C, np.dot(np.linalg.matrix_power(Hd.A, i),
+                             Hd.B)) for i in range(m-1)])
+
+            # Generate input/output data
+            T = np.array(range(n)) * Ts
+            U = np.cos(T) + np.sin(T/np.pi)
+            _, Y, _ = control.forced_response(Hd, T, U, squeeze=True)
+            Mcomp = markov(Y, U, m, transpose=False)
+
+            # Compare to results from markov()
+            np.testing.assert_array_almost_equal(Mtrue, Mcomp)
 
     def testModredMatchDC(self):
         #balanced realization computed in matlab for the transfer function:
diff --git a/control/tests/modelsimp_test.py b/control/tests/modelsimp_test.py
@@ -22,21 +22,13 @@ def testHSVD(self):
         np.testing.assert_array_almost_equal(hsv, hsvtrue)
 
     def testMarkov(self):
-        U = np.matrix("1., 1., 1., 1., 1.")
+        U = np.matrix("1.; 1.; 1.; 1.; 1.")
         Y = U
         M = 3
-        H = markov(Y, U, M, transpose=False)
+        H = markov(Y, U, M)
         Htrue = np.matrix("1.; 0.; 0.")
         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)
-        np.testing.assert_array_almost_equal( H, Htrue )
-
-        # Default (in v0.8.4 and below) should be transpose=True
-        H = markov(np.transpose(Y), np.transpose(U), M)
-        np.testing.assert_array_almost_equal( H, Htrue )
-
     def testModredMatchDC(self):
         #balanced realization computed in matlab for the transfer function:
         # num = [1 11 45 32], den = [1 15 60 200 60]
