Dec 30, 2019 · Dec 10, 2019 · Dec 12, 2019
diff --git a/control/statefbk.py b/control/statefbk.py
 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
    # Return the gain matrix, with MATLAB gain convention
    return _ssmatrix(-F)

 # contributed by Sawyer B. Fuller <minster@uw.edu>
 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)
    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)


 # Contributed by Roberto Bucher <roberto.bucher@supsi.ch>
 def acker(A, B, poles):
    """Pole placement using Ackermann method
    >>> 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
diff --git a/control/tests/statefbk_test.py b/control/tests/statefbk_test.py
 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
        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
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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
Expand DownExpand Up		@@ -219,6 +220,76 @@ 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 <minster@uw.edu>
		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)
		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)


		# Contributed by Roberto Bucher <roberto.bucher@supsi.ch>
		def acker(A, B, poles):
		"""Pole placement using Ackermann method
Expand DownExpand Up		@@ -307,6 +378,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
Expand Down
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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
Expand DownExpand Up		@@ -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(GQNG * 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
Expand Down