From 1e55a036994f758d8a5d071ecad86505aeb2b383 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Wed, 24 Nov 2021 22:06:28 -0800 Subject: [PATCH 1/3] update lqe() argument processing to match lqr() --- control/statefbk.py | 98 +++++++++++++++++++++++++++++++++++++-------- 1 file changed, 81 insertions(+), 17 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 7bd9cc409..253ce114b 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -44,7 +44,8 @@ from . import statesp from .mateqn import care -from .statesp import _ssmatrix +from .statesp import _ssmatrix, _convert_to_statespace +from .lti import LTI from .exception import ControlSlycot, ControlArgument, ControlDimension # Make sure we have access to the right slycot routines @@ -257,8 +258,8 @@ def place_varga(A, B, p, dtime=False, alpha=None): # contributed by Sawyer B. Fuller -def lqe(A, G, C, QN, RN, NN=None): - """lqe(A, G, C, QN, RN, [, N]) +def lqe(*args, **keywords): + """lqe(A, G, C, Q, R, [, N]) Linear quadratic estimator design (Kalman filter) for continuous-time systems. Given the system @@ -270,7 +271,7 @@ def lqe(A, G, C, QN, RN, NN=None): with unbiased process noise w and measurement noise v with covariances - .. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN + .. math:: E{ww'} = Q, E{vv'} = R, E{wv'} = N The lqe() function computes the observer gain matrix L such that the stationary (non-time-varying) Kalman filter @@ -278,17 +279,30 @@ def lqe(A, G, C, QN, RN, NN=None): .. 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 + using the sensor measurements y. The noise cross-correlation `N` is set to zero when omitted. + The function can be called with either 3, 4, 5, or 6 arguments: + + * ``lqe(sys, Q, R)`` + * ``lqe(sys, Q, R, N)`` + * ``lqe(A, G, C, Q, R)`` + * ``lqe(A, B, C, Q, R, N)`` + + where `sys` is an `LTI` object, and `A`, `G`, `C`, `Q`, `R`, and `N` are + 2D arrays or matrices of appropriate dimension. + Parameters ---------- - A, G : 2D array_like - Dynamics and noise input matrices - QN, RN : 2D array_like + A, G, C : 2D array_like + Dynamics, process noise (disturbance), and output matrices + sys : LTI (StateSpace or TransferFunction) + Linear I/O system, with the process noise input taken as the system + input. + Q, R : 2D array_like Process and sensor noise covariance matrices - NN : 2D array, optional - Cross covariance matrix + N : 2D array, optional + Cross covariance matrix. Not currently implemented. Returns ------- @@ -326,11 +340,61 @@ def lqe(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) - P, E, LT = care(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN) + # + # 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: + 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) + 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) + Q = np.array(args[index], ndmin=2, dtype=float) + R = 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) + 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") + + 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 weighting matrix dimensions") + + # LT, P, E = lqr(A.T, C.T, G @ Q @ G.T, R) + # P, E, LT = care(A.T, C.T, G @ Q @ G.T, R) + P, E, LT = care(A.T, C.T, np.dot(np.dot(G, Q), G.T), R) return _ssmatrix(LT.T), _ssmatrix(P), E @@ -400,11 +464,11 @@ def lqr(*args, **keywords): * ``lqr(A, B, Q, R, N)`` where `sys` is an `LTI` object, and `A`, `B`, `Q`, `R`, and `N` are - 2d arrays or matrices of appropriate dimension. + 2D arrays or matrices of appropriate dimension. Parameters ---------- - A, B : 2D array + A, B : 2D array_like Dynamics and input matrices sys : LTI (StateSpace or TransferFunction) Linear I/O system From 06aa0d8b64753261101b9a1ee33581c62620eddd Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Thu, 25 Nov 2021 13:45:09 -0800 Subject: [PATCH 2/3] updated docstrings + unit tests --- control/statefbk.py | 21 +++++----- control/tests/statefbk_test.py | 71 ++++++++++++++++++++++++++++++++++ 2 files changed, 82 insertions(+), 10 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index 253ce114b..381563005 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -46,7 +46,8 @@ from .mateqn import care from .statesp import _ssmatrix, _convert_to_statespace from .lti import LTI -from .exception import ControlSlycot, ControlArgument, ControlDimension +from .exception import ControlSlycot, ControlArgument, ControlDimension, \ + ControlNotImplemented # Make sure we have access to the right slycot routines try: @@ -284,10 +285,10 @@ def lqe(*args, **keywords): The function can be called with either 3, 4, 5, or 6 arguments: - * ``lqe(sys, Q, R)`` - * ``lqe(sys, Q, R, N)`` - * ``lqe(A, G, C, Q, R)`` - * ``lqe(A, B, C, Q, R, N)`` + * ``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)`` where `sys` is an `LTI` object, and `A`, `G`, `C`, `Q`, `R`, and `N` are 2D arrays or matrices of appropriate dimension. @@ -390,7 +391,7 @@ def lqe(*args, **keywords): 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 weighting matrix dimensions") + raise ControlDimension("incorrect covariance matrix dimensions") # LT, P, E = lqr(A.T, C.T, G @ Q @ G.T, R) # P, E, LT = care(A.T, C.T, G @ Q @ G.T, R) @@ -458,10 +459,10 @@ def lqr(*args, **keywords): The function can be called with either 3, 4, or 5 arguments: - * ``lqr(sys, Q, R)`` - * ``lqr(sys, Q, R, N)`` - * ``lqr(A, B, Q, R)`` - * ``lqr(A, B, Q, R, N)`` + * ``K, S, E = lqr(sys, Q, R)`` + * ``K, S, E = lqr(sys, Q, R, N)`` + * ``K, S, E = lqr(A, B, Q, R)`` + * ``K, S, E = lqr(A, B, Q, R, N)`` where `sys` is an `LTI` object, and `A`, `B`, `Q`, `R`, and `N` are 2D arrays or matrices of appropriate dimension. diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 1dca98659..0f73d787c 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -6,6 +6,7 @@ import numpy as np import pytest +import control as ct from control import lqe, pole, rss, ss, tf from control.exception import ControlDimension from control.mateqn import care, dare @@ -338,6 +339,39 @@ 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) + + # Weighting matrices + Q = np.eye(sys.nstates) + R = np.eye(sys.ninputs) + N = np.zeros((sys.nstates, sys.ninputs)) + + # Standard calling format + Kref, Sref, Eref = lqr(sys.A, sys.B, Q, R) + + # Call with system instead of matricees + K, S, E = lqr(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) + 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="inconsistent system"): + K, S, E = lqr(sys.A, sys.C, Q, R) + + # incorrect covariance matrix dimensions + with pytest.raises(ct.ControlDimension, match="incorrect weighting"): + K, S, E = lqr(sys.A, sys.B, sys.C, R, Q) + def check_LQE(self, L, P, poles, G, QN, RN): P_expected = asmatarrayout(np.sqrt(G.dot(QN.dot(G).dot(RN)))) L_expected = asmatarrayout(P_expected / RN) @@ -352,6 +386,43 @@ def test_LQE(self, matarrayin): L, P, poles = lqe(A, G, C, QN, RN) self.check_LQE(L, P, poles, G, QN, RN) + @slycotonly + def test_lqe_call_format(self): + # Create a random state space system for testing + sys = rss(4, 3, 2) + + # Covariance matrices + Q = np.eye(sys.ninputs) + R = np.eye(sys.noutputs) + N = np.zeros((sys.ninputs, sys.noutputs)) + + # Standard calling format + Lref, Pref, Eref = lqe(sys.A, sys.B, sys.C, Q, R) + + # Call with system instead of matricees + L, P, E = lqe(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(E_ss, 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) + + # Inconsistent system dimensions + with pytest.raises(ct.ControlDimension, match="inconsistent system"): + L, P, E = lqe(sys.A, sys.C, sys.B, Q, R) + + # incorrect covariance matrix dimensions + with pytest.raises(ct.ControlDimension, match="incorrect covariance"): + L, P, E = lqe(sys.A, sys.B, sys.C, R, Q) + @slycotonly def test_care(self, matarrayin): """Test stabilizing and anti-stabilizing feedbacks, continuous""" From 31729c818f600e8b3bd4893796fc6f24e8a8ce70 Mon Sep 17 00:00:00 2001 From: Richard Murray Date: Sat, 27 Nov 2021 10:37:10 -0800 Subject: [PATCH 3/3] remove comment per @sawyerbfuller --- control/statefbk.py | 1 - 1 file changed, 1 deletion(-) diff --git a/control/statefbk.py b/control/statefbk.py index 381563005..58d19f0ea 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -393,7 +393,6 @@ def lqe(*args, **keywords): N.shape[0] != ninputs or N.shape[1] != noutputs): raise ControlDimension("incorrect covariance matrix dimensions") - # LT, P, E = lqr(A.T, C.T, G @ Q @ G.T, R) # P, E, LT = care(A.T, C.T, G @ Q @ G.T, R) P, E, LT = care(A.T, C.T, np.dot(np.dot(G, Q), G.T), R) return _ssmatrix(LT.T), _ssmatrix(P), E