Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
58 changes: 35 additions & 23 deletions control/mateqn.py
Original file line number Diff line number Diff line change
Expand Up @@ -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, \
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
-------
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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':
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
-------
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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':
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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'.
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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.

Expand Down Expand Up @@ -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'.
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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:
Expand All @@ -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)
21 changes: 15 additions & 6 deletions control/stochsys.py
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@


# contributed by Sawyer B. Fuller <minster@uw.edu>
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).
Expand Down Expand Up @@ -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
-------
Expand Down Expand Up @@ -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 <minster@uw.edu>
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).
Expand Down Expand Up @@ -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
-------
Expand Down Expand Up @@ -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


Expand All @@ -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
Expand Down Expand Up @@ -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
-----
Expand Down Expand Up @@ -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(
Expand Down
24 changes: 23 additions & 1 deletion control/tests/mateqn_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -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


Expand Down Expand Up @@ -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):
Comment thread
slivingston marked this conversation as resolved.
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)
48 changes: 48 additions & 0 deletions control/tests/stochsys_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -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():
Comment thread
slivingston marked this conversation as resolved.
Comment thread
slivingston marked this conversation as resolved.
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():
Comment thread
slivingston marked this conversation as resolved.
Comment thread
slivingston marked this conversation as resolved.
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},
)
Comment thread
slivingston marked this conversation as resolved.
Comment thread
slivingston marked this conversation as resolved.

@pytest.mark.parametrize("cdlqe", [lqe, dlqe])
def test_lqe_call_format(cdlqe):
# Create a random state space system for testing
Expand Down