From 5692317725ca5bf30560f738621a856f2347d0de Mon Sep 17 00:00:00 2001 From: Joshua Date: Sat, 25 Nov 2023 01:34:34 -0500 Subject: [PATCH 1/7] improved speed to construct observability matrix by reducing matrix exponentiation following the MATLAB implimentation of obsv --- control/statefbk.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index a29e86ef7..78fe50b97 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -1024,7 +1024,7 @@ def ctrb(A, B): return _ssmatrix(ctrb) -def obsv(A, C): +def obsv(A, C, t=None): """Observability matrix. Parameters @@ -1050,10 +1050,17 @@ def obsv(A, C): amat = _ssmatrix(A) cmat = _ssmatrix(C) n = np.shape(amat)[0] + + if t is None: + 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 * ny, n)) + obsv[:ny, :] = c + + for k in range(1, t): + obsv[k * ny:(k + 1) * ny, :] = np.dot(obsv[(k - 1) * ny:k * ny, :], a) + return _ssmatrix(obsv) From 95ab229ff4a92aa782abe0bdd554f039ce3756e3 Mon Sep 17 00:00:00 2001 From: Joshua Date: Sat, 25 Nov 2023 01:42:07 -0500 Subject: [PATCH 2/7] improved time to create controllability matrix, fix to obsv function + parameters --- control/statefbk.py | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 78fe50b97..050b95dbe 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 = n Returns ------- @@ -1016,11 +1018,17 @@ 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] = cmat + for k in range(1, t): + ctrb[:, k * m:(k + 1) * m] = np.dot(amat, obsv[:, (k - 1) * m:k * m]) + return _ssmatrix(ctrb) @@ -1031,7 +1039,9 @@ def obsv(A, C, t=None): ---------- 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 = n + Returns ------- O : 2D array (or matrix) @@ -1050,16 +1060,17 @@ def obsv(A, C, t=None): amat = _ssmatrix(A) cmat = _ssmatrix(C) n = np.shape(amat)[0] + p = np.shape(cmat)[0] - if t is None: + if t is None or t > n: t = n # Construct the observability matrix - obsv = np.zeros((t * ny, n)) - obsv[:ny, :] = c + obsv = np.zeros((t * p, n)) + obsv[:p, :] = cmat for k in range(1, t): - obsv[k * ny:(k + 1) * ny, :] = np.dot(obsv[(k - 1) * ny:k * ny, :], a) + obsv[k * p:(k + 1) * p, :] = np.dot(obsv[(k - 1) * p:k * p, :], amat) return _ssmatrix(obsv) From 7feeb2c964196cc0010251c908400adda2ec98d3 Mon Sep 17 00:00:00 2001 From: Joshua Date: Sat, 25 Nov 2023 01:52:45 -0500 Subject: [PATCH 3/7] fixed differentce between ctrb and obsv --- control/statefbk.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 050b95dbe..97e419ade 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -1025,9 +1025,9 @@ def ctrb(A, B, t=None): # Construct the controllability matrix ctrb = np.zeros((n, t * m)) - ctrb[:, :m] = cmat + ctrb[:, :m] = bmat for k in range(1, t): - ctrb[:, k * m:(k + 1) * m] = np.dot(amat, obsv[:, (k - 1) * m:k * m]) + ctrb[:, k * m:(k + 1) * m] = np.dot(amat, ctrb[:, (k - 1) * m:k * m]) return _ssmatrix(ctrb) From bfc7e6b895bf8158627aea39b4c29d950c5e5c23 Mon Sep 17 00:00:00 2001 From: Joshua Pickard Date: Mon, 27 Nov 2023 15:04:29 -0500 Subject: [PATCH 4/7] Update control/statefbk.py Co-authored-by: Sawyer Fuller <58706249+sawyerbfuller@users.noreply.github.com> --- control/statefbk.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/statefbk.py b/control/statefbk.py index 97e419ade..fb8b877c6 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -1027,7 +1027,7 @@ def ctrb(A, B, t=None): 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]) + ctrb[:, k * m:(k + 1) * m] = amat @ ctrb[:, (k - 1) * m:k * m] return _ssmatrix(ctrb) From 44d57ac92044f90fe6f8fe1a0ec541a02d3e47f6 Mon Sep 17 00:00:00 2001 From: Joshua Pickard Date: Mon, 27 Nov 2023 15:04:43 -0500 Subject: [PATCH 5/7] Update control/statefbk.py Co-authored-by: Sawyer Fuller <58706249+sawyerbfuller@users.noreply.github.com> --- control/statefbk.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/statefbk.py b/control/statefbk.py index fb8b877c6..69a58708a 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -1070,7 +1070,7 @@ def obsv(A, C, t=None): obsv[:p, :] = cmat for k in range(1, t): - obsv[k * p:(k + 1) * p, :] = np.dot(obsv[(k - 1) * p:k * p, :], amat) + obsv[k * p:(k + 1) * p, :] = obsv[(k - 1) * p:k * p, :] @ amat return _ssmatrix(obsv) From cb4f7d90559a38a6b6a81de6c2c1267e06a6320f Mon Sep 17 00:00:00 2001 From: Joshua Date: Mon, 27 Nov 2023 15:05:52 -0500 Subject: [PATCH 6/7] changed doc string --- control/statefbk.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 97e419ade..3d2ef6d9c 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -998,7 +998,7 @@ def ctrb(A, B, t=None): 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 = n + maximum time horizon of the controllability matrix, max = A.shape[0] Returns ------- @@ -1040,7 +1040,7 @@ def obsv(A, C, t=None): 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 = n + maximum time horizon of the controllability matrix, max = A.shape[0] Returns ------- From f8ba0d9caf54d41623118cfd48f8afe041b46b15 Mon Sep 17 00:00:00 2001 From: Joshua Date: Mon, 27 Nov 2023 15:25:42 -0500 Subject: [PATCH 7/7] test cases added for t < A.shape[0] --- control/statefbk.py | 4 ++-- control/tests/statefbk_test.py | 16 ++++++++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 5c4ad7659..3d2ef6d9c 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -1027,7 +1027,7 @@ def ctrb(A, B, t=None): ctrb = np.zeros((n, t * m)) ctrb[:, :m] = bmat for k in range(1, t): - ctrb[:, k * m:(k + 1) * m] = amat @ ctrb[:, (k - 1) * m:k * m] + ctrb[:, k * m:(k + 1) * m] = np.dot(amat, ctrb[:, (k - 1) * m:k * m]) return _ssmatrix(ctrb) @@ -1070,7 +1070,7 @@ def obsv(A, C, t=None): obsv[:p, :] = cmat for k in range(1, t): - obsv[k * p:(k + 1) * p, :] = obsv[(k - 1) * p:k * p, :] @ amat + 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]])