Skip to content

added preliminary version of lqe function, which calculates the gain … #351

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
Dec 30, 2019
Merged

added preliminary version of lqe function, which calculates the gain … #351

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
052edec
added preliminary version of lqe function, which calculates the gain …
sawyerbfuller Dec 10, 2019
e312569
LQE: added python 2 compatibility and unit test
sawyerbfuller Dec 12, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
77 changes: 76 additions & 1 deletion control/statefbk.py
View file
Open in desktop
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 Down Expand 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 Down Expand 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
16 changes: 15 additions & 1 deletion control/tests/statefbk_test.py
View file
Open in desktop
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 Down Expand 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(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
Expand Down