diff --git a/control/mateqn.py b/control/mateqn.py index b23e42def..1034d7d5c 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -14,7 +14,7 @@ import numpy as np import scipy as sp -from numpy import eye, finfo, inexact +from numpy import eye, finfo from scipy.linalg import eigvals, solve from .exception import ControlArgument, ControlDimension, ControlSlycot, \ @@ -81,7 +81,7 @@ def _warn_ill_conditioned_E(E): # -def lyap(A, Q, C=None, E=None, method=None): +def lyap(A, Q, C=None, E=None, method=None, symmetric_kwargs=None): """Solves the continuous-time Lyapunov equation. X = lyap(A, Q) solves @@ -117,6 +117,9 @@ def lyap(A, Q, C=None, E=None, method=None): 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'. + symmetric_kwargs : dict, optional + Keyword arguments passed to `scipy.linalg.issymmetric` or + `scipy.linalg.ishermitian`. Returns ------- @@ -169,7 +172,7 @@ def lyap(A, Q, C=None, E=None, method=None): # Solve standard Lyapunov equation if C is None and E is None: # Check to make sure input matrices are the right shape and type - _check_shape(Q, n, n, square=True, symmetric=True, name="Q") + _check_shape(Q, n, n, square=True, symmetric=True, name="Q", symmetric_kwargs=symmetric_kwargs) if method == 'scipy': # Solve the Lyapunov equation using SciPy @@ -197,7 +200,7 @@ def lyap(A, Q, C=None, E=None, method=None): # Solve the generalized Lyapunov equation elif C is None and E is not None: # Check to make sure input matrices are the right shape and type - _check_shape(Q, n, n, square=True, symmetric=True, name="Q") + _check_shape(Q, n, n, square=True, symmetric=True, name="Q", symmetric_kwargs=symmetric_kwargs) _check_shape(E, n, n, square=True, name="E") if method == 'scipy': @@ -243,7 +246,7 @@ def lyap(A, Q, C=None, E=None, method=None): return X -def dlyap(A, Q, C=None, E=None, method=None): +def dlyap(A, Q, C=None, E=None, method=None, symmetric_kwargs=None): """Solves the discrete-time Lyapunov equation. X = dlyap(A, Q) solves @@ -279,6 +282,9 @@ def dlyap(A, Q, C=None, E=None, method=None): 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'. + symmetric_kwargs : dict, optional + Keyword arguments passed to `scipy.linalg.issymmetric` or + `scipy.linalg.ishermitian`. Returns ------- @@ -346,7 +352,7 @@ def dlyap(A, Q, C=None, E=None, method=None): # Solve standard Lyapunov equation if C is None and E is None: # Check to make sure input matrices are the right shape and type - _check_shape(Q, n, n, square=True, symmetric=True, name="Q") + _check_shape(Q, n, n, square=True, symmetric=True, name="Q", symmetric_kwargs=symmetric_kwargs) if method == 'scipy': # Solve the Lyapunov equation using SciPy @@ -406,7 +412,7 @@ def dlyap(A, Q, C=None, E=None, method=None): # Solve the generalized Lyapunov equation elif C is None and E is not None: # Check to make sure input matrices are the right shape and type - _check_shape(Q, n, n, square=True, symmetric=True, name="Q") + _check_shape(Q, n, n, square=True, symmetric=True, name="Q", symmetric_kwargs=symmetric_kwargs) _check_shape(E, n, n, square=True, name="E") if method == 'scipy': @@ -449,8 +455,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, - _As="A", _Bs="B", _Qs="Q", _Rs="R", _Ss="S", _Es="E"): +def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, symmetric_kwargs=None, + _As="A", _Bs="B", _Qs="Q", _Rs="R", _Ss="S", _Es="E", ): """Solves the continuous-time algebraic Riccati equation. X, L, G = care(A, B, Q, R=None) solves @@ -484,6 +490,9 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, 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'. + symmetric_kwargs : dict, optional + Keyword arguments passed to `scipy.linalg.issymmetric` or + `scipy.linalg.ishermitian`. stabilizing : bool, optional If `method` is 'slycot', unstabilized eigenvalues will be returned in the initial elements of `L`. Not supported for 'scipy'. @@ -518,8 +527,8 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, # Check to make sure input matrices are the right shape and type _check_shape(A, n, n, square=True, name=_As) _check_shape(B, n, m, name=_Bs) - _check_shape(Q, n, n, square=True, symmetric=True, name=_Qs) - _check_shape(R, m, m, square=True, symmetric=True, name=_Rs) + _check_shape(Q, n, n, square=True, symmetric=True, name=_Qs, symmetric_kwargs=symmetric_kwargs) + _check_shape(R, m, m, square=True, symmetric=True, name=_Rs, symmetric_kwargs=symmetric_kwargs) # Solve the standard algebraic Riccati equation if S is None and E is None: @@ -605,7 +614,7 @@ def care(A, B, Q, R=None, S=None, E=None, stabilizing=True, method=None, # the gain matrix G return X, L, G -def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, +def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, symmetric_kwargs=None, _As="A", _Bs="B", _Qs="Q", _Rs="R", _Ss="S", _Es="E"): """Solves the discrete-time algebraic Riccati equation. @@ -640,6 +649,9 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, 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'. + symmetric_kwargs : dict, optional + Keyword arguments passed to `scipy.linalg.issymmetric` or + `scipy.linalg.ishermitian`. stabilizing : bool, optional If `method` is 'slycot', unstabilized eigenvalues will be returned in the initial elements of `L`. Not supported for 'scipy'. @@ -674,8 +686,8 @@ def dare(A, B, Q, R, S=None, E=None, stabilizing=True, method=None, # Check to make sure input matrices are the right shape and type _check_shape(A, n, n, square=True, name=_As) _check_shape(B, n, m, name=_Bs) - _check_shape(Q, n, n, square=True, symmetric=True, name=_Qs) - _check_shape(R, m, m, square=True, symmetric=True, name=_Rs) + _check_shape(Q, n, n, square=True, symmetric=True, name=_Qs, symmetric_kwargs=symmetric_kwargs) + _check_shape(R, m, m, square=True, symmetric=True, name=_Rs, symmetric_kwargs=symmetric_kwargs) if E is not None: _check_shape(E, n, n, square=True, name=_Es) if S is not None: @@ -740,7 +752,7 @@ def _slycot_or_scipy(method): # Utility function to check matrix dimensions -def _check_shape(M, n, m, square=False, symmetric=False, name="??"): +def _check_shape(M, n, m, square=False, symmetric=False, name="??", symmetric_kwargs=None): """Check the shape and properties of a 2D array. This function can be used to check to make sure a 2D array_like has the @@ -773,7 +785,7 @@ def _check_shape(M, n, m, square=False, symmetric=False, name="??"): if (square or symmetric) and M.shape[0] != M.shape[1]: raise ControlDimension("%s must be a square matrix" % name) - if symmetric and not _is_symmetric(M): + if symmetric and not _is_symmetric(M, symmetric_kwargs=symmetric_kwargs): raise ControlArgument("%s must be a symmetric matrix" % name) if M.shape[0] != n or M.shape[1] != m: @@ -785,10 +797,10 @@ def _check_shape(M, n, m, square=False, symmetric=False, name="??"): # Utility function to check if a matrix is symmetric -def _is_symmetric(M): - M = np.atleast_2d(M) - if isinstance(M[0, 0], inexact): - eps = finfo(M.dtype).eps - return ((M - M.T) < eps).all() - else: - return (M == M.T).all() +def _is_symmetric(M, symmetric_kwargs=None): + if symmetric_kwargs is None: + symmetric_kwargs = {} + + if np.iscomplexobj(M): + return sp.linalg.ishermitian(M, **symmetric_kwargs) + return sp.linalg.issymmetric(M, **symmetric_kwargs) diff --git a/control/stochsys.py b/control/stochsys.py index 22e2f738b..003958f57 100644 --- a/control/stochsys.py +++ b/control/stochsys.py @@ -33,7 +33,7 @@ # contributed by Sawyer B. Fuller -def lqe(*args, **kwargs): +def lqe(*args, symmetric_kwargs=None, **kwargs): r"""lqe(A, G, C, QN, RN, [, NN]) Continuous-time linear quadratic estimator (Kalman filter). @@ -83,6 +83,9 @@ def lqe(*args, **kwargs): 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'. + symmetric_kwargs : dict, optional + Keyword arguments passed to `scipy.linalg.issymmetric` or + `scipy.linalg.ishermitian`. Returns ------- @@ -178,12 +181,12 @@ def lqe(*args, **kwargs): # 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, - _Bs="C", _Qs="QN", _Rs="RN", _Ss="NN") + _Bs="C", _Qs="QN", _Rs="RN", _Ss="NN", symmetric_kwargs=symmetric_kwargs) return LT.T, P, E # contributed by Sawyer B. Fuller -def dlqe(*args, **kwargs): +def dlqe(*args, symmetric_kwargs=None, **kwargs): r"""dlqe(A, G, C, QN, RN, [, N]) Discrete-time linear quadratic estimator (Kalman filter). @@ -220,6 +223,9 @@ def dlqe(*args, **kwargs): 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'. + symmetric_kwargs : dict, optional + Keyword arguments passed to `scipy.linalg.issymmetric` or + `scipy.linalg.ishermitian`. Returns ------- @@ -299,7 +305,7 @@ def dlqe(*args, **kwargs): # 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, - _Bs="C", _Qs="QN", _Rs="RN", _Ss="NN") + _Bs="C", _Qs="QN", _Rs="RN", _Ss="NN", symmetric_kwargs=symmetric_kwargs) return LT.T, P, E @@ -312,7 +318,7 @@ def create_estimator_iosystem( control_indices=None, disturbance_indices=None, estimate_labels='xhat[{i}]', covariance_labels='P[{i},{j}]', measurement_labels=None, control_labels=None, - inputs=None, outputs=None, states=None, **kwargs): + inputs=None, outputs=None, states=None, symmetric_kwargs=None, **kwargs): r"""Create an I/O system implementing a linear quadratic estimator. This function creates an input/output system that implements a @@ -410,6 +416,9 @@ def create_estimator_iosystem( name : string, optional System name (used for specifying signals). If unspecified, a generic name 'sys[id]' is generated with a unique integer id. + symmetric_kwargs : dict, optional + Keyword arguments passed to `scipy.linalg.issymmetric` or + `scipy.linalg.ishermitian`. Notes ----- @@ -492,7 +501,7 @@ def create_estimator_iosystem( if P0 is None: # Initialize P0 to the steady state value _, P0, _ = lqe(A, G, C, QN, RN) - P0 = _check_shape(P0, sys.nstates, sys.nstates, symmetric=True, name='P0') + P0 = _check_shape(P0, sys.nstates, sys.nstates, symmetric=True, name='P0', symmetric_kwargs=symmetric_kwargs) # Figure out the labels to use estimate_labels = _process_labels( diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 5cc34c2ab..d2cf7b2bf 100644 --- a/control/tests/mateqn_test.py +++ b/control/tests/mateqn_test.py @@ -39,7 +39,7 @@ import pytest from scipy.linalg import eigvals, solve -from control.mateqn import lyap, dlyap, care, dare +from control.mateqn import lyap, dlyap, care, dare, _is_symmetric from control.exception import ControlArgument, ControlDimension @@ -466,3 +466,25 @@ def test_raise(self): cdare(A, B, Qfs, R, S, E) with pytest.raises(ControlArgument): cdare(A, B, Q, Rfs, S, E) + + def test_is_symmetric_scale_aware(self): + M = np.array([ + [1e8, 1e8], + [1e8 + 1e-8, 1e8] + ]) + assert not _is_symmetric(M) + assert _is_symmetric(M,symmetric_kwargs={"rtol": 1e-12},) + + def test_is_symmetric_rejects_asymmetric(self): + M = np.array([ + [1., 2.], + [5., 1.] + ]) + assert not _is_symmetric(M) + + def test_is_symmetric_complex_hermitian(self): + M = np.array([ + [1., 2. + 1.j], + [2. - 1.j, 3.] + ]) + assert _is_symmetric(M) diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py index 20e799643..55a0c00b8 100644 --- a/control/tests/stochsys_test.py +++ b/control/tests/stochsys_test.py @@ -35,6 +35,54 @@ def test_LQE(method): L, P, poles = lqe(A, G, C, QN, RN, method=method) check_LQE(L, P, poles, G, QN, RN) +def test_lqe_symmetric_kwargs(): + A = np.array([ + [-1., 0.], + [0., -2.] + ]) + G = np.eye(2) + C = np.eye(2) + QN = np.array([ + [1., 0.2], + [0.2 + 1e-15, 1.] + ]) + RN = np.eye(2) + + # Exact symmetry check should fail + with pytest.raises(ControlArgument, match="symmetric"): + lqe(A, G, C, QN, RN, method="scipy") + + # Passing rtol should allow the nearly symmetric matrix + lqe( + A, G, C, QN, RN, + method="scipy", + symmetric_kwargs={"rtol": 1e-12}, + ) + +def test_dlqe_symmetric_kwargs(): + A = np.array([ + [0.5, 0.], + [0., 0.4] + ]) + G = np.eye(2) + C = np.eye(2) + QN = np.array([ + [1., 0.2], + [0.2 + 1e-15, 1.] + ]) + RN = np.eye(2) + + # Exact symmetry check should fail + with pytest.raises(ControlArgument, match="symmetric"): + dlqe(A, G, C, QN, RN, method="scipy") + + # Passing rtol should allow the nearly symmetric matrix + dlqe( + A, G, C, QN, RN, + method="scipy", + symmetric_kwargs={"rtol": 1e-12}, + ) + @pytest.mark.parametrize("cdlqe", [lqe, dlqe]) def test_lqe_call_format(cdlqe): # Create a random state space system for testing