Skip to content

Commit 20b0312

Browse files
murrayrmmurrayrm
committed
add lqr/lqe overload for discrete time systems
1 parent 3e99742 commit 20b0312

2 files changed

Lines changed: 155 additions & 20 deletions

File tree

‎control/statefbk.py‎

Lines changed: 76 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -45,7 +45,7 @@
4545
from . import statesp
4646
from .mateqn import care, dare
4747
from .statesp import _ssmatrix, _convert_to_statespace
48-
from .lti import LTI, isdtime
48+
from .lti import LTI, isdtime, isctime
4949
from .exception import ControlSlycot, ControlArgument, ControlDimension, \
5050
ControlNotImplemented
5151

@@ -325,8 +325,13 @@ def lqe(*args, **keywords):
325325
326326
Notes
327327
-----
328-
The return type for 2D arrays depends on the default class set for
329-
state space operations. See :func:`~control.use_numpy_matrix`.
328+
1. If the first argument is an LTI object, then this object will be used
329+
to define the dynamics, noise and output matrices. Furthermore, if
330+
the LTI object corresponds to a discrete time system, the ``dlqe()``
331+
function will be called.
332+
333+
2. The return type for 2D arrays depends on the default class set for
334+
state space operations. See :func:`~control.use_numpy_matrix`.
330335
331336
Examples
332337
--------
@@ -356,6 +361,11 @@ def lqe(*args, **keywords):
356361
if (len(args) < 3):
357362
raise ControlArgument("not enough input arguments")
358363

364+
# If we were passed a discrete time system as the first arg, use dlqe()
365+
if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
366+
# Call dlqe
367+
return dlqe(*args, **keywords)
368+
359369
try:
360370
sys = args[0] # Treat the first argument as a system
361371
if isinstance(sys, LTI):
@@ -405,7 +415,7 @@ def lqe(*args, **keywords):
405415

406416

407417
# contributed by Sawyer B. Fuller <minster@uw.edu>
408-
def dlqe(A, G, C, QN, RN, NN=None):
418+
def dlqe(*args, **keywords):
409419
"""dlqe(A, G, C, QN, RN, [, N])
410420
411421
Linear quadratic estimator design (Kalman filter) for discrete-time
@@ -436,7 +446,11 @@ def dlqe(A, G, C, QN, RN, NN=None):
436446
QN, RN : 2D array_like
437447
Process and sensor noise covariance matrices
438448
NN : 2D array, optional
439-
Cross covariance matrix
449+
Cross covariance matrix (not yet supported)
450+
method : str, optional
451+
Set the method used for computing the result. Current methods are
452+
'slycot' and 'scipy'. If set to None (default), try 'slycot' first
453+
and then 'scipy'.
440454
441455
Returns
442456
-------
@@ -468,14 +482,49 @@ def dlqe(A, G, C, QN, RN, NN=None):
468482
469483
"""
470484

485+
#
486+
# Process the arguments and figure out what inputs we received
487+
#
488+
489+
# Get the method to use (if specified as a keyword)
490+
method = keywords.get('method', None)
491+
492+
# Get the system description
493+
if (len(args) < 3):
494+
raise ControlArgument("not enough input arguments")
495+
496+
# If we were passed a continus time system as the first arg, raise error
497+
if isinstance(args[0], LTI) and isctime(args[0], strict=True):
498+
raise ControlArgument("dlqr() called with a continuous time system")
499+
500+
try:
501+
# If this works, we were (probably) passed a system as the
502+
# first argument; extract A and B
503+
A = np.array(args[0].A, ndmin=2, dtype=float)
504+
G = np.array(args[0].B, ndmin=2, dtype=float)
505+
C = np.array(args[0].C, ndmin=2, dtype=float)
506+
index = 1
507+
except AttributeError:
508+
# Arguments should be A and B matrices
509+
A = np.array(args[0], ndmin=2, dtype=float)
510+
G = np.array(args[1], ndmin=2, dtype=float)
511+
C = np.array(args[2], ndmin=2, dtype=float)
512+
index = 3
513+
514+
# Get the weighting matrices (converting to matrices, if needed)
515+
QN = np.array(args[index], ndmin=2, dtype=float)
516+
RN = np.array(args[index+1], ndmin=2, dtype=float)
517+
471518
# TODO: incorporate cross-covariance NN, something like this,
472519
# which doesn't work for some reason
473520
# if NN is None:
474521
# NN = np.zeros(QN.size(0),RN.size(1))
475522
# NG = G @ NN
523+
if len(args) > index + 2:
524+
NN = np.array(args[index+2], ndmin=2, dtype=float)
525+
raise ControlNotImplemented("cross-covariance not yet implememented")
476526

477-
A, G, C, QN, RN = map(np.atleast_2d, (A, G, C, QN, RN))
478-
P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN)
527+
P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN, method=method)
479528
return _ssmatrix(LT.T), _ssmatrix(P), E
480529

