Skip to content

Add dlqe filter-form gain option #1220

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

Open
marko1olo wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Add dlqe filter-form gain option #1220

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
45 changes: 35 additions & 10 deletions control/stochsys.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -199,14 +199,16 @@ def dlqe(*args, **kwargs):

.. math:: E\{w w^T\} = QN, E\{v v^T\} = RN, E\{w v^T\} = NN

The dlqe() function computes the observer gain matrix L such that the
stationary (non-time-varying) Kalman filter
The dlqe() function computes the one-step predictor gain matrix L such
that the stationary (non-time-varying) Kalman filter

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

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

@slivingston slivingston Jun 17, 2026

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It looks like this change is something that was proposed in a different PR: #1219
so it should not be included here, or perhaps the PRs can be consolidated into one PR.

x[n] - x_e[n] has minimum covariance using the sensor measurements y.
If `return_filter_form` is True, `dlqe` instead returns the filter-form
correction gain whose corresponding predictor gain is `A L`. The noise
cross-correlation `NN` is set to zero when omitted.

Parameters
----------
Expand All @@ -220,20 +222,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 +271,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 +322,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
36 changes: 36 additions & 0 deletions control/tests/stochsys_test.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,35 @@ 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)))
np.testing.assert_allclose(
np.sort_complex(E_filter),
np.sort_complex(np.linalg.eigvals(A @ (np.eye(2) - L_filter @ C))))

def test_lqe_discrete():
"""Test overloading of lqe operator for discrete-time systems"""
csys = ct.rss(2, 1, 1)
Expand All @@ -106,6 +135,13 @@ 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 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
View file
Open in desktop
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