8000 initial addition of dlqe and dlqr · basicmachines/python-control@04cfe65 · GitHub
[go: up one dir, main page]
Skip to content

Commit 04cfe65

Browse files
sawyerbfullermurrayrm
sawyerbfuller
authored and
murrayrm
committed
initial addition of dlqe and dlqr
1 parent b382f2d commit 04cfe65

File tree

2 files changed

+225
-10
lines changed

2 files changed

+225
-10
lines changed

‎control/statefbk.py‎

Lines changed: 188 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -43,9 +43,9 @@
4343
import numpy as np
4444

4545
from . import statesp
46-
from .mateqn import care
46+
from .mateqn import care, dare
4747
from .statesp import _ssmatrix, _convert_to_statespace
48-
from .lti import LTI
48+
from .lti import LTI, isdtime
4949
from .exception import ControlSlycot, ControlArgument, ControlDimension, \
5050
ControlNotImplemented
5151

@@ -69,7 +69,7 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None):
6969

7070

7171
__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'lqe',
72-
'acker']
72+
'dlqr', 'dlqe', 'acker']
7373

7474

7575
# Pole placement
@@ -335,7 +335,7 @@ def lqe(*args, **keywords):
335335
336336
See Also
337337
--------
338-
lqr
338+
lqr, dlqe, dlqr
339339
340340
"""
341341

@@ -403,6 +403,82 @@ def lqe(*args, **keywords):
403403
P, E, LT = care(A.T, C.T, G @ Q @ G.T, R, method=method)
404404
return _ssmatrix(LT.T), _ssmatrix(P), E
405405

406+
# contributed by Sawyer B. Fuller <minster@uw.edu>
407+
def dlqe(A, G, C, QN, RN, NN=None):
408+
"""dlqe(A, G, C, QN, RN, [, N])
409+
410+
Linear quadratic estimator design (Kalman filter) for discrete-time
411+
systems. Given the system
412+
413+
.. math::
414+
415+
x[n+1] &= Ax[n] + Bu[n] + Gw[n] \\\\
416+
y[n] &= Cx[n] + Du[n] + v[n]
417+
418+
with unbiased process noise w and measurement noise v with covariances
419+
420+
.. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN
421+
422+
The dlqe() function computes the observer gain matrix L such that the
423+
stationary (non-time-varying) Kalman filter
424+
425+
.. math:: x_e[n+1] = A x_e[n] + B u[n] + L(y[n] - C x_e[n] - D u[n])
426+
427+
produces a state estimate x_e[n] that minimizes the expected squared error
428+
using the sensor measurements y. The noise cross-correlation `NN` is
429+
set to zero when omitted.
430+
431+
Parameters
432+
----------
433+
A, G : 2D array_like
434+
Dynamics and noise input matrices
435+
QN, RN : 2D array_like
436+
Process and sensor noise covariance matrices
437+
NN : 2D array, optional
438+
Cross covariance matrix
439+
440+
Returns
441+
-------
442+
L : 2D array (or matrix)
443+
Kalman estimator gain
444+
P : 2D array (or matrix)
445+
Solution to Riccati equation
446+
447+
.. math::
448+
449+
A P + P A^T - (P C^T + G N) R^{-1} (C P + N^T G^T) + G Q G^T = 0
450+
451+
E : 1D array
452+
Eigenvalues of estimator poles eig(A - L C)
453+
454+
Notes
455+
-----
456+
The return type for 2D arrays depends on the default class set for
457+
state space operations. See :func:`~control.use_numpy_matrix`.
458+
459+
Examples
460+
--------
461+
>>> L, P, E = dlqe(A, G, C, QN, RN)
462+
>>> L, P, E = dlqe(A, G, C, QN, RN, NN)
463+
464+
See Also
465+
--------
466+
dlqr, lqe, lqr
467+
468+
"""
469+
470+
# TODO: incorporate cross-covariance NN, something like this,
471+
# which doesn't work for some reason
472+
# if NN is None:
473+
# NN = np.zeros(QN.size(0),RN.size(1))
474+
# NG = G @ NN
475+
476+
# LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN)
477+
# P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN)
478+
A, G, C = np.array(A, ndmin=2), np.array(G, ndmin=2), np.array(C, ndmin=2)
479+
QN, RN = np.array(QN, ndmin=2), np.array(RN, ndmin=2)
480+
P, E, LT = dare(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN)
481+
return _ssmatrix(LT.T), _ssmatrix(P), E
406482

407483
# Contributed by Roberto Bucher <roberto.bucher@supsi.ch>
408484
def acker(A, B, poles):
@@ -458,7 +534,7 @@ def lqr(*args, **keywords):
458534
Linear quadratic regulator design
459535
460536
The lqr() function computes the optimal state feedback controller
461-
that minimizes the quadratic cost
537+
u = -K x that minimizes the quadratic cost
462538
463539
.. math:: J = \\int_0^\\infty (x' Q x + u' R u + 2 x' N u) dt
464540
@@ -476,8 +552,8 @@ def lqr(*args, **keywords):
476552
----------
477553
A, B : 2D array_like
478554
Dynamics and input matrices
479-
sys : LTI (StateSpace or TransferFunction)
480-
Linear I/O system
555+
sys : LTI StateSpace system
556+
Linear system
481557
Q, R : 2D array
482558
State and input weight matrices
483559
N : 2D array, optional
@@ -546,6 +622,111 @@ def lqr(*args, **keywords):
546622
X, L, G = care(A, B, Q, R, N, None, method=method)
547623
return G, X, L
548624

625+
def dlqr(*args, **keywords):
626+
"""dlqr(A, B, Q, R[, N])
627+
628+
Discrete-time linear quadratic regulator design
629+
630+
The dlqr() function computes the optimal state feedback controller
631+
u[n] = - K x[n] that minimizes the quadratic cost
632+
633+
.. math:: J = \\Sum_0^\\infty (x[n]' Q x[n] + u[n]' R u[n] + 2 x[n]' N u[n])
634+
635+
The function can be called with either 3, 4, or 5 arguments:
636+
637+
* ``dlqr(dsys, Q, R)``
638+
* ``dlqr(dsys, Q, R, N)``
639+
* ``dlqr(A, B, Q, R)``
640+
* ``dlqr(A, B, Q, R, N)``
641+
642+
where `dsys` is a discrete-time :class:`StateSpace` system, and `A`, `B`,
643+
`Q`, `R`, and `N` are 2d arrays of appropriate dimension (`dsys.dt` must
644+
not be 0.)
645+
646+
Parameters
647+
----------
648+
A, B : 2D array
649+
Dynamics and input matrices
650+
dsys : LTI :class:`StateSpace`
651+
Discrete-time linear system
652+
Q, R : 2D array
653+
State and input weight matrices
654+
N : 2D array, optional
655+
Cross weight matrix
656+
657+
Returns
658+
-------
659+
K : 2D array (or matrix)
660+
State feedback gains
661+
S : 2D array (or matrix)
662+
Solution to Riccati equation
663+
E : 1D array
664+
Eigenvalues of the closed loop system
665+
666+
See Also
667+
--------
668+
lqr, lqe, dlqe
669+
670+
Notes
671+
-----
672+
The return type for 2D arrays depends on the default class set for
673+
state space operations. See :func:`~control.use_numpy_matrix`.
674+
675+
Examples
676+
--------
677+
>>> K, S, E = dlqr(dsys, Q, R, [N])
678+
>>> K, S, E = dlqr(A, B, Q, R, [N])
679+
"""
680+
681+
#
682+
# Process the arguments and figure out what inputs we received
683+
#
684+
685+
# Get the system description
686+
if (len(args) < 3):
687+
raise ControlArgument("not enough input arguments")
688+
689+
try:
690+
# If this works, we were (probably) passed a system as the
691+
# first argument; extract A and B
692+
A = np.array(args[0].A, ndmin=2, dtype=float)
693+
B = np.array(args[0].B, ndmin=2, dtype=float)
694+
index = 1
695+
except AttributeError:
696+
# Arguments should be A and B matrices
697+
A = np.array(args[0], ndmin=2, dtype=float)
698+
B = np.array(args[1], ndmin=2, dtype=float)
699+
index = 2
700+
701+
# confirm that if we received a system that it was discrete-time
702+
if index == 1:
703+
if not isdtime(args[0]):
704+
raise ValueError("dsys must be discrete (dt !=0)")
705+
706+
# Get the weighting matrices (converting to matrices, if needed)
707+
Q = np.array(args[index], ndmin=2, dtype=float)
708+
R = np.array(args[index+1], ndmin=2, dtype=float)
709+
if (len(args) > index + 2):
710+
N = np.array(args[index+2], ndmin=2, dtype=float)
711+
else:
712+
N = np.zeros((Q.shape[0], R.shape[1]))
713+
714+
# Check dimensions for consistency
715+
nstates = B.shape[0]
716+
ninputs = B.shape[1]
717+
if (A.shape[0] != nstates or A.shape[1] != nstates):
718+
raise ControlDimension("inconsistent system dimensions")
719+
720+
elif (Q.shape[0] != nstates or Q.shape[1] != nstates or
721+
R.shape[0] != ninputs or R.shape[1] != ninputs or
722+
N.shape[0] != nstates or N.shape[1] != ninputs):
723+
raise ControlDimension("incorrect weighting matrix dimensions")
724+
725+
# compute the result
726+
S, E, K = dare(A, B, Q, R, N)
727+
return _ssmatrix(K), _ssmatrix(S), E
728+
729+
549730
def ctrb(A, B):
550731
"""Controllabilty matrix
551732

‎control/tests/statefbk_test.py‎

Lines changed: 37 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,8 @@
1111
from control.exception import ControlDimension, ControlSlycot, \
1212
ControlArgument, slycot_check
1313
from control.mateqn import care, dare
14-
from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker
14+
from control.statefbk import (ctrb, obsv, place, place_varga, lqr, dlqr,
15+
lqe, dlqe, gram, acker)
1516
from control.tests.conftest import (slycotonly, check_deprecated_matrix,
1617
ismatarrayout, asmatarrayout)
1718

@@ -77,7 +78,7 @@ def testCtrbObsvDuality(self, matarrayin):
7778
Wc = ctrb(A, B)
7879
A = np.transpose(A)
7980
C = np.transpose(B)
80-
Wo = np.transpose(obsv(A, C));
81+
Wo = np.transpose(obsv(A, C))
8182
np.testing.assert_array_almost_equal(Wc,Wo)
8283

8384
@slycotonly
@@ -306,6 +307,14 @@ def check_LQR(self, K, S, poles, Q, R):
306307
np.testing.assert_array_almost_equal(K, K_expected)
307308
np.testing.assert_array_almost_equal(poles, poles_expected)
308309

310+
def check_DLQR(self, K, S, poles, Q, R):
311+
S_expected = asmatarrayout(Q)
312+
K_expected = asmatarrayout(0)
313+
poles_expected = -np.squeeze(np.asarray(K_expected))
314+
np.testing.assert_array_almost_equal(S, S_expected)
315+
np.testing.assert_array_almost_equal(K, K_expected)
316+
np.testing.assert_array_almost_equal(poles, poles_expected)
317+
309318
@pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
310319
def test_LQR_integrator(self, matarrayin, matarrayout, method):
311320
if method == 'slycot' and not slycot_check():
@@ -323,6 +332,13 @@ def test_LQR_3args(self, matarrayin, matarrayout, method):
323332
K, S, poles = lqr(sys, Q, R, method=method)
324333
self.check_LQR(K, S, poles, Q, R)
325334

335+
@pytest.mark.parametrize("method", [None, 'slycot', 'scipy'])
336+
def test_DLQR_3args(self, matarrayin, matarrayout, method):
337+
dsys = ss(0., 1., 1., 0., .1)
338+
Q, R = (matarrayin([[X]]) for X in [10., 2.])
339+
K, S, poles = dlqr(dsys, Q, R, method=method)
340+
self.check_DLQR(K, S, poles, Q, R)
341+
326342
def test_lqr_badmethod(self):
327343
A, B, Q, R = 0, 1, 10, 2
328344
with pytest.raises(ControlArgument, match="Unknown method"):
@@ -353,7 +369,6 @@ def testLQR_warning(self):
353369
with pytest.warns(UserWarning):
354370
(K, S, E) = lqr(A, B, Q, R, N)
355371

356-
@slycotonly
357372
def test_lqr_call_format(self):
358373
# Create a random state space system for testing
359374
sys = rss(2, 3, 2)
@@ -386,6 +401,25 @@ def test_lqr_call_format(self):
386401
with pytest.raises(ct.ControlDimension, match="Q must be a square"):
387402
K, S, E = lqr(sys.A, sys.B, sys.C, R, Q)
388403

404+
@pytest.mark.xfail(reason="warning not implemented")
405+
def testDLQR_warning(self):
406+
"""Test dlqr()
407+
408+
Make sure we get a warning if [Q N;N' R] is not positive semi-definite
409+
"""
410+
# from matlab_test siso.ss2 (testLQR); probably not referenced before
411+
# not yet implemented check
412+
A = np.array([[-2, 3, 1],
413+
[-1, 0, 0],
414+
[0, 1, 0]])
415+
B = np.array([[-1, 0, 0]]).T
416+
Q = np.eye(3)
417+
R = np.eye(1)
418+
N = np.array([[1, 1, 2]]).T
419+
# assert any(np.linalg.eigvals(np.block([[Q, N], [N.T, R]])) < 0)
420+
with pytest.warns(UserWarning):
421+
(K, S, E) = dlqr(A, B, Q, R, N)
422+
389423
def check_LQE(self, L, P, poles, G, QN, RN):
390424
P_expected = asmatarrayout(np.sqrt(G @ QN @ G @ RN))
391425
L_expected = asmatarrayout(P_expected / RN)

0 commit comments

Comments
 (0)
0