diff --git a/control/stochsys.py b/control/stochsys.py index 22e2f738b..3718a829a 100644 --- a/control/stochsys.py +++ b/control/stochsys.py @@ -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 -------- @@ -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)) @@ -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 diff --git a/control/tests/stochsys_test.py b/control/tests/stochsys_test.py index 20e799643..171d53dbc 100644 --- a/control/tests/stochsys_test.py +++ b/control/tests/stochsys_test.py @@ -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) @@ -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) 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