From 51b5298d994e0cebe4b8c5c18f58771ddd60f68e Mon Sep 17 00:00:00 2001 From: marko1olo Date: Sat, 6 Jun 2026 06:01:59 +0400 Subject: [PATCH 1/4] Add dlqe filter-form gain option --- control/stochsys.py | 39 ++++++++++++++++++++++++------ control/tests/stochsys_test.py | 43 ++++++++++++++++++++++++++++++++++ doc/stochastic.rst | 9 +++++-- 3 files changed, 82 insertions(+), 9 deletions(-) diff --git a/control/stochsys.py b/control/stochsys.py index 22e2f738b..6a4c89fac 100644 --- a/control/stochsys.py +++ b/control/stochsys.py @@ -205,8 +205,10 @@ def dlqe(*args, **kwargs): .. 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 mean squared - estimation error x[n] - x_e[n] using the sensor measurements y. The - noise cross-correlation `NN` is set to zero when omitted. + estimation error x[n] - x_e[n] 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 ---------- @@ -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 -------- @@ -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)) @@ -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 diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py index 20e799643..406a1a464 100644 --- a/control/tests/stochsys_test.py +++ b/control/tests/stochsys_test.py @@ -85,6 +85,38 @@ 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)))) + + 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) @@ -106,6 +138,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) diff --git a/doc/stochastic.rst b/doc/stochastic.rst index 881cf234a..e90979992 100644 --- a/doc/stochastic.rst +++ b/doc/stochastic.rst @@ -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 From 6f14b6fdb34ed2e5d53cadb2cba83a5c7a6e09c5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=9F=D0=B5=D1=82=D1=83=D1=88=D0=BA=D0=BE=D0=B2=20=D0=90?= =?UTF-8?q?=2E?= <96879250+marko1olo@users.noreply.github.com> Date: Sun, 16 Aug 2026 16:17:03 +0000 Subject: [PATCH 2/4] test(dlqe): add atol=1e-12 to eigenvalue comparison for zero-eigenvalue tolerance --- control/tests/stochsys_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py index 406a1a464..09713fafa 100644 --- a/control/tests/stochsys_test.py +++ b/control/tests/stochsys_test.py @@ -109,7 +109,7 @@ def test_DLQE_return_filter_form(method): 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.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)))) From 1c0262b74ce9c342a54470550ac3217f2cd8c8f2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=9F=D0=B5=D1=82=D1=83=D1=88=D0=BA=D0=BE=D0=B2=20=D0=90?= =?UTF-8?q?=2E?= <96879250+marko1olo@users.noreply.github.com> Date: Sun, 16 Aug 2026 16:40:25 +0000 Subject: [PATCH 3/4] test(dlqe): add atol=1e-12 to E_filter eigenvalue assertion for numerical zero tolerance --- control/tests/stochsys_test.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py index 09713fafa..171d53dbc 100644 --- a/control/tests/stochsys_test.py +++ b/control/tests/stochsys_test.py @@ -112,7 +112,8 @@ def test_DLQE_return_filter_form(method): 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)))) + 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) From 5a6a19ca0a02127cda0d011f7bd6fa6294698462 Mon Sep 17 00:00:00 2001 From: marko1olo Date: Sun, 16 Aug 2026 23:47:47 +0400 Subject: [PATCH 4/4] docs(dlqe): revert overlapping intro-paragraph edit (already in #1219) Remove the intro-paragraph change that duplicated work from PR #1219 (docs-lqe-dlqe-error-covariance, now merged). Keep only the return_filter_form parameter docs and Notes section additions. --- control/stochsys.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/control/stochsys.py b/control/stochsys.py index 6a4c89fac..3718a829a 100644 --- a/control/stochsys.py +++ b/control/stochsys.py @@ -205,10 +205,8 @@ def dlqe(*args, **kwargs): .. 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 mean squared - estimation error x[n] - x_e[n] 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. + estimation error x[n] - x_e[n] using the sensor measurements y. The + noise cross-correlation `NN` is set to zero when omitted. Parameters ----------