Skip to content

Commit 619c8b8

Browse files
marko1olomarko1olo
committed
fix(stochsys): stabilize lqe covariance symmetry
1 parent 146ccee commit 619c8b8

4 files changed

Lines changed: 127 additions & 7 deletions

File tree

‎control/mateqn.py‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -653,7 +653,9 @@ def _check_shape(M, n, m, square=False, symmetric=False, name="??"):
653653
def _is_symmetric(M):
654654
M = np.atleast_2d(M)
655655
if isinstance(M[0, 0], inexact):
656-
eps = finfo(M.dtype).eps
657-
return ((M - M.T) < eps).all()
656+
eps = finfo(M.real.dtype if np.iscomplexobj(M) else M.dtype).eps
657+
if np.iscomplexobj(M):
658+
return sp.linalg.ishermitian(M, atol=100 * eps, rtol=100 * eps)
659+
return sp.linalg.issymmetric(M, atol=100 * eps, rtol=100 * eps)
658660
else:
659661
return (M == M.T).all()

‎control/stochsys.py‎

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,10 @@
3232
'correlation']
3333

3434

35+
def _symmetrize_covariance(M):
36+
return (M + M.T) / 2
37+
38+
3539
# contributed by Sawyer B. Fuller <minster@uw.edu>
3640
def lqe(*args, **kwargs):
3741
r"""lqe(A, G, C, QN, RN, [, NN])
@@ -174,10 +178,11 @@ def lqe(*args, **kwargs):
174178

175179

176180
# Check dimensions of G (needed before calling care())
177-
_check_shape(QN, G.shape[1], G.shape[1], name="QN")
181+
_check_shape(QN, G.shape[1], G.shape[1], symmetric=True, name="QN")
178182

179183
# Compute the result (dimension and symmetry checking done in care())
180-
P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN, method=method,
184+
QG = _symmetrize_covariance(G @ QN @ G.T)
185+
P, E, LT = care(A.T, C.T, QG, RN, method=method,
181186
_Bs="C", _Qs="QN", _Rs="RN", _Ss="NN")
182187
return LT.T, P, E
183188

@@ -295,10 +300,11 @@ def dlqe(*args, **kwargs):
295300
raise ControlNotImplemented("cross-covariance not yet implemented")
296301

297302
# Check dimensions of G (needed before calling care())
298-
_check_shape(QN, G.shape[1], G.shape[1], name="QN")
303+
_check_shape(QN, G.shape[1], G.shape[1], symmetric=True, name="QN")
299304

300305
# Compute the result (dimension and symmetry checking done in dare())
301-
P, E, LT = dare(A.T, C.T, G @ QN @ G.T, RN, method=method,
306+
QG = _symmetrize_covariance(G @ QN @ G.T)
307+
P, E, LT = dare(A.T, C.T, QG, RN, method=method,
302308
_Bs="C", _Qs="QN", _Rs="RN", _Ss="NN")
303309
return LT.T, P, E
304310

‎control/tests/mateqn_test.py‎

Lines changed: 24 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@
3939
import pytest
4040
from scipy.linalg import eigvals, solve
4141

42-
from control.mateqn import lyap, dlyap, care, dare
42+
from control.mateqn import _check_shape, lyap, dlyap, care, dare
4343
from control.exception import ControlArgument, ControlDimension
4444

4545

@@ -430,3 +430,26 @@ def test_raise(self):
430430
cdare(A, B, Qfs, R, S, E)
431431
with pytest.raises(ControlArgument):
432432
cdare(A, B, Q, Rfs, S, E)
433+
434+
def test_symmetric_shape_check_tolerance(self):
435+
eps = np.finfo(float).eps
436+
437+
_check_shape(
438+
np.array([[1., 100 * eps], [0., 1.]]),
439+
2, 2, symmetric=True, name="M")
440+
_check_shape(
441+
np.array([[1., 0.], [-100 * eps, 1.]]),
442+
2, 2, symmetric=True, name="M")
443+
_check_shape(
444+
np.array([[1., 1j * eps], [-1j * eps, 1.]]),
445+
2, 2, symmetric=True, name="M")
446+
447+
with pytest.raises(ControlArgument, match="M must be a symmetric"):
448+
_check_shape(
449+
np.array([[1., 1e-3], [0., 1.]]),
450+
2, 2, symmetric=True, name="M")
451+
452+
with pytest.raises(ControlArgument, match="M must be a symmetric"):
453+
_check_shape(
454+
np.array([[1., 1j], [1j, 1.]]),
455+
2, 2, symmetric=True, name="M")

‎control/tests/stochsys_test.py‎

Lines changed: 89 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -85,6 +85,95 @@ def test_DLQE(method):
8585
L, P, poles = dlqe(A, G, C, QN, RN, method=method)
8686
check_DLQE(L, P, poles, G, QN, RN)
8787

88+
89+
def test_lqe_symmetrizes_projected_process_covariance():
90+
A = np.array([
91+
[-1.945215, 0.071519, 0.060276, 0.054488, 0.042365],
92+
[0.064589, -1.956241, 0.089177, 0.043759, 0.038344],
93+
[0.079173, 0.052889, -1.943196, 0.096366, 0.038344],
94+
[0., 0., 0., -2.920827, 0.052889],
95+
[0., 0., 0., 0.056804, -2.90744],
96+
])
97+
G = np.array([
98+
[0.087129, 0.020218, 0.83262],
99+
[0.778157, 0.870012, 0.978618],
100+
[0.799159, 0.461479, 0.780529],
101+
[0., 0., 0.],
102+
[0., 0., 0.],
103+
])
104+
C = np.array([
105+
[0.118274, 0.639921, 0.143353, 0.944669, 0.521848],
106+
[0.414662, 0.264556, 0.774234, 0.45615, 0.568434],
107+
[0.01879, 0.617635, 0.612096, 0.616934, 0.943748],
108+
])
109+
QN = np.array([
110+
[3.661526, -1.465017, 1.00303],
111+
[-1.465017, 1.013103, -0.582237],
112+
[1.00303, -0.582237, 0.502097],
113+
])
114+
RN = np.array([
115+
[1.17295, -0.446977, -0.010618],
116+
[-0.446977, 3.675234, 0.558705],
117+
[-0.010618, 0.558705, 0.53198],
118+
])
119+
120+
L, P, poles = lqe(A, G, C, QN, RN, method='scipy')
121+
122+
assert L.shape == (A.shape[0], C.shape[0])
123+
assert P.shape == A.shape
124+
assert poles.shape == (A.shape[0],)
125+
126+
127+
def test_dlqe_symmetrizes_projected_process_covariance():
128+
A = np.array([
129+
[0.109763, 0.143038, 0.120553, 0.054488, 0.042365],
130+
[0.108977, 0.084731, 0.129179, 0.043759, 0.089177],
131+
[0.192733, 0.076688, 0.158345, 0.096366, 0.038344],
132+
[0., 0., 0., 0.279173, 0.052889],
133+
[0., 0., 0., 0.056804, 0.29256],
134+
])
135+
G = np.array([
136+
[0.087129, 0.020218, 0.83262],
137+
[0.778157, 0.870012, 0.978618],
138+
[0.799159, 0.461479, 0.780529],
139+
[0., 0., 0.],
140+
[0., 0., 0.],
141+
])
142+
C = np.array([
143+
[0.118274, 0.639921, 0.143353, 0.944669, 0.521848],
144+
[0.414662, 0.264556, 0.774234, 0.45615, 0.568434],
145+
[0.01879, 0.617635, 0.612096, 0.616934, 0.943748],
146+
])
147+
QN = np.array([
148+
[3.661526, -1.465017, 1.00303],
149+
[-1.465017, 1.013103, -0.582237],
150+
[1.00303, -0.582237, 0.502097],
151+
])
152+
RN = np.array([
153+
[1.17295, -0.446977, -0.010618],
154+
[-0.446977, 3.675234, 0.558705],
155+
[-0.010618, 0.558705, 0.53198],
156+
])
157+
158+
L, P, poles = dlqe(A, G, C, QN, RN, method='scipy')
159+
160+
assert L.shape == (A.shape[0], C.shape[0])
161+
assert P.shape == A.shape
162+
assert poles.shape == (A.shape[0],)
163+
164+
165+
@pytest.mark.parametrize("cdlqe", [lqe, dlqe])
166+
def test_lqe_rejects_user_supplied_asymmetric_covariance(cdlqe):
167+
A = np.diag([0.4, 0.5])
168+
G = np.eye(2)
169+
C = np.array([[1., 0.]])
170+
QN = np.array([[1., 1.], [0., 1.]])
171+
RN = np.array([[1.]])
172+
173+
with pytest.raises(ControlArgument, match="QN must be a symmetric matrix"):
174+
cdlqe(A, G, C, QN, RN, method='scipy')
175+
176+
88177
def test_lqe_discrete():
89178
"""Test overloading of lqe operator for discrete-time systems"""
90179
csys = ct.rss(2, 1, 1)

0 commit comments

Comments
 (0)