Add dlqe filter-form gain option

marko1olo · marko1olo · commit 5a342d7d02c9 · 2026-06-06T06:01:59.000+04:00
diff --git a/control/stochsys.py b/control/stochsys.py
@@ -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
+    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
     ----------
@@ -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
@@ -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)
@@ -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)
diff --git 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
 
