Add okid, add okid example

KybernetikJo · KybernetikJo · commit ae2aea7644d6 · 2024-07-15T20:04:54.000+02:00
diff --git a/control/modelsimp.py b/control/modelsimp.py
@@ -44,12 +44,13 @@
 import numpy as np
 import warnings
 from .exception import ControlSlycot, ControlMIMONotImplemented, \
-    ControlDimension
+    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']
+__all__ = ['hsvd', 'balred', 'modred', 'era', 'markov', 'minreal', 'okid']
 
 
 # Hankel Singular Value Decomposition
@@ -556,3 +557,193 @@ def markov(Y, U, m=None, transpose=False):
 
     # Return the first m Markov parameters
     return H if transpose else np.transpose(H)
+
+def okid(*args, m=None, transpose=False, dt=True, truncate=False):
+    """okid(Y, U, [, m])
+
+    Calculate the first `m+1` Markov parameters [D CB CAB ...]
+    from data.
+
+    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).
+
+    The function can be called with either 1, 2 or 3 arguments:
+
+    * ``H = okid(data)``
+    * ``H = okid(data, m)``
+    * ``H = okid(Y, U)``
+    * ``H = okid(Y, U, m)``
+
+    where `data` is an `TimeResponseData` object, and `Y`, `U`, are 1D or 2D
+    array and m is an integer.
+
+    Parameters
+    ----------
+    Y : array_like
+        Output data.  If the array is 1D, the system is assumed to be single
+        input.  If the array is 2D and transpose=False, the columns of `Y`
+        are taken as time points, otherwise the rows of `Y` are taken as
+        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 and a
+        positive float is discrete time with the specified sampling time.
+        It can be used to scale the Markov parameters in order to match
+        the unit-area impulse response of python-control. 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`. For TimeResponseData this parameter
+        is ignored. Default is False.
+
+    Returns
+    -------
+    H : ndarray
+        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
+
+    Examples
+    --------
+    >>> T = np.linspace(0, 10, 100)
+    >>> U = np.ones((1, 100))
+    >>> T, Y = ct.forced_response(ct.tf([1], [1, 0.5], True), T, U)
+    >>> H = ct.okid(Y, U, 3, transpose=False)
+
+    """
+    # Convert input parameters to 2D arrays (if they aren't already)
+
+    # 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")
+    l = Umat.shape[1]
+
+    # If number of desired parameters was not given, set to size of input data
+    if m is None:
+        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*p > (l-t):
+        warnings.warn("Not enough data for requested number of parameters")
+
+    # the algorithm - Construct a matrix of control inputs to invert
+    #
+    # (q,l)   = (q,(p+q)*m+p) @ ((p+q)*m+p,l)
+    # YY.T    = Ybar @ VV.T
+    #
+    # This algorithm sets up the following problem and solves it for
+    # the Markov parameters
+    #
+    # (l,q)   = (l,(p+q)*m+p) @ ((p+q)*m+p,q) 
+    # YY      = VV @ Ybar.T
+    #
+    # truncated version t=m, do not use first m equation
+    #
+    # 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
+    #   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
+    #
+
+    Vmat = np.concatenate((Umat, Ymat),axis=0)
+
+    VVT = np.zeros(((p + q)*m + p, l))
+    
+    VVT[:p,:] = Umat
+    for i in range(m):
+        # Shift previous column down and keep zeros at the top
+        VVT[(p+q)*i+p:(p+q)*(i+1)+p, i+1:] = Vmat[:, :l-i-1]
+
+    YY = Ymat[:,t:].T
+    VV = VVT[:,t:].T
+
+    # Solve for the observer Markov parameters from  YY = VV @ Ybar.T
+    YbarT, _, _, _ = np.linalg.lstsq(VV, YY, rcond=None)
+    Ybar = YbarT.T
+
+    # Equation 11, Page 9
+    D = Ybar[:,:p]
+    
+    Ybar_r = Ybar[:,p:].reshape(q,m,p+q) # output, time*v_input -> output, time, v_input
+    Ybar_r = Ybar_r.transpose(0,2,1) # output, v_input, time
+
+    Ybar1 = Ybar_r[:,:p,:] # select observer Markov parameters generated by input
+    Ybar2 = Ybar_r[:,p:,:] # select observer Markov parameters generated by output
+
+    # Using recursive substitution to solve for Markov parameters   
+    Y = np.zeros((q,p,m))
+    # Equation 12, Page 9
+    Y[:,:,0] = Ybar1[:,:,0] + Ybar2[:,:,0]@D
+
+    # Equation 15, Page 10
+    for k in range(1,m):
+        Y[:,:,k] = Ybar1[:,:,k] + Ybar2[:,:,k]@D
+        for i in range(k-1):
+            Y[:,:,k] += Ybar2[:,:,i]@Y[:,:,k-i-1]
+
+    # Equation 11, Page 9
+    H = np.zeros((q,p,m+1))
+    H[:,:,0] = D
+    H[:,:,1:] = Y[:,:,:]
+    H = H/dt # scaling
+
+    # Return the first m Markov parameters
+    return H if not transpose else np.transpose(H)
diff --git a/examples/okid_msd.py b/examples/okid_msd.py
@@ -0,0 +1,111 @@
+# okid_msd.py
+# Johannes Kaisinger, 13 July 2024
+#
+# Demonstrate estimation of markov parameters via okid.
+# SISO, SIMO, MISO, MIMO case
+
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+
+import control as ct
+
+def create_impulse_response(H, time, transpose, dt):
+    """Helper function to use TimeResponseData type for plotting"""
+
+    H = np.array(H, ndmin=3)
+
+    if transpose:
+        H = np.transpose(H)
+
+    q, p, m = H.shape
+    inputs = np.zeros((p,p,m))
+
+    issiso = True if (q == 1 and p == 1) else False
+
+    input_labels = []
+    trace_labels, trace_types = [], []
+    for i in range(p):
+        inputs[i,i,0] = 1/dt # unit area impulse
+        input_labels.append(f"u{[i]}")
+        trace_labels.append(f"From u{[i]}")
+        trace_types.append('impulse')
+
+    output_labels = []
+    for i in range(q):
+        output_labels.append(f"y{[i]}")
+
+    return ct.TimeResponseData(time=time[:m],
+                            outputs=H,
+                            output_labels=output_labels,
+                            inputs=inputs,
+                            input_labels=input_labels,
+                            trace_labels=trace_labels,
+                            trace_types=trace_types,
+                            sysname="H_est",
+                            transpose=transpose,
+                            plot_inputs=False,
+                            issiso=issiso)
+
+# 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., 1.
+
+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')
+sysd.name = "H_true"
+
+ # random forcing input
+t = np.arange(0,100,dt)
+u = np.random.randn(sysd.B.shape[-1], len(t))
+
+response = ct.forced_response(sysd, U=u)
+response.plot()
+plt.show()
+
+m = 100
+ir_true = ct.impulse_response(sysd, T=t[:m+1])
+H_true = ir_true.outputs
+print(H_true.shape)
+
+H_est = ct.okid(response, m, dt=dt)
+print(H_est.shape)
+# Helper function for plotting only
+ir_est = create_impulse_response(H_est,
+                                 ir_true.time,
+                                 ir_true.transpose,
+                                 dt)
+
+ir_true.plot(title=xixo)
+ir_est.plot(color='orange',linestyle='dashed')
+plt.show()
+
+if 'PYCONTROL_TEST_EXAMPLES' not in os.environ:
+    plt.show()
