diff --git a/control/statefbk.py b/control/statefbk.py index a29e86ef7..3d2ef6d9c 100644 --- 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 index d605c9be7..cb677b40a 100644 --- 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]])