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 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