Merge pull request #941 from Jpickard1/main

sawyerbfuller · web-flow · commit 82f3fe343422 · 2023-12-01T15:44:48.000-08:00
Improved speed of ctrb and obsv functions
diff --git a/control/statefbk.py b/control/statefbk.py
@@ -990,13 +990,15 @@ def _control_output(t, states, inputs, params):
     return ctrl, closed
 
 
-def ctrb(A, B):
+def ctrb(A, B, t=None):
     """Controllabilty matrix.
 
     Parameters
     ----------
     A, B : array_like or string
         Dynamics and input matrix of the system
+    t : None or integer
+        maximum time horizon of the controllability matrix, max = A.shape[0]
 
     Returns
     -------
@@ -1016,22 +1018,30 @@ def ctrb(A, B):
     amat = _ssmatrix(A)
     bmat = _ssmatrix(B)
     n = np.shape(amat)[0]
+    m = np.shape(bmat)[1]
+    
+    if t is None or t > n:
+        t = n
 
     # Construct the controllability matrix
-    ctrb = np.hstack(
-        [bmat] + [np.linalg.matrix_power(amat, i) @ bmat
-                  for i in range(1, n)])
+    ctrb = np.zeros((n, t * m))
+    ctrb[:, :m] = bmat
+    for k in range(1, t):
+        ctrb[:, k * m:(k + 1) * m] = np.dot(amat, ctrb[:, (k - 1) * m:k * m])
+
     return _ssmatrix(ctrb)
 
 
-def obsv(A, C):
+def obsv(A, C, t=None):
     """Observability matrix.
 
     Parameters
     ----------
     A, C : array_like or string
         Dynamics and output matrix of the system
-
+    t : None or integer
+        maximum time horizon of the controllability matrix, max = A.shape[0]
+        
     Returns
     -------
     O : 2D array (or matrix)
@@ -1050,10 +1060,18 @@ def obsv(A, C):
     amat = _ssmatrix(A)
     cmat = _ssmatrix(C)
     n = np.shape(amat)[0]
+    p = np.shape(cmat)[0]
+    
+    if t is None or t > n:
+        t = n
 
     # Construct the observability matrix
-    obsv = np.vstack([cmat] + [cmat @ np.linalg.matrix_power(amat, i)
-                               for i in range(1, n)])
+    obsv = np.zeros((t * p, n))
+    obsv[:p, :] = cmat
+        
+    for k in range(1, t):
+        obsv[k * p:(k + 1) * p, :] = np.dot(obsv[(k - 1) * p:k * p, :], amat)
+
     return _ssmatrix(obsv)
 
 
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
@@ -49,6 +49,14 @@ def testCtrbMIMO(self):
         Wc = ctrb(A, B)
         np.testing.assert_array_almost_equal(Wc, Wctrue)
 
+    def testCtrbT(self):
+        A = np.array([[1., 2.], [3., 4.]])
+        B = np.array([[5., 6.], [7., 8.]])
+        t = 1
+        Wctrue = np.array([[5., 6.], [7., 8.]])
+        Wc = ctrb(A, B, t=t)
+        np.testing.assert_array_almost_equal(Wc, Wctrue)
+        
     def testObsvSISO(self):
         A = np.array([[1., 2.], [3., 4.]])
         C = np.array([[5., 7.]])
@@ -62,6 +70,14 @@ def testObsvMIMO(self):
         Wotrue = np.array([[5., 6.], [7., 8.], [23., 34.], [31., 46.]])
         Wo = obsv(A, C)
         np.testing.assert_array_almost_equal(Wo, Wotrue)
+        
+    def testObsvT(self):
+        A = np.array([[1., 2.], [3., 4.]])
+        C = np.array([[5., 6.], [7., 8.]])
+        t = 1
+        Wotrue = np.array([[5., 6.], [7., 8.]])
+        Wo = obsv(A, C, t=t)
+        np.testing.assert_array_almost_equal(Wo, Wotrue)
 
     def testCtrbObsvDuality(self):
         A = np.array([[1.2, -2.3], [3.4, -4.5]])
