diff --git a/control/mateqn.py b/control/mateqn.py index 9d1349b0c..88206a0b7 100644 --- a/control/mateqn.py +++ b/control/mateqn.py @@ -653,7 +653,9 @@ def _check_shape(M, n, m, square=False, symmetric=False, name="??"): 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() + eps = finfo(M.real.dtype if np.iscomplexobj(M) else M.dtype).eps + if np.iscomplexobj(M): + return sp.linalg.ishermitian(M, atol=100 * eps, rtol=100 * eps) + return sp.linalg.issymmetric(M, atol=100 * eps, rtol=100 * eps) else: return (M == M.T).all() diff --git a/control/stochsys.py b/control/stochsys.py index 756d83e13..546eb48a4 100644 --- a/control/stochsys.py +++ b/control/stochsys.py @@ -32,6 +32,10 @@ 'correlation'] +def _symmetrize_covariance(M): + return (M + M.T) / 2 + + # contributed by Sawyer B. Fuller def lqe(*args, **kwargs): r"""lqe(A, G, C, QN, RN, [, NN]) @@ -174,10 +178,11 @@ def lqe(*args, **kwargs): # Check dimensions of G (needed before calling care()) - _check_shape(QN, G.shape[1], G.shape[1], name="QN") + _check_shape(QN, G.shape[1], G.shape[1], symmetric=True, name="QN") # 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, + QG = _symmetrize_covariance(G @ QN @ G.T) + P, E, LT = care(A.T, C.T, QG, RN, method=method, _Bs="C", _Qs="QN", _Rs="RN", _Ss="NN") return LT.T, P, E @@ -295,10 +300,11 @@ def dlqe(*args, **kwargs): raise ControlNotImplemented("cross-covariance not yet implemented") # Check dimensions of G (needed before calling care()) - _check_shape(QN, G.shape[1], G.shape[1], name="QN") + _check_shape(QN, G.shape[1], G.shape[1], symmetric=True, name="QN") # 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, + QG = _symmetrize_covariance(G @ QN @ G.T) + P, E, LT = dare(A.T, C.T, QG, RN, method=method, _Bs="C", _Qs="QN", _Rs="RN", _Ss="NN") return LT.T, P, E diff --git a/control/tests/mateqn_test.py b/control/tests/mateqn_test.py index 77bf553bf..57a306e5a 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 _check_shape, lyap, dlyap, care, dare from control.exception import ControlArgument, ControlDimension @@ -430,3 +430,26 @@ def test_raise(self): cdare(A, B, Qfs, R, S, E) with pytest.raises(ControlArgument): cdare(A, B, Q, Rfs, S, E) + + def test_symmetric_shape_check_tolerance(self): + eps = np.finfo(float).eps + + _check_shape( + np.array([[1., 100 * eps], [0., 1.]]), + 2, 2, symmetric=True, name="M") + _check_shape( + np.array([[1., 0.], [-100 * eps, 1.]]), + 2, 2, symmetric=True, name="M") + _check_shape( + np.array([[1., 1j * eps], [-1j * eps, 1.]]), + 2, 2, symmetric=True, name="M") + + with pytest.raises(ControlArgument, match="M must be a symmetric"): + _check_shape( + np.array([[1., 1e-3], [0., 1.]]), + 2, 2, symmetric=True, name="M") + + with pytest.raises(ControlArgument, match="M must be a symmetric"): + _check_shape( + np.array([[1., 1j], [1j, 1.]]), + 2, 2, symmetric=True, name="M") diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py index 20e799643..26db69b74 100644 --- a/control/tests/stochsys_test.py +++ b/control/tests/stochsys_test.py @@ -85,6 +85,95 @@ def test_DLQE(method): L, P, poles = dlqe(A, G, C, QN, RN, method=method) check_DLQE(L, P, poles, G, QN, RN) + +def test_lqe_symmetrizes_projected_process_covariance(): + A = np.array([ + [-1.945215, 0.071519, 0.060276, 0.054488, 0.042365], + [0.064589, -1.956241, 0.089177, 0.043759, 0.038344], + [0.079173, 0.052889, -1.943196, 0.096366, 0.038344], + [0., 0., 0., -2.920827, 0.052889], + [0., 0., 0., 0.056804, -2.90744], + ]) + G = np.array([ + [0.087129, 0.020218, 0.83262], + [0.778157, 0.870012, 0.978618], + [0.799159, 0.461479, 0.780529], + [0., 0., 0.], + [0., 0., 0.], + ]) + C = np.array([ + [0.118274, 0.639921, 0.143353, 0.944669, 0.521848], + [0.414662, 0.264556, 0.774234, 0.45615, 0.568434], + [0.01879, 0.617635, 0.612096, 0.616934, 0.943748], + ]) + QN = np.array([ + [3.661526, -1.465017, 1.00303], + [-1.465017, 1.013103, -0.582237], + [1.00303, -0.582237, 0.502097], + ]) + RN = np.array([ + [1.17295, -0.446977, -0.010618], + [-0.446977, 3.675234, 0.558705], + [-0.010618, 0.558705, 0.53198], + ]) + + L, P, poles = lqe(A, G, C, QN, RN, method='scipy') + + assert L.shape == (A.shape[0], C.shape[0]) + assert P.shape == A.shape + assert poles.shape == (A.shape[0],) + + +def test_dlqe_symmetrizes_projected_process_covariance(): + A = np.array([ + [0.109763, 0.143038, 0.120553, 0.054488, 0.042365], + [0.108977, 0.084731, 0.129179, 0.043759, 0.089177], + [0.192733, 0.076688, 0.158345, 0.096366, 0.038344], + [0., 0., 0., 0.279173, 0.052889], + [0., 0., 0., 0.056804, 0.29256], + ]) + G = np.array([ + [0.087129, 0.020218, 0.83262], + [0.778157, 0.870012, 0.978618], + [0.799159, 0.461479, 0.780529], + [0., 0., 0.], + [0., 0., 0.], + ]) + C = np.array([ + [0.118274, 0.639921, 0.143353, 0.944669, 0.521848], + [0.414662, 0.264556, 0.774234, 0.45615, 0.568434], + [0.01879, 0.617635, 0.612096, 0.616934, 0.943748], + ]) + QN = np.array([ + [3.661526, -1.465017, 1.00303], + [-1.465017, 1.013103, -0.582237], + [1.00303, -0.582237, 0.502097], + ]) + RN = np.array([ + [1.17295, -0.446977, -0.010618], + [-0.446977, 3.675234, 0.558705], + [-0.010618, 0.558705, 0.53198], + ]) + + L, P, poles = dlqe(A, G, C, QN, RN, method='scipy') + + assert L.shape == (A.shape[0], C.shape[0]) + assert P.shape == A.shape + assert poles.shape == (A.shape[0],) + + +@pytest.mark.parametrize("cdlqe", [lqe, dlqe]) +def test_lqe_rejects_user_supplied_asymmetric_covariance(cdlqe): + A = np.diag([0.4, 0.5]) + G = np.eye(2) + C = np.array([[1., 0.]]) + QN = np.array([[1., 1.], [0., 1.]]) + RN = np.array([[1.]]) + + with pytest.raises(ControlArgument, match="QN must be a symmetric matrix"): + cdlqe(A, G, C, QN, RN, method='scipy') + + def test_lqe_discrete(): """Test overloading of lqe operator for discrete-time systems""" csys = ct.rss(2, 1, 1)