Skip to content

Improved lqe calling functionality #673

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Dec 2, 2021
Merged

Improved lqe calling functionality #673

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
1e55a03
update lqe() argument processing to match lqr()
murrayrm Nov 25, 2021
06aa0d8
updated docstrings + unit tests
murrayrm Nov 25, 2021
31729c8
remove comment per @sawyerbfuller
murrayrm Nov 27, 2021
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
108 changes: 86 additions & 22 deletions control/statefbk.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,10 @@

from . import statesp
from .mateqn import care
from .statesp import _ssmatrix
from .exception import ControlSlycot, ControlArgument, ControlDimension
from .statesp import _ssmatrix, _convert_to_statespace
from .lti import LTI
from .exception import ControlSlycot, ControlArgument, ControlDimension, \
ControlNotImplemented

# Make sure we have access to the right slycot routines
try:
Expand Down Expand Up @@ -257,8 +259,8 @@ def place_varga(A, B, p, dtime=False, alpha=None):


# contributed by Sawyer B. Fuller <minster@uw.edu>
def lqe(A, G, C, QN, RN, NN=None):
"""lqe(A, G, C, QN, RN, [, N])
def lqe(*args, **keywords):
"""lqe(A, G, C, Q, R, [, N])

Linear quadratic estimator design (Kalman filter) for continuous-time
systems. Given the system
Expand All @@ -270,25 +272,38 @@ def lqe(A, G, C, QN, RN, NN=None):

with unbiased process noise w and measurement noise v with covariances

.. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN
.. math:: E{ww'} = Q, E{vv'} = R, E{wv'} = N

The lqe() function computes the observer gain matrix L such that the
stationary (non-time-varying) Kalman filter

.. math:: x_e = A x_e + B u + L(y - C x_e - D u)

produces a state estimate x_e that minimizes the expected squared error
using the sensor measurements y. The noise cross-correlation `NN` is
using the sensor measurements y. The noise cross-correlation `N` is
set to zero when omitted.

The function can be called with either 3, 4, 5, or 6 arguments:

* ``L, P, E = lqe(sys, Q, R)``
* ``L, P, E = lqe(sys, Q, R, N)``
* ``L, P, E = lqe(A, G, C, Q, R)``
* ``L, P, E = lqe(A, B, C, Q, R, N)``

where `sys` is an `LTI` object, and `A`, `G`, `C`, `Q`, `R`, and `N` are
2D arrays or matrices of appropriate dimension.

Parameters
----------
A, G : 2D array_like
Dynamics and noise input matrices
QN, RN : 2D array_like
A, G, C : 2D array_like
Dynamics, process noise (disturbance), and output matrices
sys : LTI (StateSpace or TransferFunction)
Linear I/O system, with the process noise input taken as the system
input.
Q, R : 2D array_like
Process and sensor noise covariance matrices
NN : 2D array, optional
Cross covariance matrix
N : 2D array, optional
Cross covariance matrix. Not currently implemented.

Returns
-------
Expand Down Expand Up @@ -326,11 +341,60 @@ def lqe(A, G, C, QN, RN, NN=None):
# NN = np.zeros(QN.size(0),RN.size(1))
# NG = G @ NN

# LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN)
# P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN)
A, G, C = np.array(A, ndmin=2), np.array(G, ndmin=2), np.array(C, ndmin=2)
QN, RN = np.array(QN, ndmin=2), np.array(RN, ndmin=2)
P, E, LT = care(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN)
#
# Process the arguments and figure out what inputs we received
#

# Get the system description
if (len(args) < 3):
raise ControlArgument("not enough input arguments")

try:
sys = args[0] # Treat the first argument as a system
if isinstance(sys, LTI):
# Convert LTI system to state space
sys = _convert_to_statespace(sys)

# Extract A, G (assume disturbances come through input), and C
A = np.array(sys.A, ndmin=2, dtype=float)
G = np.array(sys.B, ndmin=2, dtype=float)
C = np.array(sys.C, ndmin=2, dtype=float)
index = 1

except AttributeError:
# Arguments should be A and B matrices
A = np.array(args[0], ndmin=2, dtype=float)
G = np.array(args[1], ndmin=2, dtype=float)
C = np.array(args[2], ndmin=2, dtype=float)
index = 3

# Get the weighting matrices (converting to matrices, if needed)
Q = np.array(args[index], ndmin=2, dtype=float)
R = np.array(args[index+1], ndmin=2, dtype=float)

# Get the cross-covariance matrix, if given
if (len(args) > index + 2):
N = np.array(args[index+2], ndmin=2, dtype=float)
raise ControlNotImplemented("cross-covariance not implemented")

else:
N = np.zeros((Q.shape[0], R.shape[1]))

# Check dimensions for consistency
nstates = A.shape[0]
ninputs = G.shape[1]
noutputs = C.shape[0]
if (A.shape[0] != nstates or A.shape[1] != nstates or
G.shape[0] != nstates or C.shape[1] != nstates):
raise ControlDimension("inconsistent system dimensions")

elif (Q.shape[0] != ninputs or Q.shape[1] != ninputs or
R.shape[0] != noutputs or R.shape[1] != noutputs or
N.shape[0] != ninputs or N.shape[1] != noutputs):
raise ControlDimension("incorrect covariance matrix dimensions")

# P, E, LT = care(A.T, C.T, G @ Q @ G.T, R)
P, E, LT = care(A.T, C.T, np.dot(np.dot(G, Q), G.T), R)
return _ssmatrix(LT.T), _ssmatrix(P), E


Expand Down Expand Up @@ -394,17 +458,17 @@ def lqr(*args, **keywords):

The function can be called with either 3, 4, or 5 arguments:

* ``lqr(sys, Q, R)``
* ``lqr(sys, Q, R, N)``
* ``lqr(A, B, Q, R)``
* ``lqr(A, B, Q, R, N)``
* ``K, S, E = lqr(sys, Q, R)``
* ``K, S, E = lqr(sys, Q, R, N)``
* ``K, S, E = lqr(A, B, Q, R)``
* ``K, S, E = lqr(A, B, Q, R, N)``

where `sys` is an `LTI` object, and `A`, `B`, `Q`, `R`, and `N` are
2d arrays or matrices of appropriate dimension.
2D arrays or matrices of appropriate dimension.

Parameters
----------
A, B : 2D array
A, B : 2D array_like
Dynamics and input matrices
sys : LTI (StateSpace or TransferFunction)
Linear I/O system
Expand Down
71 changes: 71 additions & 0 deletions control/tests/statefbk_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
import numpy as np
import pytest

import control as ct
from control import lqe, pole, rss, ss, tf
from control.exception import ControlDimension
from control.mateqn import care, dare
Expand Down Expand Up @@ -338,6 +339,39 @@ def testLQR_warning(self):
with pytest.warns(UserWarning):
(K, S, E) = lqr(A, B, Q, R, N)

@slycotonly
def test_lqr_call_format(self):
# Create a random state space system for testing
sys = rss(2, 3, 2)

# Weighting matrices
Q = np.eye(sys.nstates)
R = np.eye(sys.ninputs)
N = np.zeros((sys.nstates, sys.ninputs))

# Standard calling format
Kref, Sref, Eref = lqr(sys.A, sys.B, Q, R)

# Call with system instead of matricees
K, S, E = lqr(sys, Q, R)
np.testing.assert_array_almost_equal(Kref, K)
np.testing.assert_array_almost_equal(Sref, S)
np.testing.assert_array_almost_equal(Eref, E)

# Pass a cross-weighting matrix
K, S, E = lqr(sys, Q, R, N)
np.testing.assert_array_almost_equal(Kref, K)
np.testing.assert_array_almost_equal(Sref, S)
np.testing.assert_array_almost_equal(Eref, E)

# Inconsistent system dimensions
with pytest.raises(ct.ControlDimension, match="inconsistent system"):
K, S, E = lqr(sys.A, sys.C, Q, R)

# incorrect covariance matrix dimensions
with pytest.raises(ct.ControlDimension, match="incorrect weighting"):
K, S, E = lqr(sys.A, sys.B, sys.C, R, Q)

def check_LQE(self, L, P, poles, G, QN, RN):
P_expected = asmatarrayout(np.sqrt(G.dot(QN.dot(G).dot(RN))))
L_expected = asmatarrayout(P_expected / RN)
Expand All @@ -352,6 +386,43 @@ def test_LQE(self, matarrayin):
L, P, poles = lqe(A, G, C, QN, RN)
self.check_LQE(L, P, poles, G, QN, RN)

@slycotonly
def test_lqe_call_format(self):
# Create a random state space system for testing
sys = rss(4, 3, 2)

# Covariance matrices
Q = np.eye(sys.ninputs)
R = np.eye(sys.noutputs)
N = np.zeros((sys.ninputs, sys.noutputs))

# Standard calling format
Lref, Pref, Eref = lqe(sys.A, sys.B, sys.C, Q, R)

# Call with system instead of matricees
L, P, E = lqe(sys, Q, R)
np.testing.assert_array_almost_equal(Lref, L)
np.testing.assert_array_almost_equal(Pref, P)
np.testing.assert_array_almost_equal(Eref, E)

# Compare state space and transfer function (SISO only)
sys_siso = rss(4, 1, 1)
L_ss, P_ss, E_ss = lqe(sys_siso, np.eye(1), np.eye(1))
L_tf, P_tf, E_tf = lqe(tf(sys_siso), np.eye(1), np.eye(1))
np.testing.assert_array_almost_equal(E_ss, E_tf)

# Make sure we get an error if we specify N
with pytest.raises(ct.ControlNotImplemented):
L, P, E = lqe(sys, Q, R, N)

# Inconsistent system dimensions
with pytest.raises(ct.ControlDimension, match="inconsistent system"):
L, P, E = lqe(sys.A, sys.C, sys.B, Q, R)

# incorrect covariance matrix dimensions
with pytest.raises(ct.ControlDimension, match="incorrect covariance"):
L, P, E = lqe(sys.A, sys.B, sys.C, R, Q)

@slycotonly
def test_care(self, matarrayin):
"""Test stabilizing and anti-stabilizing feedbacks, continuous"""
Expand Down