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