Implement okid

KybernetikJo · KybernetikJo · commit 8b337f039de9 · 2024-07-15T19:52:40.000+02:00
diff --git a/control/modelsimp.py b/control/modelsimp.py
@@ -561,8 +561,8 @@ def markov(Y, U, m=None, transpose=False):
 def okid(*args, m=None, transpose=False, dt=True, truncate=False):
     """okid(Y, U, [, m])
 
-    Calculate the first `m` Markov parameters [D CB CAB ...]
-    from data
+    Calculate the first `m+1` Markov parameters [D CB CAB ...]
+    from data.
 
     This function computes the Markov parameters for a discrete time system
 
@@ -578,12 +578,12 @@ def okid(*args, m=None, transpose=False, dt=True, truncate=False):
 
     The function can be called with either 1, 2 or 3 arguments:
 
-    * ``H = okid(response)``
-    * ``H = okid(respnose, m)``
+    * ``H = okid(data)``
+    * ``H = okid(data, m)``
     * ``H = okid(Y, U)``
     * ``H = okid(Y, U, m)``
 
-    where `response` is an `TimeResponseData` object, and `Y`, `U`, are 1D or 2D
+    where `data` is an `TimeResponseData` object, and `Y`, `U`, are 1D or 2D
     array and m is an integer.
 
     Parameters
@@ -601,10 +601,10 @@ def okid(*args, m=None, transpose=False, dt=True, truncate=False):
     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.
+        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
@@ -615,7 +615,7 @@ def okid(*args, m=None, transpose=False, dt=True, truncate=False):
     Returns
     -------
     H : ndarray
-        First m Markov parameters, [D CB CAB ...]
+        First m Markov parameters, [D CB CAB ...].
 
     References
     ----------
@@ -683,29 +683,17 @@ def okid(*args, m=None, transpose=False, dt=True, truncate=False):
 
     # the algorithm - Construct a matrix of control inputs to invert
     #
-    # (q,l)   = (q,p*m) @ (p*m,l)
-    # YY.T    = H @ UU.T
+    # (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*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(l-1) ]   [ u(l-1)  u(l-2)  u(l-3) ... u(l-m) ] [ C A^{m-2} B ]
+    # (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
     #
-    # [ 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: This algorithm assumes C A^{j} B = 0
     # for j > m-2.  See equation (3) in
     #
@@ -716,42 +704,46 @@ def okid(*args, m=None, transpose=False, dt=True, truncate=False):
     #
 
     Vmat = np.concatenate((Umat, Ymat),axis=0)
-    print(Vmat.shape)
 
-    VVT = np.zeros(((q + p)*m + p, l))
+    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[i*(q+p)+p:(i+1)*(q+p)+p,i+1:] = Vmat[:,:l-i-1]
+        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 Markov parameters from  YY = VV @ Ybar.T
+    # Solve for the observer Markov parameters from  YY = VV @ Ybar.T
     YbarT, _, _, _ = np.linalg.lstsq(VV, YY, rcond=None)
-    Ybar = YbarT.T/dt # scaling
+    Ybar = YbarT.T
 
+    # Equation 11, Page 9
     D = Ybar[:,:p]
-    print(D.shape)
     
-    Ybar_r = Ybar[:,p:].reshape(q,m,q+p) # output, time*input -> output, time, input
-    Ybar_r = Ybar_r.transpose(0,2,1) # output, input, time
+    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,:] # from input
-    Ybar2 = Ybar_r[:,p:,:] # from output
+    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))
-    Y[:,:,0] = Ybar1[:,:,0] + Ybar1[:,:,0]@D
+    # Equation 12, Page 9
+    Y[:,:,0] = Ybar1[:,:,0] + Ybar2[:,:,0]@D
+
+    # Equation 15, Page 10
     for k in range(1,m):
-        Y[:,:,i] = Ybar1[:,:,i] + Ybar1[:,:,i]@D
+        Y[:,:,k] = Ybar1[:,:,k] + Ybar2[:,:,k]@D
         for i in range(k-1):
             Y[:,:,k] += Ybar2[:,:,i]@Y[:,:,k-i-1]
 
-
-    H = np.zeros((q,p,m))
+    # Equation 11, Page 9
+    H = np.zeros((q,p,m+1))
     H[:,:,0] = D
-    H[:,:,1:] = Y[:,:,:-1]
+    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
@@ -67,7 +67,7 @@ def create_impulse_response(H, time, transpose, dt):
 
 
 xixo_list = ["SISO","SIMO","MISO","MIMO"]
-xixo = xixo_list[0] # choose a system for estimation
+xixo = xixo_list[3] # choose a system for estimation
 match xixo:
     case "SISO":
         sys = ct.StateSpace(A, B[:,0], C[0,:], D[0,0])
@@ -90,8 +90,10 @@ def create_impulse_response(H, time, transpose, dt):
 response.plot()
 plt.show()
 
-m = 200
-ir_true = ct.impulse_response(sysd, T=dt*m)
+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)
