Skip to content
Closed
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
33 changes: 28 additions & 5 deletions control/stochsys.py
Original file line number Diff line number Diff line change
Expand Up @@ -220,20 +220,39 @@ 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'.
return_filter_form : bool, optional
If True, return the Kalman filter gain
:math:`P C^T (C P C^T + R_N)^{-1}` instead of the default predictor
gain :math:`A P C^T (C P C^T + R_N)^{-1}`.

Returns
-------
L : 2D array
Kalman estimator gain.
Kalman estimator gain. By default, this is the predictor-form gain.
If `return_filter_form` is True, this is the filter-form gain.
P : 2D array
Solution to Riccati equation.
Steady-state prediction error covariance (prior covariance) that
solves the Riccati equation.

.. math::

A P + P A^T - (P C^T + G N) R^{-1} (C P + N^T G^T) + G Q G^T = 0
P = A P A^T
- A P C^T (C P C^T + R_N)^{-1} C P A^T
+ G Q_N G^T

E : 1D array
Eigenvalues of estimator poles eig(A - L C).
Eigenvalues of estimator poles. With the default predictor-form gain,
these are `eig(A - L C)`. With `return_filter_form=True`, these are
`eig(A @ (I - L C))`.

Notes
-----
By default, `dlqe` returns the predictor-form gain
:math:`A P C^T (C P C^T + R_N)^{-1}`. Use
`return_filter_form=True` to return the filter-form gain
:math:`P C^T (C P C^T + R_N)^{-1}` instead. The returned covariance
`P` is the steady-state prediction error covariance used in both gain
formulas.

Examples
--------
Expand All @@ -250,8 +269,9 @@ def dlqe(*args, **kwargs):
# Process the arguments and figure out what inputs we received
#

# Get the method to use (if specified as a keyword)
# Get keywords
method = kwargs.pop('method', None)
return_filter_form = kwargs.pop('return_filter_form', False)
if kwargs:
raise TypeError("unrecognized keyword(s): ", str(kwargs))

Expand Down Expand Up @@ -300,6 +320,9 @@ 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")
if return_filter_form:
L = sp.linalg.solve((C @ P @ C.T + RN).T, (P @ C.T).T).T
return L, P, E
return LT.T, P, E


Expand Down
44 changes: 44 additions & 0 deletions control/tests/stochsys_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,39 @@ def test_DLQE(method):
L, P, poles = dlqe(A, G, C, QN, RN, method=method)
check_DLQE(L, P, poles, G, QN, RN)

@pytest.mark.parametrize("method", [None,
pytest.param('slycot', marks=pytest.mark.slycot),
'scipy'])
def test_DLQE_return_filter_form(method):
A = np.array([[0., 1.], [0., 0.5]])
G = np.eye(2)
C = np.array([[1., 0.]])
QN = np.eye(2)
RN = np.array([[1.]])

L_pred, P_pred, E_pred = dlqe(A, G, C, QN, RN, method=method)
L_filter, P_filter, E_filter = dlqe(
A, G, C, QN, RN, method=method, return_filter_form=True)
L_expected = np.linalg.solve(
(C @ P_pred @ C.T + RN).T, (P_pred @ C.T).T).T

assert np.linalg.matrix_rank(A) < A.shape[0]
assert not np.allclose(L_filter, L_pred)
np.testing.assert_allclose(P_filter, P_pred)
np.testing.assert_allclose(E_filter, E_pred)
np.testing.assert_allclose(L_filter, L_expected)
np.testing.assert_allclose(L_pred, A @ L_filter)
np.testing.assert_allclose(
np.sort_complex(E_pred),
np.sort_complex(np.linalg.eigvals(A - L_pred @ C)), atol=1e-12)
np.testing.assert_allclose(
np.sort_complex(E_filter),
np.sort_complex(np.linalg.eigvals(A @ (np.eye(2) - L_filter @ C))),
atol=1e-12)

L_pred_false, _, _ = dlqe(A, G, C, QN, RN, method=method, return_filter_form=False)
np.testing.assert_allclose(L_pred, L_pred_false)

def test_lqe_discrete():
"""Test overloading of lqe operator for discrete-time systems"""
csys = ct.rss(2, 1, 1)
Expand All @@ -106,6 +139,17 @@ def test_lqe_discrete():
np.testing.assert_almost_equal(S_lqe, S_dlqe)
np.testing.assert_almost_equal(E_lqe, E_dlqe)

# Optional dlqe() keywords should pass through lqe() for DT systems
K_lqe, S_lqe, E_lqe = ct.lqe(dsys, Q, R, return_filter_form=True)
K_dlqe, S_dlqe, E_dlqe = ct.dlqe(dsys, Q, R, return_filter_form=True)
np.testing.assert_almost_equal(K_lqe, K_dlqe)
np.testing.assert_almost_equal(S_lqe, S_dlqe)
np.testing.assert_almost_equal(E_lqe, E_dlqe)

# Calling lqe with return_filter_form=True for a continuous-time system should raise TypeError
with pytest.raises(TypeError, match="unrecognized keyword"):
ct.lqe(csys, Q, R, return_filter_form=True)

# Calling lqe() with no timebase should call lqe()
asys = ct.ss(csys.A, csys.B, csys.C, csys.D, dt=None)
K_asys, S_asys, E_asys = ct.lqe(asys, Q, R)
Expand Down
9 changes: 7 additions & 2 deletions doc/stochastic.rst
Original file line number Diff line number Diff line change
Expand Up @@ -183,9 +183,14 @@ called in several forms:
where :code:`sys` is an :class:`LTI` object, and `A`, `G`, `C`, `QN`, `RN`,
and `NN` are 2D arrays of appropriate dimension. If :code:`sys` is a
discrete-time system, the first two forms will compute the discrete
time optimal controller. For the second two forms, the :func:`dlqr`
time optimal estimator. For the second two forms, the :func:`dlqe`
function can be used. Additional arguments and details are given on
the :func:`lqr` and :func:`dlqr` documentation pages.
the :func:`lqe` and :func:`dlqe` documentation pages.

For discrete-time systems, :func:`dlqe` returns the predictor-form gain
:math:`A P C^T (C P C^T + R_N)^{-1}` by default. Use
``return_filter_form=True`` to return the filter-form gain
:math:`P C^T (C P C^T + R_N)^{-1}` instead.

.. testsetup:: kalman

Expand Down
Loading