481530
# Contributed by Roberto Bucher <roberto.bucher@supsi.ch>
@@ -576,8 +625,13 @@ def lqr(*args, **keywords):
576625
577626
Notes
578627
-----
579-
The return type for 2D arrays depends on the default class set for
580-
state space operations. See :func:`~control.use_numpy_matrix`.
628+
1. If the first argument is an LTI object, then this object will be used
629+
to define the dynamics and input matrices. Furthermore, if the LTI
630+
object corresponds to a discrete time system, the ``dlqr()`` function
631+
will be called.
632+
633+
2. The return type for 2D arrays depends on the default class set for
634+
state space operations. See :func:`~control.use_numpy_matrix`.
581635
582636
Examples
583637
--------
@@ -596,6 +650,11 @@ def lqr(*args, **keywords):
596650
if (len(args) < 3):
597651
raise ControlArgument("not enough input arguments")
598652

653+
# If we were passed a discrete time system as the first arg, use dlqr()
654+
if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
655+
# Call dlqr
656+
return dlqr(*args, **keywords)
657+
599658
try:
600659
# If this works, we were (probably) passed a system as the
601660
# first argument; extract A and B
@@ -681,10 +740,17 @@ def dlqr(*args, **keywords):
681740
# Process the arguments and figure out what inputs we received
682741
#
683742

743+
# Get the method to use (if specified as a keyword)
744+
method = keywords.get('method', None)
745+
684746
# Get the system description
685747
if (len(args) < 3):
686748
raise ControlArgument("not enough input arguments")
687749

750+
# If we were passed a continus time system as the first arg, raise error
751+
if isinstance(args[0], LTI) and isctime(args[0], strict=True):
752+
raise ControlArgument("dsys must be discrete time (dt != 0)")
753+
688754
try:
689755
# If this works, we were (probably) passed a system as the
690756
# first argument; extract A and B
@@ -697,11 +763,6 @@ def dlqr(*args, **keywords):
697763
B = np.array(args[1], ndmin=2, dtype=float)
698764
index = 2
699765

700-
# confirm that if we received a system that it was discrete-time
701-
if index == 1:
702-
if not isdtime(args[0]):
703-
raise ValueError("dsys must be discrete (dt !=0)")
704-
705766
# Get the weighting matrices (converting to matrices, if needed)
706767
Q = np.array(args[index], ndmin=2, dtype=float)
707768
R = np.array(args[index+1], ndmin=2, dtype=float)
@@ -722,7 +783,7 @@ def dlqr(*args, **keywords):
722783
raise ControlDimension("incorrect weighting matrix dimensions")
723784

724785
# compute the result
725-
S, E, K = dare(A, B, Q, R, N)
786+
S, E, K = dare(A, B, Q, R, N, method=method)
726787
return _ssmatrix(K), _ssmatrix(S), E
727788

728789

‎control/tests/statefbk_test.py‎

Lines changed: 79 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -298,7 +298,6 @@ def testPlace_varga_discrete_partial_eigs(self, matarrayin):
298298
P_placed = np.linalg.eigvals(A - B @ K)
299299
self.checkPlaced(P_expected, P_placed)
300300

301-
302301
def check_LQR(self, K, S, poles, Q, R):
303302
S_expected = asmatarrayout(np.sqrt(Q @ R))
304303
K_expected = asmatarrayout(S_expected / R)
@@ -334,6 +333,8 @@ def test_LQR_3args(self, matarrayin, matarrayout, method):
334333

335334
@pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
336335
def test_DLQR_3args(self, matarrayin, matarrayout, method):
336+
if method == 'slycot' and not slycot_check():
337+
return
337338
dsys = ss(0., 1., 1., 0., .1)
338339
Q, R = (matarrayin([[X]]) for X in [10., 2.])
339340
K, S, poles = dlqr(dsys, Q, R, method=method)
@@ -433,9 +434,13 @@ def check_LQE(self, L, P, poles, G, QN, RN):
433434
np.testing.assert_array_almost_equal(L, L_expected)
434435
np.testing.assert_array_almost_equal(poles, poles_expected)
435436

436-
def test_LQE(self, matarrayin):
437+
@pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
438+
def test_LQE(self, matarrayin, method):
439+
if method == 'slycot' and not slycot_check():
440+
return
441+
437442
A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.])
438-
L, P, poles = lqe(A, G, C, QN, RN)
443+
L, P, poles = lqe(A, G, C, QN, RN, method=method)
439444
self.check_LQE(L, P, poles, G, QN, RN)
440445

441446
def test_lqe_call_format(self):
@@ -482,9 +487,13 @@ def check_DLQE(self, L, P, poles, G, QN, RN):
482487
np.testing.assert_array_almost_equal(L, L_expected)
483488
np.testing.assert_array_almost_equal(poles, poles_expected)
484489

