From e2d8f9d4efe1a410c83ba8228878a4c6ccd2c86f Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Tue, 19 May 2020 11:44:58 -0700 Subject: [PATCH 01/15] added link to lqe (linear quadratic estimator) function in docs --- doc/control.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/control.rst b/doc/control.rst index 8fd3db58a..57d64b1eb 100644 --- a/doc/control.rst +++ b/doc/control.rst @@ -117,6 +117,7 @@ Control system synthesis h2syn hinfsyn lqr + lqe mixsyn place From b382f2de4ec73c033d5148095020823111ff94c8 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Mon, 8 Nov 2021 20:08:56 -0800 Subject: [PATCH 02/15] enable non-slycot lqr --- control/statefbk.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index e710f6b13..8ddaad431 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -511,7 +511,6 @@ def lqr(*args, **keywords): >>> K, S, E = lqr(A, B, Q, R, [N]) """ - # # Process the arguments and figure out what inputs we received # @@ -547,7 +546,6 @@ def lqr(*args, **keywords): X, L, G = care(A, B, Q, R, N, None, method=method) return G, X, L - def ctrb(A, B): """Controllabilty matrix From 04cfe6582143e227f708dcd2e32d3a9b60123ad5 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Mon, 8 Nov 2021 21:18:12 -0800 Subject: [PATCH 03/15] initial addition of dlqe and dlqr --- control/statefbk.py | 195 +++++++++++++++++++++++++++++++-- control/tests/statefbk_test.py | 40 ++++++- 2 files changed, 225 insertions(+), 10 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 8ddaad431..1b2582c13 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -43,9 +43,9 @@ import numpy as np from . import statesp -from .mateqn import care +from .mateqn import care, dare from .statesp import _ssmatrix, _convert_to_statespace -from .lti import LTI +from .lti import LTI, isdtime from .exception import ControlSlycot, ControlArgument, ControlDimension, \ ControlNotImplemented @@ -69,7 +69,7 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None): __all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'lqe', - 'acker'] + 'dlqr', 'dlqe', 'acker'] # Pole placement @@ -335,7 +335,7 @@ def lqe(*args, **keywords): See Also -------- - lqr + lqr, dlqe, dlqr """ @@ -403,6 +403,82 @@ def lqe(*args, **keywords): P, E, LT = care(A.T, C.T, G @ Q @ G.T, R, method=method) return _ssmatrix(LT.T), _ssmatrix(P), E +# contributed by Sawyer B. Fuller +def dlqe(A, G, C, QN, RN, NN=None): + """dlqe(A, G, C, QN, RN, [, N]) + + Linear quadratic estimator design (Kalman filter) for discrete-time + systems. Given the system + + .. math:: + + x[n+1] &= Ax[n] + Bu[n] + Gw[n] \\\\ + y[n] &= Cx[n] + Du[n] + v[n] + + with unbiased process noise w and measurement noise v with covariances + + .. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN + + The dlqe() function computes the observer gain matrix L such that the + stationary (non-time-varying) Kalman filter + + .. math:: x_e[n+1] = A x_e[n] + B u[n] + L(y[n] - C x_e[n] - D u[n]) + + produces a state estimate x_e[n] that minimizes the expected squared error + using the sensor measurements y. The noise cross-correlation `NN` is + set to zero when omitted. + + Parameters + ---------- + A, G : 2D array_like + Dynamics and noise input matrices + QN, RN : 2D array_like + Process and sensor noise covariance matrices + NN : 2D array, optional + Cross covariance matrix + + Returns + ------- + L : 2D array (or matrix) + Kalman estimator gain + P : 2D array (or matrix) + Solution to Riccati equation + + .. math:: + + A P + P A^T - (P C^T + G N) R^{-1} (C P + N^T G^T) + G Q G^T = 0 + + E : 1D array + Eigenvalues of estimator poles eig(A - L C) + + Notes + ----- + The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. + + Examples + -------- + >>> L, P, E = dlqe(A, G, C, QN, RN) + >>> L, P, E = dlqe(A, G, C, QN, RN, NN) + + See Also + -------- + dlqr, lqe, lqr + + """ + + # TODO: incorporate cross-covariance NN, something like this, + # which doesn't work for some reason + # if NN is None: + # NN = np.zeros(QN.size(0),RN.size(1)) + # NG = G @ NN + + # LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN) + # P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN) + A, G, C = np.array(A, ndmin=2), np.array(G, ndmin=2), np.array(C, ndmin=2) + QN, RN = np.array(QN, ndmin=2), np.array(RN, ndmin=2) + P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN) + return _ssmatrix(LT.T), _ssmatrix(P), E # Contributed by Roberto Bucher def acker(A, B, poles): @@ -458,7 +534,7 @@ def lqr(*args, **keywords): Linear quadratic regulator design The lqr() function computes the optimal state feedback controller - that minimizes the quadratic cost + u = -K x that minimizes the quadratic cost .. math:: J = \\int_0^\\infty (x' Q x + u' R u + 2 x' N u) dt @@ -476,8 +552,8 @@ def lqr(*args, **keywords): ---------- A, B : 2D array_like Dynamics and input matrices - sys : LTI (StateSpace or TransferFunction) - Linear I/O system + sys : LTI StateSpace system + Linear system Q, R : 2D array State and input weight matrices N : 2D array, optional @@ -546,6 +622,111 @@ def lqr(*args, **keywords): X, L, G = care(A, B, Q, R, N, None, method=method) return G, X, L +def dlqr(*args, **keywords): + """dlqr(A, B, Q, R[, N]) + + Discrete-time linear quadratic regulator design + + The dlqr() function computes the optimal state feedback controller + u[n] = - K x[n] that minimizes the quadratic cost + + .. math:: J = \\Sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n]) + + The function can be called with either 3, 4, or 5 arguments: + + * ``dlqr(dsys, Q, R)`` + * ``dlqr(dsys, Q, R, N)`` + * ``dlqr(A, B, Q, R)`` + * ``dlqr(A, B, Q, R, N)`` + + where `dsys` is a discrete-time :class:`StateSpace` system, and `A`, `B`, + `Q`, `R`, and `N` are 2d arrays of appropriate dimension (`dsys.dt` must + not be 0.) + + Parameters + ---------- + A, B : 2D array + Dynamics and input matrices + dsys : LTI :class:`StateSpace` + Discrete-time linear system + Q, R : 2D array + State and input weight matrices + N : 2D array, optional + Cross weight matrix + + Returns + ------- + K : 2D array (or matrix) + State feedback gains + S : 2D array (or matrix) + Solution to Riccati equation + E : 1D array + Eigenvalues of the closed loop system + + See Also + -------- + lqr, lqe, dlqe + + Notes + ----- + The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. + + Examples + -------- + >>> K, S, E = dlqr(dsys, Q, R, [N]) + >>> K, S, E = dlqr(A, B, Q, R, [N]) + """ + + # + # Process the arguments and figure out what inputs we received + # + + # Get the system description + if (len(args) < 3): + raise ControlArgument("not enough input arguments") + + try: + # If this works, we were (probably) passed a system as the + # first argument; extract A and B + A = np.array(args[0].A, ndmin=2, dtype=float) + B = np.array(args[0].B, ndmin=2, dtype=float) + index = 1 + except AttributeError: + # Arguments should be A and B matrices + A = np.array(args[0], ndmin=2, dtype=float) + B = np.array(args[1], ndmin=2, dtype=float) + index = 2 + + # confirm that if we received a system that it was discrete-time + if index == 1: + if not isdtime(args[0]): + raise ValueError("dsys must be discrete (dt !=0)") + + # Get the weighting matrices (converting to matrices, if needed) + Q = np.array(args[index], ndmin=2, dtype=float) + R = np.array(args[index+1], ndmin=2, dtype=float) + if (len(args) > index + 2): + N = np.array(args[index+2], ndmin=2, dtype=float) + else: + N = np.zeros((Q.shape[0], R.shape[1])) + + # Check dimensions for consistency + nstates = B.shape[0] + ninputs = B.shape[1] + if (A.shape[0] != nstates or A.shape[1] != nstates): + raise ControlDimension("inconsistent system dimensions") + + elif (Q.shape[0] != nstates or Q.shape[1] != nstates or + R.shape[0] != ninputs or R.shape[1] != ninputs or + N.shape[0] != nstates or N.shape[1] != ninputs): + raise ControlDimension("incorrect weighting matrix dimensions") + + # compute the result + S, E, K = dare(A, B, Q, R, N) + return _ssmatrix(K), _ssmatrix(S), E + + def ctrb(A, B): """Controllabilty matrix diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index fad848da2..eeac09370 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -11,7 +11,8 @@ from control.exception import ControlDimension, ControlSlycot, \ ControlArgument, slycot_check from control.mateqn import care, dare -from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker +from control.statefbk import (ctrb, obsv, place, place_varga, lqr, dlqr, + lqe, dlqe, gram, acker) from control.tests.conftest import (slycotonly, check_deprecated_matrix, ismatarrayout, asmatarrayout) @@ -77,7 +78,7 @@ def testCtrbObsvDuality(self, matarrayin): Wc = ctrb(A, B) A = np.transpose(A) C = np.transpose(B) - Wo = np.transpose(obsv(A, C)); + Wo = np.transpose(obsv(A, C)) np.testing.assert_array_almost_equal(Wc,Wo) @slycotonly @@ -306,6 +307,14 @@ def check_LQR(self, K, S, poles, Q, R): np.testing.assert_array_almost_equal(K, K_expected) np.testing.assert_array_almost_equal(poles, poles_expected) + def check_DLQR(self, K, S, poles, Q, R): + S_expected = asmatarrayout(Q) + K_expected = asmatarrayout(0) + poles_expected = -np.squeeze(np.asarray(K_expected)) + np.testing.assert_array_almost_equal(S, S_expected) + np.testing.assert_array_almost_equal(K, K_expected) + np.testing.assert_array_almost_equal(poles, poles_expected) + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) def test_LQR_integrator(self, matarrayin, matarrayout, method): if method == 'slycot' and not slycot_check(): @@ -323,6 +332,13 @@ def test_LQR_3args(self, matarrayin, matarrayout, method): K, S, poles = lqr(sys, Q, R, method=method) self.check_LQR(K, S, poles, Q, R) + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) + def test_DLQR_3args(self, matarrayin, matarrayout, method): + dsys = ss(0., 1., 1., 0., .1) + Q, R = (matarrayin([[X]]) for X in [10., 2.]) + K, S, poles = dlqr(dsys, Q, R, method=method) + self.check_DLQR(K, S, poles, Q, R) + def test_lqr_badmethod(self): A, B, Q, R = 0, 1, 10, 2 with pytest.raises(ControlArgument, match="Unknown method"): @@ -353,7 +369,6 @@ def testLQR_warning(self): with pytest.warns(UserWarning): (K, S, E) = lqr(A, B, Q, R, N) - @slycotonly def test_lqr_call_format(self): # Create a random state space system for testing sys = rss(2, 3, 2) @@ -386,6 +401,25 @@ def test_lqr_call_format(self): with pytest.raises(ct.ControlDimension, match="Q must be a square"): K, S, E = lqr(sys.A, sys.B, sys.C, R, Q) + @pytest.mark.xfail(reason="warning not implemented") + def testDLQR_warning(self): + """Test dlqr() + + Make sure we get a warning if [Q N;N' R] is not positive semi-definite + """ + # from matlab_test siso.ss2 (testLQR); probably not referenced before + # not yet implemented check + A = np.array([[-2, 3, 1], + [-1, 0, 0], + [0, 1, 0]]) + B = np.array([[-1, 0, 0]]).T + Q = np.eye(3) + R = np.eye(1) + N = np.array([[1, 1, 2]]).T + # assert any(np.linalg.eigvals(np.block([[Q, N], [N.T, R]])) < 0) + with pytest.warns(UserWarning): + (K, S, E) = dlqr(A, B, Q, R, N) + def check_LQE(self, L, P, poles, G, QN, RN): P_expected = asmatarrayout(np.sqrt(G @ QN @ G @ RN)) L_expected = asmatarrayout(P_expected / RN) From 0fa431c5ebb3c5bc3af4a24d5af594db85b6cee5 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Tue, 9 Nov 2021 13:11:34 -0800 Subject: [PATCH 04/15] dare and care now use scipy routines if slycot not available. fixed bug in non-slycot care. lqr, lqe, dlqr, dlqe now get tested without slycot. --- control/mateqn.py | 3 ++- control/statefbk.py | 9 ++++----- control/tests/statefbk_test.py | 24 +++++++++++++++++++++--- 3 files changed, 27 insertions(+), 9 deletions(-) diff --git a/control/mateqn.py b/control/mateqn.py index 3a723591f..668309a1e 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -527,7 +527,8 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): :math:`A^T X A - E^T X E - (A^T X B + S) (B^T X B + R)^{-1} (B^T X A + S^T) + Q = 0` where A, Q and E are square matrices of the same dimension. Further, Q and - R are symmetric matrices. The function returns the solution X, the gain + R are symmetric matrices. If R is None, it is set to the identity + matrix. The function returns the solution X, the gain matrix :math:`G = (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop eigenvalues L, i.e., the eigenvalues of A - B G , E. diff --git a/control/statefbk.py b/control/statefbk.py index 1b2582c13..69f1edbe8 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -403,6 +403,7 @@ def lqe(*args, **keywords): P, E, LT = care(A.T, C.T, G @ Q @ G.T, R, method=method) return _ssmatrix(LT.T), _ssmatrix(P), E + # contributed by Sawyer B. Fuller def dlqe(A, G, C, QN, RN, NN=None): """dlqe(A, G, C, QN, RN, [, N]) @@ -473,10 +474,7 @@ def dlqe(A, G, C, QN, RN, NN=None): # NN = np.zeros(QN.size(0),RN.size(1)) # NG = G @ NN - # LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN) - # P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN) - A, G, C = np.array(A, ndmin=2), np.array(G, ndmin=2), np.array(C, ndmin=2) - QN, RN = np.array(QN, ndmin=2), np.array(RN, ndmin=2) + A, G, C, QN, RN = map(np.atleast_2d, (A, G, C, QN, RN)) P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN) return _ssmatrix(LT.T), _ssmatrix(P), E @@ -574,7 +572,7 @@ def lqr(*args, **keywords): See Also -------- - lqe + lqe, dlqr, dlqe Notes ----- @@ -622,6 +620,7 @@ def lqr(*args, **keywords): X, L, G = care(A, B, Q, R, N, None, method=method) return G, X, L + def dlqr(*args, **keywords): """dlqr(A, B, Q, R[, N]) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index eeac09370..551bfb5a7 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -166,7 +166,7 @@ def testAcker(self, fixedseed): continue # Place the poles at random locations - des = rss(states, 1, 1); + des = rss(states, 1, 1) poles = pole(des) # Now place the poles using acker @@ -339,6 +339,11 @@ def test_DLQR_3args(self, matarrayin, matarrayout, method): K, S, poles = dlqr(dsys, Q, R, method=method) self.check_DLQR(K, S, poles, Q, R) + def test_DLQR_4args(self, matarrayin, matarrayout): + A, B, Q, R = (matarrayin([[X]]) for X in [0., 1., 10., 2.]) + K, S, poles = dlqr(A, B, Q, R) + self.check_DLQR(K, S, poles, Q, R) + def test_lqr_badmethod(self): A, B, Q, R = 0, 1, 10, 2 with pytest.raises(ControlArgument, match="Unknown method"): @@ -469,8 +474,21 @@ def test_lqe_call_format(self): with pytest.raises(ct.ControlDimension, match="incorrect covariance"): L, P, E = lqe(sys.A, sys.B, sys.C, R, Q) + def check_DLQE(self, L, P, poles, G, QN, RN): + P_expected = asmatarrayout(G.dot(QN).dot(G)) + L_expected = asmatarrayout(0) + poles_expected = -np.squeeze(np.asarray(L_expected)) + np.testing.assert_array_almost_equal(P, P_expected) + np.testing.assert_array_almost_equal(L, L_expected) + np.testing.assert_array_almost_equal(poles, poles_expected) + + def test_DLQE(self, matarrayin): + A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.]) + L, P, poles = dlqe(A, G, C, QN, RN) + self.check_DLQE(L, P, poles, G, QN, RN) + def test_care(self, matarrayin): - """Test stabilizing and anti-stabilizing feedbacks, continuous""" + """Test stabilizing feedback, continuous""" A = matarrayin(np.diag([1, -1])) B = matarrayin(np.identity(2)) Q = matarrayin(np.identity(2)) @@ -489,7 +507,7 @@ def test_care(self, matarrayin): X, L, G = care(A, B, Q, R, S, E, stabilizing=False) def test_dare(self, matarrayin): - """Test stabilizing and anti-stabilizing feedbacks, discrete""" + """Test stabilizing feedback, discrete""" A = matarrayin(np.diag([0.5, 2])) B = matarrayin(np.identity(2)) Q = matarrayin(np.identity(2)) From 0af8f8327a4f74b0b5278a3deae50cf7c9ae488e Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Tue, 9 Nov 2021 13:32:22 -0800 Subject: [PATCH 05/15] enabled some non-slycot testing of dare and care functions in mateqn.py --- control/tests/mateqn_test.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 0ae5a7db2..224cf1bdc 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -48,6 +48,7 @@ class TestMatrixEquations: """These are tests for the matrix equation solvers in mateqn.py""" + @slycotonly def test_lyap(self): A = array([[-1, 1], [-1, 0]]) Q = array([[1, 0], [0, 1]]) @@ -67,6 +68,7 @@ def test_lyap(self): X_slycot = lyap(A, Q, method='slycot') assert_array_almost_equal(X_scipy, X_slycot) + @slycotonly def test_lyap_sylvester(self): A = 5 B = array([[4, 3], [4, 3]]) @@ -129,7 +131,6 @@ def test_dlyap_g(self): with pytest.raises(ControlArgument, match="'scipy' not valid"): X = dlyap(A, Q, None, E, method='scipy') - @slycotonly def test_dlyap_sylvester(self): A = 5 B = array([[4, 3], [4, 3]]) @@ -317,6 +318,7 @@ def test_dare_g2(self): lam = eigvals(A - B @ G, E) assert_array_less(abs(lam), 1.0) + @slycotonly def test_raise(self): """ Test exception raise for invalid inputs """ From 798ccd599b5ee00b8ad7af3261b5bbc0689fb284 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Tue, 9 Nov 2021 13:49:56 -0800 Subject: [PATCH 06/15] change exceptions to controlDimension instead of controlArgument for when arrays are not of the correct dimension --- control/tests/mateqn_test.py | 1 + 1 file changed, 1 insertion(+) diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 224cf1bdc..96fb6da49 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -131,6 +131,7 @@ def test_dlyap_g(self): with pytest.raises(ControlArgument, match="'scipy' not valid"): X = dlyap(A, Q, None, E, method='scipy') + @slycotonly def test_dlyap_sylvester(self): A = 5 B = array([[4, 3], [4, 3]]) From 88960af64254b0551fa47bbb87c15ec6a1f63e49 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Tue, 9 Nov 2021 14:05:57 -0800 Subject: [PATCH 07/15] correct the expected error type in mateqn_test.py --- control/tests/mateqn_test.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 96fb6da49..e9011b982 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -341,7 +341,7 @@ def test_raise(self): cdlyap(Afq, Q) with pytest.raises(ControlDimension): cdlyap(A, Qfq) - with pytest.raises(ControlArgument): + with pytest.raises(ValueError): cdlyap(A, Qfs) with pytest.raises(ControlDimension): cdlyap(Afq, Q, C) @@ -353,9 +353,9 @@ def test_raise(self): cdlyap(A, Qfq, None, E) with pytest.raises(ControlDimension): cdlyap(A, Q, None, Efq) - with pytest.raises(ControlArgument): + with pytest.raises(ValueError): cdlyap(A, Qfs, None, E) - with pytest.raises(ControlArgument): + with pytest.raises(ValueError): cdlyap(A, Q, C, E) B = array([[1, 0], [0, 1]]) @@ -376,7 +376,7 @@ def test_raise(self): care(A, Bf, Q) with pytest.raises(ControlDimension): care(1, B, 1) - with pytest.raises(ControlArgument): + with pytest.raises(ValueError): care(A, B, Qfs) with pytest.raises(ControlArgument): dare(A, B, Q, Rfs) @@ -393,7 +393,7 @@ def test_raise(self): cdare(A, B, Q, Rfq, S, E) with pytest.raises(ControlDimension): cdare(A, B, Q, R, Sf, E) - with pytest.raises(ControlArgument): + with pytest.raises(ValueError): cdare(A, B, Qfs, R, S, E) - with pytest.raises(ControlArgument): + with pytest.raises(ValueError): cdare(A, B, Q, Rfs, S, E) From 90ad3a6cb59487742a519eb0b4ad0d455c57814c Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Tue, 9 Nov 2021 14:10:27 -0800 Subject: [PATCH 08/15] one more error type correction --- control/tests/mateqn_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index e9011b982..7c0ad36b0 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -355,7 +355,7 @@ def test_raise(self): cdlyap(A, Q, None, Efq) with pytest.raises(ValueError): cdlyap(A, Qfs, None, E) - with pytest.raises(ValueError): + with pytest.raises(ControlArgument): cdlyap(A, Q, C, E) B = array([[1, 0], [0, 1]]) From e26f7650216a6ba00ffbf11c7b2f054a082953c6 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Tue, 9 Nov 2021 14:38:37 -0800 Subject: [PATCH 09/15] shortened testing code as suggested by @bnavigator --- control/tests/mateqn_test.py | 11 +++++------ control/tests/statefbk_test.py | 20 +++++++++----------- 2 files changed, 14 insertions(+), 17 deletions(-) diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 7c0ad36b0..4c02b3102 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -319,7 +319,6 @@ def test_dare_g2(self): lam = eigvals(A - B @ G, E) assert_array_less(abs(lam), 1.0) - @slycotonly def test_raise(self): """ Test exception raise for invalid inputs """ @@ -341,7 +340,7 @@ def test_raise(self): cdlyap(Afq, Q) with pytest.raises(ControlDimension): cdlyap(A, Qfq) - with pytest.raises(ValueError): + with pytest.raises(ControlArgument): cdlyap(A, Qfs) with pytest.raises(ControlDimension): cdlyap(Afq, Q, C) @@ -353,7 +352,7 @@ def test_raise(self): cdlyap(A, Qfq, None, E) with pytest.raises(ControlDimension): cdlyap(A, Q, None, Efq) - with pytest.raises(ValueError): + with pytest.raises(ControlArgument): cdlyap(A, Qfs, None, E) with pytest.raises(ControlArgument): cdlyap(A, Q, C, E) @@ -376,7 +375,7 @@ def test_raise(self): care(A, Bf, Q) with pytest.raises(ControlDimension): care(1, B, 1) - with pytest.raises(ValueError): + with pytest.raises(ControlArgument): care(A, B, Qfs) with pytest.raises(ControlArgument): dare(A, B, Q, Rfs) @@ -393,7 +392,7 @@ def test_raise(self): cdare(A, B, Q, Rfq, S, E) with pytest.raises(ControlDimension): cdare(A, B, Q, R, Sf, E) - with pytest.raises(ValueError): + with pytest.raises(ControlArgument): cdare(A, B, Qfs, R, S, E) - with pytest.raises(ValueError): + with pytest.raises(ControlArgument): cdare(A, B, Q, Rfs, S, E) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 551bfb5a7..3e31d2372 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -488,7 +488,7 @@ def test_DLQE(self, matarrayin): self.check_DLQE(L, P, poles, G, QN, RN) def test_care(self, matarrayin): - """Test stabilizing feedback, continuous""" + """Test stabilizing and anti-stabilizing feedback, continuous""" A = matarrayin(np.diag([1, -1])) B = matarrayin(np.identity(2)) Q = matarrayin(np.identity(2)) @@ -506,8 +506,11 @@ def test_care(self, matarrayin): with pytest.raises(ControlArgument, match="'scipy' not valid"): X, L, G = care(A, B, Q, R, S, E, stabilizing=False) - def test_dare(self, matarrayin): - """Test stabilizing feedback, discrete""" + @pytest.mark.parametrize( + "stabilizing", + [True, pytest.param(False, marks=slycotonly)]) + def test_dare(self, matarrayin, stabilizing): + """Test stabilizing and anti-stabilizing feedback, discrete""" A = matarrayin(np.diag([0.5, 2])) B = matarrayin(np.identity(2)) Q = matarrayin(np.identity(2)) @@ -515,12 +518,7 @@ def test_dare(self, matarrayin): S = matarrayin(np.zeros((2, 2))) E = matarrayin(np.identity(2)) - X, L, G = dare(A, B, Q, R, S, E, stabilizing=True) - assert np.all(np.abs(L) < 1) + X, L, G = dare(A, B, Q, R, S, E, stabilizing=stabilizing) + sgn = {True: -1, False: 1}[stabilizing] + assert np.all(sgn * (np.abs(L) - 1) > 0) - if slycot_check(): - X, L, G = dare(A, B, Q, R, S, E, stabilizing=False) - assert np.all(np.abs(L) > 1) - else: - with pytest.raises(ControlArgument, match="'scipy' not valid"): - X, L, G = dare(A, B, Q, R, S, E, stabilizing=False) From 3e997420a1a54c26fe7cf23404ae741b36e4b1e9 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 26 Dec 2021 13:24:52 -0800 Subject: [PATCH 10/15] enable non-slycot testing + minor cleanup after rebase --- control/statefbk.py | 6 +++--- control/tests/mateqn_test.py | 2 -- control/tests/statefbk_test.py | 6 +++--- 3 files changed, 6 insertions(+), 8 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 69f1edbe8..6dab3814a 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -638,15 +638,15 @@ def dlqr(*args, **keywords): * ``dlqr(A, B, Q, R)`` * ``dlqr(A, B, Q, R, N)`` - where `dsys` is a discrete-time :class:`StateSpace` system, and `A`, `B`, - `Q`, `R`, and `N` are 2d arrays of appropriate dimension (`dsys.dt` must + where `dsys` is a discrete-time :class:`StateSpace` system, and `A`, `B`, + `Q`, `R`, and `N` are 2d arrays of appropriate dimension (`dsys.dt` must not be 0.) Parameters ---------- A, B : 2D array Dynamics and input matrices - dsys : LTI :class:`StateSpace` + dsys : LTI :class:`StateSpace` Discrete-time linear system Q, R : 2D array State and input weight matrices diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 4c02b3102..0ae5a7db2 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -48,7 +48,6 @@ class TestMatrixEquations: """These are tests for the matrix equation solvers in mateqn.py""" - @slycotonly def test_lyap(self): A = array([[-1, 1], [-1, 0]]) Q = array([[1, 0], [0, 1]]) @@ -68,7 +67,6 @@ def test_lyap(self): X_slycot = lyap(A, Q, method='slycot') assert_array_almost_equal(X_scipy, X_slycot) - @slycotonly def test_lyap_sylvester(self): A = 5 B = array([[4, 3], [4, 3]]) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 3e31d2372..d2a5ddc14 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -11,8 +11,8 @@ from control.exception import ControlDimension, ControlSlycot, \ ControlArgument, slycot_check from control.mateqn import care, dare -from control.statefbk import (ctrb, obsv, place, place_varga, lqr, dlqr, - lqe, dlqe, gram, acker) +from control.statefbk import (ctrb, obsv, place, place_varga, lqr, dlqr, + lqe, dlqe, gram, acker) from control.tests.conftest import (slycotonly, check_deprecated_matrix, ismatarrayout, asmatarrayout) @@ -507,7 +507,7 @@ def test_care(self, matarrayin): X, L, G = care(A, B, Q, R, S, E, stabilizing=False) @pytest.mark.parametrize( - "stabilizing", + "stabilizing", [True, pytest.param(False, marks=slycotonly)]) def test_dare(self, matarrayin, stabilizing): """Test stabilizing and anti-stabilizing feedback, discrete""" From 20b0312a79b94254296c5629fbe3a10271a333b4 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 26 Dec 2021 15:38:15 -0800 Subject: [PATCH 11/15] add lqr/lqe overload for discrete time systems --- control/statefbk.py | 91 ++++++++++++++++++++++++++++------ control/tests/statefbk_test.py | 84 +++++++++++++++++++++++++++++-- 2 files changed, 155 insertions(+), 20 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 6dab3814a..eb52cb123 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -45,7 +45,7 @@ from . import statesp from .mateqn import care, dare from .statesp import _ssmatrix, _convert_to_statespace -from .lti import LTI, isdtime +from .lti import LTI, isdtime, isctime from .exception import ControlSlycot, ControlArgument, ControlDimension, \ ControlNotImplemented @@ -325,8 +325,13 @@ def lqe(*args, **keywords): Notes ----- - The return type for 2D arrays depends on the default class set for - state space operations. See :func:`~control.use_numpy_matrix`. + 1. If the first argument is an LTI object, then this object will be used + to define the dynamics, noise and output matrices. Furthermore, if + the LTI object corresponds to a discrete time system, the ``dlqe()`` + function will be called. + + 2. The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. Examples -------- @@ -356,6 +361,11 @@ def lqe(*args, **keywords): if (len(args) < 3): raise ControlArgument("not enough input arguments") + # If we were passed a discrete time system as the first arg, use dlqe() + if isinstance(args[0], LTI) and isdtime(args[0], strict=True): + # Call dlqe + return dlqe(*args, **keywords) + try: sys = args[0] # Treat the first argument as a system if isinstance(sys, LTI): @@ -405,7 +415,7 @@ def lqe(*args, **keywords): # contributed by Sawyer B. Fuller -def dlqe(A, G, C, QN, RN, NN=None): +def dlqe(*args, **keywords): """dlqe(A, G, C, QN, RN, [, N]) Linear quadratic estimator design (Kalman filter) for discrete-time @@ -436,7 +446,11 @@ def dlqe(A, G, C, QN, RN, NN=None): QN, RN : 2D array_like Process and sensor noise covariance matrices NN : 2D array, optional - Cross covariance matrix + Cross covariance matrix (not yet supported) + method : str, optional + Set the method used for computing the result. Current methods are + 'slycot' and 'scipy'. If set to None (default), try 'slycot' first + and then 'scipy'. Returns ------- @@ -468,14 +482,49 @@ def dlqe(A, G, C, QN, RN, NN=None): """ + # + # Process the arguments and figure out what inputs we received + # + + # Get the method to use (if specified as a keyword) + method = keywords.get('method', None) + + # Get the system description + if (len(args) < 3): + raise ControlArgument("not enough input arguments") + + # If we were passed a continus time system as the first arg, raise error + if isinstance(args[0], LTI) and isctime(args[0], strict=True): + raise ControlArgument("dlqr() called with a continuous time system") + + try: + # If this works, we were (probably) passed a system as the + # first argument; extract A and B + A = np.array(args[0].A, ndmin=2, dtype=float) + G = np.array(args[0].B, ndmin=2, dtype=float) + C = np.array(args[0].C, ndmin=2, dtype=float) + index = 1 + except AttributeError: + # Arguments should be A and B matrices + A = np.array(args[0], ndmin=2, dtype=float) + G = np.array(args[1], ndmin=2, dtype=float) + C = np.array(args[2], ndmin=2, dtype=float) + index = 3 + + # Get the weighting matrices (converting to matrices, if needed) + QN = np.array(args[index], ndmin=2, dtype=float) + RN = np.array(args[index+1], ndmin=2, dtype=float) + # TODO: incorporate cross-covariance NN, something like this, # which doesn't work for some reason # if NN is None: # NN = np.zeros(QN.size(0),RN.size(1)) # NG = G @ NN + if len(args) > index + 2: + NN = np.array(args[index+2], ndmin=2, dtype=float) + raise ControlNotImplemented("cross-covariance not yet implememented") - A, G, C, QN, RN = map(np.atleast_2d, (A, G, C, QN, RN)) - P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN) + P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN, method=method) return _ssmatrix(LT.T), _ssmatrix(P), E # Contributed by Roberto Bucher @@ -576,8 +625,13 @@ def lqr(*args, **keywords): Notes ----- - The return type for 2D arrays depends on the default class set for - state space operations. See :func:`~control.use_numpy_matrix`. + 1. If the first argument is an LTI object, then this object will be used + to define the dynamics and input matrices. Furthermore, if the LTI + object corresponds to a discrete time system, the ``dlqr()`` function + will be called. + + 2. The return type for 2D arrays depends on the default class set for + state space operations. See :func:`~control.use_numpy_matrix`. Examples -------- @@ -596,6 +650,11 @@ def lqr(*args, **keywords): if (len(args) < 3): raise ControlArgument("not enough input arguments") + # If we were passed a discrete time system as the first arg, use dlqr() + if isinstance(args[0], LTI) and isdtime(args[0], strict=True): + # Call dlqr + return dlqr(*args, **keywords) + try: # If this works, we were (probably) passed a system as the # first argument; extract A and B @@ -681,10 +740,17 @@ def dlqr(*args, **keywords): # Process the arguments and figure out what inputs we received # + # Get the method to use (if specified as a keyword) + method = keywords.get('method', None) + # Get the system description if (len(args) < 3): raise ControlArgument("not enough input arguments") + # If we were passed a continus time system as the first arg, raise error + if isinstance(args[0], LTI) and isctime(args[0], strict=True): + raise ControlArgument("dsys must be discrete time (dt != 0)") + try: # If this works, we were (probably) passed a system as the # first argument; extract A and B @@ -697,11 +763,6 @@ def dlqr(*args, **keywords): B = np.array(args[1], ndmin=2, dtype=float) index = 2 - # confirm that if we received a system that it was discrete-time - if index == 1: - if not isdtime(args[0]): - raise ValueError("dsys must be discrete (dt !=0)") - # Get the weighting matrices (converting to matrices, if needed) Q = np.array(args[index], ndmin=2, dtype=float) R = np.array(args[index+1], ndmin=2, dtype=float) @@ -722,7 +783,7 @@ def dlqr(*args, **keywords): raise ControlDimension("incorrect weighting matrix dimensions") # compute the result - S, E, K = dare(A, B, Q, R, N) + S, E, K = dare(A, B, Q, R, N, method=method) return _ssmatrix(K), _ssmatrix(S), E diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index d2a5ddc14..738f068fb 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -298,7 +298,6 @@ def testPlace_varga_discrete_partial_eigs(self, matarrayin): P_placed = np.linalg.eigvals(A - B @ K) self.checkPlaced(P_expected, P_placed) - def check_LQR(self, K, S, poles, Q, R): S_expected = asmatarrayout(np.sqrt(Q @ R)) K_expected = asmatarrayout(S_expected / R) @@ -334,6 +333,8 @@ def test_LQR_3args(self, matarrayin, matarrayout, method): @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) def test_DLQR_3args(self, matarrayin, matarrayout, method): + if method == 'slycot' and not slycot_check(): + return dsys = ss(0., 1., 1., 0., .1) Q, R = (matarrayin([[X]]) for X in [10., 2.]) K, S, poles = dlqr(dsys, Q, R, method=method) @@ -433,9 +434,13 @@ def check_LQE(self, L, P, poles, G, QN, RN): np.testing.assert_array_almost_equal(L, L_expected) np.testing.assert_array_almost_equal(poles, poles_expected) - def test_LQE(self, matarrayin): + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) + def test_LQE(self, matarrayin, method): + if method == 'slycot' and not slycot_check(): + return + A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.]) - L, P, poles = lqe(A, G, C, QN, RN) + L, P, poles = lqe(A, G, C, QN, RN, method=method) self.check_LQE(L, P, poles, G, QN, RN) def test_lqe_call_format(self): @@ -482,9 +487,13 @@ def check_DLQE(self, L, P, poles, G, QN, RN): np.testing.assert_array_almost_equal(L, L_expected) np.testing.assert_array_almost_equal(poles, poles_expected) - def test_DLQE(self, matarrayin): + @pytest.mark.parametrize("method", [None, 'slycot', 'scipy']) + def test_DLQE(self, matarrayin, method): + if method == 'slycot' and not slycot_check(): + return + A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.]) - L, P, poles = dlqe(A, G, C, QN, RN) + L, P, poles = dlqe(A, G, C, QN, RN, method=method) self.check_DLQE(L, P, poles, G, QN, RN) def test_care(self, matarrayin): @@ -522,3 +531,68 @@ def test_dare(self, matarrayin, stabilizing): sgn = {True: -1, False: 1}[stabilizing] assert np.all(sgn * (np.abs(L) - 1) > 0) + def test_lqr_discrete(self): + """Test overloading of lqr operator for discrete time systems""" + csys = ct.rss(2, 1, 1) + dsys = ct.drss(2, 1, 1) + Q = np.eye(2) + R = np.eye(1) + + # Calling with a system versus explicit A, B should be the sam + K_csys, S_csys, E_csys = ct.lqr(csys, Q, R) + K_expl, S_expl, E_expl = ct.lqr(csys.A, csys.B, Q, R) + np.testing.assert_almost_equal(K_csys, K_expl) + np.testing.assert_almost_equal(S_csys, S_expl) + np.testing.assert_almost_equal(E_csys, E_expl) + + # Calling lqr() with a discrete time system should call dlqr() + K_lqr, S_lqr, E_lqr = ct.lqr(dsys, Q, R) + K_dlqr, S_dlqr, E_dlqr = ct.dlqr(dsys, Q, R) + np.testing.assert_almost_equal(K_lqr, K_dlqr) + np.testing.assert_almost_equal(S_lqr, S_dlqr) + np.testing.assert_almost_equal(E_lqr, E_dlqr) + + # Calling lqr() with no timebase should call lqr() + asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None) + K_asys, S_asys, E_asys = ct.lqr(asys, Q, R) + K_expl, S_expl, E_expl = ct.lqr(csys.A, csys.B, Q, R) + np.testing.assert_almost_equal(K_asys, K_expl) + np.testing.assert_almost_equal(S_asys, S_expl) + np.testing.assert_almost_equal(E_asys, E_expl) + + # Calling dlqr() with a continuous time system should raise an error + with pytest.raises(ControlArgument, match="dsys must be discrete"): + K, S, E = ct.dlqr(csys, Q, R) + + def test_lqe_discrete(self): + """Test overloading of lqe operator for discrete time systems""" + csys = ct.rss(2, 1, 1) + dsys = ct.drss(2, 1, 1) + Q = np.eye(1) + R = np.eye(1) + + # Calling with a system versus explicit A, B should be the sam + K_csys, S_csys, E_csys = ct.lqe(csys, Q, R) + K_expl, S_expl, E_expl = ct.lqe(csys.A, csys.B, csys.C, Q, R) + np.testing.assert_almost_equal(K_csys, K_expl) + np.testing.assert_almost_equal(S_csys, S_expl) + np.testing.assert_almost_equal(E_csys, E_expl) + + # Calling lqe() with a discrete time system should call dlqe() + K_lqe, S_lqe, E_lqe = ct.lqe(dsys, Q, R) + K_dlqe, S_dlqe, E_dlqe = ct.dlqe(dsys, Q, R) + np.testing.assert_almost_equal(K_lqe, K_dlqe) + np.testing.assert_almost_equal(S_lqe, S_dlqe) + np.testing.assert_almost_equal(E_lqe, E_dlqe) + + # Calling lqe() with no timebase should call lqe() + asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None) + K_asys, S_asys, E_asys = ct.lqe(asys, Q, R) + K_expl, S_expl, E_expl = ct.lqe(csys.A, csys.B, csys.C, Q, R) + np.testing.assert_almost_equal(K_asys, K_expl) + np.testing.assert_almost_equal(S_asys, S_expl) + np.testing.assert_almost_equal(E_asys, E_expl) + + # Calling dlqe() with a continuous time system should raise an error + with pytest.raises(ControlArgument, match="called with a continuous"): + K, S, E = ct.dlqe(csys, Q, R) From cf3c9b2749d190616a39acc009ae93526d109a42 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 26 Dec 2021 16:33:20 -0800 Subject: [PATCH 12/15] remove redundant argument process in care/dare + fix up error strings --- control/mateqn.py | 84 +++++++++++++--------------------- control/statefbk.py | 80 +++++++++++++------------------- control/tests/statefbk_test.py | 4 +- 3 files changed, 65 insertions(+), 103 deletions(-) diff --git a/control/mateqn.py b/control/mateqn.py index 668309a1e..6493f537f 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -343,7 +343,8 @@ def dlyap(A, Q, C=None, E=None, method=None): # Riccati equation solvers care and dare # -def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): +def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, + A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"): """X, L, G = care(A, B, Q, R=None) solves the continuous-time algebraic Riccati equation @@ -428,10 +429,10 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): m = B.shape[1] # Check to make sure input matrices are the right shape and type - _check_shape("A", A, n, n, square=True) - _check_shape("B", B, n, m) - _check_shape("Q", Q, n, n, square=True, symmetric=True) - _check_shape("R", R, m, m, square=True, symmetric=True) + _check_shape(A_s, A, n, n, square=True) + _check_shape(B_s, B, n, m) + _check_shape(Q_s, Q, n, n, square=True, symmetric=True) + _check_shape(R_s, R, m, m, square=True, symmetric=True) # Solve the standard algebraic Riccati equation if S is None and E is None: @@ -471,8 +472,8 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2) # Check to make sure input matrices are the right shape and type - _check_shape("E", E, n, n, square=True) - _check_shape("S", S, n, m) + _check_shape(E_s, E, n, n, square=True) + _check_shape(S_s, S, n, m) # See if we should solve this using SciPy if method == 'scipy': @@ -510,8 +511,9 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None): # the gain matrix G return _ssmatrix(X), L, _ssmatrix(G) -def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): - """(X, L, G) = dare(A, B, Q, R) solves the discrete-time algebraic Riccati +def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, + A_s="A", B_s="B", Q_s="Q", R_s="R", S_s="S", E_s="E"): + """X, L, G = dare(A, B, Q, R) solves the discrete-time algebraic Riccati equation :math:`A^T X A - X - A^T X B (B^T X B + R)^{-1} B^T X A + Q = 0` @@ -521,16 +523,17 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): matrix G = (B^T X B + R)^-1 B^T X A and the closed loop eigenvalues L, i.e., the eigenvalues of A - B G. - (X, L, G) = dare(A, B, Q, R, S, E) solves the generalized discrete-time + X, L, G = dare(A, B, Q, R, S, E) solves the generalized discrete-time algebraic Riccati equation :math:`A^T X A - E^T X E - (A^T X B + S) (B^T X B + R)^{-1} (B^T X A + S^T) + Q = 0` - where A, Q and E are square matrices of the same dimension. Further, Q and - R are symmetric matrices. If R is None, it is set to the identity - matrix. The function returns the solution X, the gain - matrix :math:`G = (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop - eigenvalues L, i.e., the eigenvalues of A - B G , E. + where A, Q and E are square matrices of the same dimension. Further, Q + and R are symmetric matrices. If R is None, it is set to the identity + matrix. The function returns the solution X, the gain matrix :math:`G = + (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop eigenvalues L, + i.e., the (generalized) eigenvalues of A - B G (with respect to E, if + specified). Parameters ---------- @@ -576,7 +579,14 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): m = B.shape[1] # Check to make sure input matrices are the right shape and type - _check_shape("A", A, n, n, square=True) + _check_shape(A_s, A, n, n, square=True) + _check_shape(B_s, B, n, m) + _check_shape(Q_s, Q, n, n, square=True, symmetric=True) + _check_shape(R_s, R, m, m, square=True, symmetric=True) + if E is not None: + _check_shape(E_s, E, n, n, square=True) + if S is not None: + _check_shape(S_s, S, n, m) # Figure out how to solve the problem if method == 'scipy' and not stabilizing: @@ -587,21 +597,11 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): return _dare_slycot(A, B, Q, R, S, E, stabilizing) else: - _check_shape("B", B, n, m) - _check_shape("Q", Q, n, n, square=True, symmetric=True) - _check_shape("R", R, m, m, square=True, symmetric=True) - if E is not None: - _check_shape("E", E, n, n, square=True) - if S is not None: - _check_shape("S", S, n, m) - - Rmat = _ssmatrix(R) - Qmat = _ssmatrix(Q) - X = sp.linalg.solve_discrete_are(A, B, Qmat, Rmat, e=E, s=S) + X = sp.linalg.solve_discrete_are(A, B, Q, R, e=E, s=S) if S is None: - G = solve(B.T @ X @ B + Rmat, B.T @ X @ A) + G = solve(B.T @ X @ B + R, B.T @ X @ A) else: - G = solve(B.T @ X @ B + Rmat, B.T @ X @ A + S.T) + G = solve(B.T @ X @ B + R, B.T @ X @ A + S.T) if E is None: L = eigvals(A - B @ G) else: @@ -611,7 +611,7 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None): def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): - # Make sure we can import required slycot routine + # Make sure we can import required slycot routines try: from slycot import sb02md except ImportError: @@ -622,18 +622,11 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): except ImportError: raise ControlSlycot("Can't find slycot module 'sb02mt'") - # Make sure we can find the required slycot routine try: from slycot import sg02ad except ImportError: raise ControlSlycot("Can't find slycot module 'sg02ad'") - # Reshape input arrays - A = np.array(A, ndmin=2) - B = np.array(B, ndmin=2) - Q = np.array(Q, ndmin=2) - R = np.eye(B.shape[1]) if R is None else np.array(R, ndmin=2) - # Determine main dimensions n = A.shape[0] m = B.shape[1] @@ -642,21 +635,6 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): S = np.zeros((n, m)) if S is None else np.array(S, ndmin=2) E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2) - # Check to make sure input matrices are the right shape and type - _check_shape("A", A, n, n, square=True) - _check_shape("B", B, n, m) - _check_shape("Q", Q, n, n, square=True, symmetric=True) - _check_shape("R", R, m, m, square=True, symmetric=True) - _check_shape("E", E, n, n, square=True) - _check_shape("S", S, n, m) - - # Create back-up of arrays needed for later computations - A_b = copy(A) - R_b = copy(R) - B_b = copy(B) - E_b = copy(E) - S_b = copy(S) - # Solve the generalized algebraic Riccati equation by calling the # Slycot function sg02ad sort = 'S' if stabilizing else 'U' @@ -670,7 +648,7 @@ def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): L = np.array([(alfar[i] + alfai[i]*1j) / beta[i] for i in range(n)]) # Calculate the gain matrix G - G = solve(B_b.T @ X @ B_b + R_b, B_b.T @ X @ A_b + S_b.T) + G = solve(B.T @ X @ B + R, B.T @ X @ A + S.T) # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G diff --git a/control/statefbk.py b/control/statefbk.py index eb52cb123..9840d175e 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -43,7 +43,7 @@ import numpy as np from . import statesp -from .mateqn import care, dare +from .mateqn import care, dare, _check_shape from .statesp import _ssmatrix, _convert_to_statespace from .lti import LTI, isdtime, isctime from .exception import ControlSlycot, ControlArgument, ControlDimension, \ @@ -260,7 +260,7 @@ def place_varga(A, B, p, dtime=False, alpha=None): # contributed by Sawyer B. Fuller def lqe(*args, **keywords): - """lqe(A, G, C, Q, R, [, N]) + """lqe(A, G, C, QN, RN, [, NN]) Linear quadratic estimator design (Kalman filter) for continuous-time systems. Given the system @@ -272,26 +272,26 @@ def lqe(*args, **keywords): with unbiased process noise w and measurement noise v with covariances - .. math:: E{ww'} = Q, E{vv'} = R, E{wv'} = N + .. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN The lqe() function computes the observer gain matrix L such that the stationary (non-time-varying) Kalman filter - .. math:: x_e = A x_e + B u + L(y - C x_e - D u) + .. math:: x_e = A x_e + G u + L(y - C x_e - D u) produces a state estimate x_e that minimizes the expected squared error - using the sensor measurements y. The noise cross-correlation `N` is + using the sensor measurements y. The noise cross-correlation `NN` is set to zero when omitted. The function can be called with either 3, 4, 5, or 6 arguments: - * ``L, P, E = lqe(sys, Q, R)`` - * ``L, P, E = lqe(sys, Q, R, N)`` - * ``L, P, E = lqe(A, G, C, Q, R)`` - * ``L, P, E = lqe(A, B, C, Q, R, N)`` + * ``L, P, E = lqe(sys, QN, RN)`` + * ``L, P, E = lqe(sys, QN, RN, NN)`` + * ``L, P, E = lqe(A, G, C, QN, RN)`` + * ``L, P, E = lqe(A, G, C, QN, RN, NN)`` - where `sys` is an `LTI` object, and `A`, `G`, `C`, `Q`, `R`, and `N` are - 2D arrays or matrices of appropriate dimension. + where `sys` is an `LTI` object, and `A`, `G`, `C`, `QN`, `RN`, and `NN` + are 2D arrays or matrices of appropriate dimension. Parameters ---------- @@ -300,9 +300,9 @@ def lqe(*args, **keywords): sys : LTI (StateSpace or TransferFunction) Linear I/O system, with the process noise input taken as the system input. - Q, R : 2D array_like + QN, RN : 2D array_like Process and sensor noise covariance matrices - N : 2D array, optional + NN : 2D array, optional Cross covariance matrix. Not currently implemented. method : str, optional Set the method used for computing the result. Current methods are @@ -335,8 +335,8 @@ def lqe(*args, **keywords): Examples -------- - >>> L, P, E = lqe(A, G, C, Q, R) - >>> L, P, E = lqe(A, G, C, Q, R, N) + >>> L, P, E = lqe(A, G, C, QN, RN) + >>> L, P, E = lqe(A, G, C, Q, RN, NN) See Also -------- @@ -386,31 +386,24 @@ def lqe(*args, **keywords): index = 3 # Get the weighting matrices (converting to matrices, if needed) - Q = np.array(args[index], ndmin=2, dtype=float) - R = np.array(args[index+1], ndmin=2, dtype=float) + QN = np.array(args[index], ndmin=2, dtype=float) + RN = np.array(args[index+1], ndmin=2, dtype=float) # Get the cross-covariance matrix, if given if (len(args) > index + 2): - N = np.array(args[index+2], ndmin=2, dtype=float) + NN = np.array(args[index+2], ndmin=2, dtype=float) raise ControlNotImplemented("cross-covariance not implemented") else: - N = np.zeros((Q.shape[0], R.shape[1])) - - # Check dimensions for consistency - nstates = A.shape[0] - ninputs = G.shape[1] - noutputs = C.shape[0] - if (A.shape[0] != nstates or A.shape[1] != nstates or - G.shape[0] != nstates or C.shape[1] != nstates): - raise ControlDimension("inconsistent system dimensions") + # For future use (not currently used below) + NN = np.zeros((QN.shape[0], RN.shape[1])) - elif (Q.shape[0] != ninputs or Q.shape[1] != ninputs or - R.shape[0] != noutputs or R.shape[1] != noutputs or - N.shape[0] != ninputs or N.shape[1] != noutputs): - raise ControlDimension("incorrect covariance matrix dimensions") + # Check dimensions of G (needed before calling care()) + _check_shape("QN", QN, G.shape[1], G.shape[1]) - P, E, LT = care(A.T, C.T, G @ Q @ G.T, R, method=method) + # Compute the result (dimension and symmetry checking done in care()) + P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN, method=method, + B_s="C", Q_s="QN", R_s="RN", S_s="NN") return _ssmatrix(LT.T), _ssmatrix(P), E @@ -524,7 +517,9 @@ def dlqe(*args, **keywords): NN = np.array(args[index+2], ndmin=2, dtype=float) raise ControlNotImplemented("cross-covariance not yet implememented") - P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN, method=method) + # Compute the result (dimension and symmetry checking done in dare()) + P, E, LT = dare(A.T, C.T, G @ QN @ G.T, RN, method=method, + B_s="C", Q_s="QN", R_s="RN", S_s="NN") return _ssmatrix(LT.T), _ssmatrix(P), E # Contributed by Roberto Bucher @@ -675,8 +670,8 @@ def lqr(*args, **keywords): else: N = None - # Solve continuous algebraic Riccati equation - X, L, G = care(A, B, Q, R, N, None, method=method) + # Compute the result (dimension and symmetry checking done in care()) + X, L, G = care(A, B, Q, R, N, None, method=method, S_s="N") return G, X, L @@ -771,19 +766,8 @@ def dlqr(*args, **keywords): else: N = np.zeros((Q.shape[0], R.shape[1])) - # Check dimensions for consistency - nstates = B.shape[0] - ninputs = B.shape[1] - if (A.shape[0] != nstates or A.shape[1] != nstates): - raise ControlDimension("inconsistent system dimensions") - - elif (Q.shape[0] != nstates or Q.shape[1] != nstates or - R.shape[0] != ninputs or R.shape[1] != ninputs or - N.shape[0] != nstates or N.shape[1] != ninputs): - raise ControlDimension("incorrect weighting matrix dimensions") - - # compute the result - S, E, K = dare(A, B, Q, R, N, method=method) + # Compute the result (dimension and symmetry checking done in dare()) + S, E, K = dare(A, B, Q, R, N, method=method, S_s="N") return _ssmatrix(K), _ssmatrix(S), E diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 738f068fb..458e30d4d 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -472,11 +472,11 @@ def test_lqe_call_format(self): L, P, E = lqe(sys, Q, R, N) # Inconsistent system dimensions - with pytest.raises(ct.ControlDimension, match="inconsistent system"): + with pytest.raises(ct.ControlDimension, match="Incompatible"): L, P, E = lqe(sys.A, sys.C, sys.B, Q, R) # incorrect covariance matrix dimensions - with pytest.raises(ct.ControlDimension, match="incorrect covariance"): + with pytest.raises(ct.ControlDimension, match="Incompatible"): L, P, E = lqe(sys.A, sys.B, sys.C, R, Q) def check_DLQE(self, L, P, poles, G, QN, RN): From f1f5375978e4c4795926e6f72c9882ef57087906 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 26 Dec 2021 16:56:45 -0800 Subject: [PATCH 13/15] remove _dare_slycot and make slycot/scipy structure moer uniform --- control/mateqn.py | 79 ++++++++++++++++------------------------------- 1 file changed, 26 insertions(+), 53 deletions(-) diff --git a/control/mateqn.py b/control/mateqn.py index 6493f537f..23ae1e64e 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -3,7 +3,7 @@ # Implementation of the functions lyap, dlyap, care and dare # for solution of Lyapunov and Riccati equations. # -# Author: Bjorn Olofsson +# Original author: Bjorn Olofsson # Copyright (c) 2011, All rights reserved. @@ -162,6 +162,7 @@ def lyap(A, Q, C=None, E=None, method=None): _check_shape("Q", Q, n, n, square=True, symmetric=True) if method == 'scipy': + # Solve the Lyapunov equation using SciPy return sp.linalg.solve_continuous_lyapunov(A, -Q) # Solve the Lyapunov equation by calling Slycot function sb03md @@ -177,6 +178,7 @@ def lyap(A, Q, C=None, E=None, method=None): _check_shape("C", C, n, m) if method == 'scipy': + # Solve the Sylvester equation using SciPy return sp.linalg.solve_sylvester(A, Q, -C) # Solve the Sylvester equation by calling the Slycot function sb04md @@ -293,6 +295,7 @@ def dlyap(A, Q, C=None, E=None, method=None): _check_shape("Q", Q, n, n, square=True, symmetric=True) if method == 'scipy': + # Solve the Lyapunov equation using SciPy return sp.linalg.solve_discrete_lyapunov(A, Q) # Solve the Lyapunov equation by calling the Slycot function sb03md @@ -396,24 +399,6 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, # Decide what method to use method = _slycot_or_scipy(method) - if method == 'slycot': - # Make sure we can import required slycot routines - try: - from slycot import sb02md - except ImportError: - raise ControlSlycot("Can't find slycot module 'sb02md'") - - try: - from slycot import sb02mt - except ImportError: - raise ControlSlycot("Can't find slycot module 'sb02mt'") - - # Make sure we can find the required slycot routine - try: - from slycot import sg02ad - except ImportError: - raise ControlSlycot("Can't find slycot module 'sg02ad'") - # Reshape input arrays A = np.array(A, ndmin=2) B = np.array(B, ndmin=2) @@ -447,9 +432,16 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, E, _ = np.linalg.eig(A - B @ K) return _ssmatrix(X), E, _ssmatrix(K) - # Create back-up of arrays needed for later computations - R_ba = copy(R) - B_ba = copy(B) + # Make sure we can import required slycot routines + try: + from slycot import sb02md + except ImportError: + raise ControlSlycot("Can't find slycot module 'sb02md'") + + try: + from slycot import sb02mt + except ImportError: + raise ControlSlycot("Can't find slycot module 'sb02mt'") # Solve the standard algebraic Riccati equation by calling Slycot # functions sb02mt and sb02md @@ -459,7 +451,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, X, rcond, w, S_o, U, A_inv = sb02md(n, A, G, Q, 'C', sort=sort) # Calculate the gain matrix G - G = solve(R_ba, B_ba.T) @ X + G = solve(R, B.T) @ X # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G @@ -486,11 +478,11 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, eigs, _ = sp.linalg.eig(A - B @ K, E) return _ssmatrix(X), eigs, _ssmatrix(K) - # Create back-up of arrays needed for later computations - R_b = copy(R) - B_b = copy(B) - E_b = copy(E) - S_b = copy(S) + # Make sure we can find the required slycot routine + try: + from slycot import sg02ad + except ImportError: + raise ControlSlycot("Can't find slycot module 'sg02ad'") # Solve the generalized algebraic Riccati equation by calling the # Slycot function sg02ad @@ -505,7 +497,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, L = np.array([(alfar[i] + alfai[i]*1j) / beta[i] for i in range(n)]) # Calculate the gain matrix G - G = solve(R_b, B_b.T @ X @ E_b + S_b.T) + G = solve(R, B.T @ X @ E + S.T) # Return the solution X, the closed-loop eigenvalues L and # the gain matrix G @@ -589,14 +581,11 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, _check_shape(S_s, S, n, m) # Figure out how to solve the problem - if method == 'scipy' and not stabilizing: - raise ControlArgument( - "method='scipy' not valid when stabilizing is not True") - - elif method == 'slycot': - return _dare_slycot(A, B, Q, R, S, E, stabilizing) + if method == 'scipy': + if not stabilizing: + raise ControlArgument( + "method='scipy' not valid when stabilizing is not True") - else: X = sp.linalg.solve_discrete_are(A, B, Q, R, e=E, s=S) if S is None: G = solve(B.T @ X @ B + R, B.T @ X @ A) @@ -609,28 +598,12 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, return _ssmatrix(X), L, _ssmatrix(G) - -def _dare_slycot(A, B, Q, R, S=None, E=None, stabilizing=True): - # Make sure we can import required slycot routines - try: - from slycot import sb02md - except ImportError: - raise ControlSlycot("Can't find slycot module 'sb02md'") - - try: - from slycot import sb02mt - except ImportError: - raise ControlSlycot("Can't find slycot module 'sb02mt'") - + # Make sure we can import required slycot routine try: from slycot import sg02ad except ImportError: raise ControlSlycot("Can't find slycot module 'sg02ad'") - # Determine main dimensions - n = A.shape[0] - m = B.shape[1] - # Initialize optional matrices S = np.zeros((n, m)) if S is None else np.array(S, ndmin=2) E = np.eye(A.shape[0]) if E is None else np.array(E, ndmin=2) From fdd399f6cacf1eb427f8fcb023d3c4946c85bbe9 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 26 Dec 2021 18:22:55 -0800 Subject: [PATCH 14/15] require StateSpace for lqr/lqe first arg + process uniformly --- control/statefbk.py | 62 +++++++++++++++++++------------ control/tests/statefbk_test.py | 68 ++++++++++++++++++++++------------ 2 files changed, 82 insertions(+), 48 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 9840d175e..b1c6db5bd 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -44,7 +44,7 @@ from . import statesp from .mateqn import care, dare, _check_shape -from .statesp import _ssmatrix, _convert_to_statespace +from .statesp import StateSpace, _ssmatrix, _convert_to_statespace from .lti import LTI, isdtime, isctime from .exception import ControlSlycot, ControlArgument, ControlDimension, \ ControlNotImplemented @@ -366,19 +366,18 @@ def lqe(*args, **keywords): # Call dlqe return dlqe(*args, **keywords) - try: - sys = args[0] # Treat the first argument as a system - if isinstance(sys, LTI): - # Convert LTI system to state space - sys = _convert_to_statespace(sys) - - # Extract A, G (assume disturbances come through input), and C - A = np.array(sys.A, ndmin=2, dtype=float) - G = np.array(sys.B, ndmin=2, dtype=float) - C = np.array(sys.C, ndmin=2, dtype=float) + # If we were passed a state space system, use that to get system matrices + if isinstance(args[0], StateSpace): + A = np.array(args[0].A, ndmin=2, dtype=float) + G = np.array(args[0].B, ndmin=2, dtype=float) + C = np.array(args[0].C, ndmin=2, dtype=float) index = 1 - except AttributeError: + elif isinstance(args[0], LTI): + # Don't allow other types of LTI systems + raise ControlArgument("LTI system must be in state space form") + + else: # Arguments should be A and B matrices A = np.array(args[0], ndmin=2, dtype=float) G = np.array(args[1], ndmin=2, dtype=float) @@ -490,14 +489,18 @@ def dlqe(*args, **keywords): if isinstance(args[0], LTI) and isctime(args[0], strict=True): raise ControlArgument("dlqr() called with a continuous time system") - try: - # If this works, we were (probably) passed a system as the - # first argument; extract A and B + # If we were passed a state space system, use that to get system matrices + if isinstance(args[0], StateSpace): A = np.array(args[0].A, ndmin=2, dtype=float) G = np.array(args[0].B, ndmin=2, dtype=float) C = np.array(args[0].C, ndmin=2, dtype=float) index = 1 - except AttributeError: + + elif isinstance(args[0], LTI): + # Don't allow other types of LTI systems + raise ControlArgument("LTI system must be in state space form") + + else: # Arguments should be A and B matrices A = np.array(args[0], ndmin=2, dtype=float) G = np.array(args[1], ndmin=2, dtype=float) @@ -517,6 +520,9 @@ def dlqe(*args, **keywords): NN = np.array(args[index+2], ndmin=2, dtype=float) raise ControlNotImplemented("cross-covariance not yet implememented") + # Check dimensions of G (needed before calling care()) + _check_shape("QN", QN, G.shape[1], G.shape[1]) + # Compute the result (dimension and symmetry checking done in dare()) P, E, LT = dare(A.T, C.T, G @ QN @ G.T, RN, method=method, B_s="C", Q_s="QN", R_s="RN", S_s="NN") @@ -650,13 +656,17 @@ def lqr(*args, **keywords): # Call dlqr return dlqr(*args, **keywords) - try: - # If this works, we were (probably) passed a system as the - # first argument; extract A and B + # If we were passed a state space system, use that to get system matrices + if isinstance(args[0], StateSpace): A = np.array(args[0].A, ndmin=2, dtype=float) B = np.array(args[0].B, ndmin=2, dtype=float) index = 1 - except AttributeError: + + elif isinstance(args[0], LTI): + # Don't allow other types of LTI systems + raise ControlArgument("LTI system must be in state space form") + + else: # Arguments should be A and B matrices A = np.array(args[0], ndmin=2, dtype=float) B = np.array(args[1], ndmin=2, dtype=float) @@ -746,13 +756,17 @@ def dlqr(*args, **keywords): if isinstance(args[0], LTI) and isctime(args[0], strict=True): raise ControlArgument("dsys must be discrete time (dt != 0)") - try: - # If this works, we were (probably) passed a system as the - # first argument; extract A and B + # If we were passed a state space system, use that to get system matrices + if isinstance(args[0], StateSpace): A = np.array(args[0].A, ndmin=2, dtype=float) B = np.array(args[0].B, ndmin=2, dtype=float) index = 1 - except AttributeError: + + elif isinstance(args[0], LTI): + # Don't allow other types of LTI systems + raise ControlArgument("LTI system must be in state space form") + + else: # Arguments should be A and B matrices A = np.array(args[0], ndmin=2, dtype=float) B = np.array(args[1], ndmin=2, dtype=float) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 458e30d4d..73410312f 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -345,16 +345,18 @@ def test_DLQR_4args(self, matarrayin, matarrayout): K, S, poles = dlqr(A, B, Q, R) self.check_DLQR(K, S, poles, Q, R) - def test_lqr_badmethod(self): + @pytest.mark.parametrize("cdlqr", [lqr, dlqr]) + def test_lqr_badmethod(self, cdlqr): A, B, Q, R = 0, 1, 10, 2 with pytest.raises(ControlArgument, match="Unknown method"): - K, S, poles = lqr(A, B, Q, R, method='nosuchmethod') + K, S, poles = cdlqr(A, B, Q, R, method='nosuchmethod') - def test_lqr_slycot_not_installed(self): + @pytest.mark.parametrize("cdlqr", [lqr, dlqr]) + def test_lqr_slycot_not_installed(self, cdlqr): A, B, Q, R = 0, 1, 10, 2 if not slycot_check(): with pytest.raises(ControlSlycot, match="Can't find slycot"): - K, S, poles = lqr(A, B, Q, R, method='slycot') + K, S, poles = cdlqr(A, B, Q, R, method='slycot') @pytest.mark.xfail(reason="warning not implemented") def testLQR_warning(self): @@ -375,9 +377,11 @@ def testLQR_warning(self): with pytest.warns(UserWarning): (K, S, E) = lqr(A, B, Q, R, N) - def test_lqr_call_format(self): + @pytest.mark.parametrize("cdlqr", [lqr, dlqr]) + def test_lqr_call_format(self, cdlqr): # Create a random state space system for testing sys = rss(2, 3, 2) + sys.dt = None # treat as either continuous or discrete time # Weighting matrices Q = np.eye(sys.nstates) @@ -385,27 +389,37 @@ def test_lqr_call_format(self): N = np.zeros((sys.nstates, sys.ninputs)) # Standard calling format - Kref, Sref, Eref = lqr(sys.A, sys.B, Q, R) + Kref, Sref, Eref = cdlqr(sys.A, sys.B, Q, R) # Call with system instead of matricees - K, S, E = lqr(sys, Q, R) + K, S, E = cdlqr(sys, Q, R) np.testing.assert_array_almost_equal(Kref, K) np.testing.assert_array_almost_equal(Sref, S) np.testing.assert_array_almost_equal(Eref, E) # Pass a cross-weighting matrix - K, S, E = lqr(sys, Q, R, N) + K, S, E = cdlqr(sys, Q, R, N) np.testing.assert_array_almost_equal(Kref, K) np.testing.assert_array_almost_equal(Sref, S) np.testing.assert_array_almost_equal(Eref, E) # Inconsistent system dimensions with pytest.raises(ct.ControlDimension, match="Incompatible dimen"): - K, S, E = lqr(sys.A, sys.C, Q, R) + K, S, E = cdlqr(sys.A, sys.C, Q, R) - # incorrect covariance matrix dimensions + # Incorrect covariance matrix dimensions with pytest.raises(ct.ControlDimension, match="Q must be a square"): - K, S, E = lqr(sys.A, sys.B, sys.C, R, Q) + K, S, E = cdlqr(sys.A, sys.B, sys.C, R, Q) + + # Too few input arguments + with pytest.raises(ct.ControlArgument, match="not enough input"): + K, S, E = cdlqr(sys.A, sys.B) + + # First argument is the wrong type (use SISO for non-slycot tests) + sys_tf = tf(rss(3, 1, 1)) + sys_tf.dt = None # treat as either continuous or discrete time + with pytest.raises(ct.ControlArgument, match="LTI system must be"): + K, S, E = cdlqr(sys_tf, Q, R) @pytest.mark.xfail(reason="warning not implemented") def testDLQR_warning(self): @@ -443,9 +457,11 @@ def test_LQE(self, matarrayin, method): L, P, poles = lqe(A, G, C, QN, RN, method=method) self.check_LQE(L, P, poles, G, QN, RN) - def test_lqe_call_format(self): + @pytest.mark.parametrize("cdlqe", [lqe, dlqe]) + def test_lqe_call_format(self, cdlqe): # Create a random state space system for testing sys = rss(4, 3, 2) + sys.dt = None # treat as either continuous or discrete time # Covariance matrices Q = np.eye(sys.ninputs) @@ -453,31 +469,35 @@ def test_lqe_call_format(self): N = np.zeros((sys.ninputs, sys.noutputs)) # Standard calling format - Lref, Pref, Eref = lqe(sys.A, sys.B, sys.C, Q, R) + Lref, Pref, Eref = cdlqe(sys.A, sys.B, sys.C, Q, R) # Call with system instead of matricees - L, P, E = lqe(sys, Q, R) + L, P, E = cdlqe(sys, Q, R) np.testing.assert_array_almost_equal(Lref, L) np.testing.assert_array_almost_equal(Pref, P) np.testing.assert_array_almost_equal(Eref, E) - # Compare state space and transfer function (SISO only) - sys_siso = rss(4, 1, 1) - L_ss, P_ss, E_ss = lqe(sys_siso, np.eye(1), np.eye(1)) - L_tf, P_tf, E_tf = lqe(tf(sys_siso), np.eye(1), np.eye(1)) - np.testing.assert_array_almost_equal(np.sort(E_ss), np.sort(E_tf)) - # Make sure we get an error if we specify N with pytest.raises(ct.ControlNotImplemented): - L, P, E = lqe(sys, Q, R, N) + L, P, E = cdlqe(sys, Q, R, N) # Inconsistent system dimensions with pytest.raises(ct.ControlDimension, match="Incompatible"): - L, P, E = lqe(sys.A, sys.C, sys.B, Q, R) + L, P, E = cdlqe(sys.A, sys.C, sys.B, Q, R) - # incorrect covariance matrix dimensions + # Incorrect covariance matrix dimensions with pytest.raises(ct.ControlDimension, match="Incompatible"): - L, P, E = lqe(sys.A, sys.B, sys.C, R, Q) + L, P, E = cdlqe(sys.A, sys.B, sys.C, R, Q) + + # Too few input arguments + with pytest.raises(ct.ControlArgument, match="not enough input"): + L, P, E = cdlqe(sys.A, sys.C) + + # First argument is the wrong type (use SISO for non-slycot tests) + sys_tf = tf(rss(3, 1, 1)) + sys_tf.dt = None # treat as either continuous or discrete time + with pytest.raises(ct.ControlArgument, match="LTI system must be"): + L, P, E = cdlqe(sys_tf, Q, R) def check_DLQE(self, L, P, poles, G, QN, RN): P_expected = asmatarrayout(G.dot(QN).dot(G)) From 0d4ff4c34112cbcbfdfbe1605dfa6f1a447f588f Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sun, 26 Dec 2021 22:46:24 -0800 Subject: [PATCH 15/15] Fix lqe docstring per @sawyerbfuller --- control/statefbk.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/statefbk.py b/control/statefbk.py index b1c6db5bd..ef16cbfff 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -277,7 +277,7 @@ def lqe(*args, **keywords): The lqe() function computes the observer gain matrix L such that the stationary (non-time-varying) Kalman filter - .. math:: x_e = A x_e + G u + L(y - C x_e - D u) + .. math:: x_e = A x_e + B u + L(y - C x_e - D u) produces a state estimate x_e that minimizes the expected squared error using the sensor measurements y. The noise cross-correlation `NN` is pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy