From 052edec8baa4344d4e94ee1f557214d5bdc04a52 Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Tue, 10 Dec 2019 09:15:29 -0800 Subject: [PATCH 1/2] added preliminary version of lqe function, which calculates the gain for a steady-state Kalman filter. --- control/statefbk.py | 74 ++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 73 insertions(+), 1 deletion(-) diff --git a/control/statefbk.py b/control/statefbk.py index cd69190d3..e6f080639 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -43,10 +43,11 @@ import numpy as np import scipy as sp from . import statesp +from .mateqn import care from .statesp import _ssmatrix from .exception import ControlSlycot, ControlArgument, ControlDimension -__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'acker'] +__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'lqe', 'acker'] # Pole placement @@ -219,6 +220,73 @@ def place_varga(A, B, p, dtime=False, alpha=None): # Return the gain matrix, with MATLAB gain convention return _ssmatrix(-F) +# contributed by Sawyer B. Fuller +def lqe(A, G, C, QN, RN, NN=None): + """lqe(A, G, C, QN, RN, [, N]) + + Linear quadratic estimator design (Kalman filter) for continuous-time + systems. Given the system + + Given the system + .. math:: + x = Ax + Bu + Gw + y = Cx + Du + v + + with unbiased process noise w and measurement noise v with covariances + + .. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN + + The lqe() function computes the observer gain matrix L such that the + stationary (non-time-varying) Kalman filter + + .. math:: x_e = A x_e + B u + L(y - C x_e - D u) + + produces a state estimate that x_e that minimizes the expected squared error + using the sensor measurements y. The noise cross-correlation `NN` is set to + zero when omitted. + + Parameters + ---------- + A, G: 2-d array + Dynamics and noise input matrices + QN, RN: 2-d array + Process and sensor noise covariance matrices + NN: 2-d array, optional + Cross covariance matrix + + Returns + ------- + L: 2D array + Kalman estimator gain + P: 2D array + Solution to 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 + E: 1D array + Eigenvalues of estimator poles eig(A - L C) + + + Examples + -------- + >>> K, P, E = lqe(A, G, C, QN, RN) + >>> K, P, E = lqe(A, G, C, QN, RN, NN) + + See Also + -------- + lqr + """ + + # TODO: incorporate cross-covariance NN, something like this, + # which doesn't work for some reason + #if NN is None: + # NN = np.zeros(QN.size(0),RN.size(1)) + #NG = G @ NN + + #LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN) + P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN) + return _ssmatrix(LT.T), _ssmatrix(P), _ssmatrix(E) + + # Contributed by Roberto Bucher def acker(A, B, poles): """Pole placement using Ackermann method @@ -307,6 +375,10 @@ def lqr(*args, **keywords): >>> K, S, E = lqr(sys, Q, R, [N]) >>> K, S, E = lqr(A, B, Q, R, [N]) + See Also + -------- + lqe + """ # Make sure that SLICOT is installed From e3125696a56131bc49ddba30b5e292965f2d1ebe Mon Sep 17 00:00:00 2001 From: Sawyer Fuller Date: Thu, 12 Dec 2019 11:05:01 -0800 Subject: [PATCH 2/2] LQE: added python 2 compatibility and unit test --- control/statefbk.py | 5 ++++- control/tests/statefbk_test.py | 16 +++++++++++++++- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/control/statefbk.py b/control/statefbk.py index e6f080639..d90bfd586 100644 --- a/control/statefbk.py +++ b/control/statefbk.py @@ -283,7 +283,10 @@ def lqe(A, G, C, QN, RN, NN=None): #NG = G @ NN #LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN) - P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN) + #P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN) + A, G, C = np.array(A, ndmin=2), np.array(G, ndmin=2), np.array(C, ndmin=2) + QN, RN = np.array(QN, ndmin=2), np.array(RN, ndmin=2) + P, E, LT = care(A.T, C.T, np.dot(np.dot(G, QN), G.T), RN) return _ssmatrix(LT.T), _ssmatrix(P), _ssmatrix(E) diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py index 66dce2b12..133631232 100644 --- a/control/tests/statefbk_test.py +++ b/control/tests/statefbk_test.py @@ -6,7 +6,7 @@ from __future__ import print_function import unittest import numpy as np -from control.statefbk import ctrb, obsv, place, place_varga, lqr, gram, acker +from control.statefbk import ctrb, obsv, place, place_varga, lqr, lqe, gram, acker from control.matlab import * from control.exception import slycot_check, ControlDimension from control.mateqn import care, dare @@ -299,6 +299,20 @@ def test_LQR_3args(self): K, S, poles = lqr(sys, Q, R) self.check_LQR(K, S, poles, Q, R) + def check_LQE(self, L, P, poles, G, QN, RN): + P_expected = np.array(np.sqrt(G*QN*G * RN)) + L_expected = P_expected / RN + poles_expected = np.array([-L_expected], ndmin=2) + np.testing.assert_array_almost_equal(P, P_expected) + np.testing.assert_array_almost_equal(L, L_expected) + np.testing.assert_array_almost_equal(poles, poles_expected) + + @unittest.skipIf(not slycot_check(), "slycot not installed") + def test_LQE(self): + A, G, C, QN, RN = 0., .1, 1., 10., 2. + L, P, poles = lqe(A, G, C, QN, RN) + self.check_LQE(L, P, poles, G, QN, RN) + @unittest.skipIf(not slycot_check(), "slycot not installed") def test_care(self): #unit test for stabilizing and anti-stabilizing feedbacks