485-
def test_DLQE(self, matarrayin):
490+
@pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
491+
def test_DLQE(self, matarrayin, method):
492+
if method == 'slycot' and not slycot_check():
493+
return
494+
486495
A, G, C, QN, RN = (matarrayin([[X]]) for X in [0., .1, 1., 10., 2.])
487-
L, P, poles = dlqe(A, G, C, QN, RN)
496+
L, P, poles = dlqe(A, G, C, QN, RN, method=method)
488497
self.check_DLQE(L, P, poles, G, QN, RN)
489498

490499
def test_care(self, matarrayin):
@@ -522,3 +531,68 @@ def test_dare(self, matarrayin, stabilizing):
522531
sgn = {True: -1, False: 1}[stabilizing]
523532
assert np.all(sgn * (np.abs(L) - 1) > 0)
524533

534+
def test_lqr_discrete(self):
535+
"""Test overloading of lqr operator for discrete time systems"""
536+
csys = ct.rss(2, 1, 1)
537+
dsys = ct.drss(2, 1, 1)
538+
Q = np.eye(2)
539+
R = np.eye(1)
540+
541+
# Calling with a system versus explicit A, B should be the sam
542+
K_csys, S_csys, E_csys = ct.lqr(csys, Q, R)
543+
K_expl, S_expl, E_expl = ct.lqr(csys.A, csys.B, Q, R)
544+
np.testing.assert_almost_equal(K_csys, K_expl)
545+
np.testing.assert_almost_equal(S_csys, S_expl)
546+
np.testing.assert_almost_equal(E_csys, E_expl)
547+
548+
# Calling lqr() with a discrete time system should call dlqr()
549+
K_lqr, S_lqr, E_lqr = ct.lqr(dsys, Q, R)
550+
K_dlqr, S_dlqr, E_dlqr = ct.dlqr(dsys, Q, R)
551+
np.testing.assert_almost_equal(K_lqr, K_dlqr)
552+
np.testing.assert_almost_equal(S_lqr, S_dlqr)
553+
np.testing.assert_almost_equal(E_lqr, E_dlqr)
554+
555+
# Calling lqr() with no timebase should call lqr()
556+
asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None)
557+
K_asys, S_asys, E_asys = ct.lqr(asys, Q, R)
558+
K_expl, S_expl, E_expl = ct.lqr(csys.A, csys.B, Q, R)
559+
np.testing.assert_almost_equal(K_asys, K_expl)
560+
np.testing.assert_almost_equal(S_asys, S_expl)
561+
np.testing.assert_almost_equal(E_asys, E_expl)
562+
563+
# Calling dlqr() with a continuous time system should raise an error
564+
with pytest.raises(ControlArgument, match="dsys must be discrete"):
565+
K, S, E = ct.dlqr(csys, Q, R)
566+
567+
def test_lqe_discrete(self):
568+
"""Test overloading of lqe operator for discrete time systems"""
569+
csys = ct.rss(2, 1, 1)
570+
dsys = ct.drss(2, 1, 1)
571+
Q = np.eye(1)
572+
R = np.eye(1)
573+
574+
# Calling with a system versus explicit A, B should be the sam
575+
K_csys, S_csys, E_csys = ct.lqe(csys, Q, R)
576+
K_expl, S_expl, E_expl = ct.lqe(csys.A, csys.B, csys.C, Q, R)
577+
np.testing.assert_almost_equal(K_csys, K_expl)
578+
np.testing.assert_almost_equal(S_csys, S_expl)
579+
np.testing.assert_almost_equal(E_csys, E_expl)
580+
581+
# Calling lqe() with a discrete time system should call dlqe()
582+
K_lqe, S_lqe, E_lqe = ct.lqe(dsys, Q, R)
583+
K_dlqe, S_dlqe, E_dlqe = ct.dlqe(dsys, Q, R)
584+
np.testing.assert_almost_equal(K_lqe, K_dlqe)
585+
np.testing.assert_almost_equal(S_lqe, S_dlqe)
586+
np.testing.assert_almost_equal(E_lqe, E_dlqe)
587+
588+
# Calling lqe() with no timebase should call lqe()
589+
asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None)
590+
K_asys, S_asys, E_asys = ct.lqe(asys, Q, R)
591+
K_expl, S_expl, E_expl = ct.lqe(csys.A, csys.B, csys.C, Q, R)
592+
np.testing.assert_almost_equal(K_asys, K_expl)
593+
np.testing.assert_almost_equal(S_asys, S_expl)
594+
np.testing.assert_almost_equal(E_asys, E_expl)
595+
596+
# Calling dlqe() with a continuous time system should raise an error
597+
with pytest.raises(ControlArgument, match="called with a continuous"):
598+
K, S, E = ct.dlqe(csys, Q, R)

0 commit comments

Comments
 (0)