Skip to content

Commit 4a4f0cb

Browse files
dongxuelian2dongxuelian2
committed
Add optional measurement-update gain to dlqe
1 parent b4cf736 commit 4a4f0cb

3 files changed

Lines changed: 184 additions & 17 deletions

File tree

‎control/stochsys.py‎

Lines changed: 47 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,10 @@ def lqe(*args, symmetric_kwargs=None, **kwargs):
8383
Set the method used for computing the result. Current methods are
8484
'slycot' and 'scipy'. If set to None (default), try 'slycot' first
8585
and then 'scipy'.
86+
return_filter_form : bool, optional
87+
For a discrete-time `sys`, return the measurement-update gain instead
88+
of the predictor gain. Default is False. See `dlqe` for details.
89+
Only supported for discrete-time systems.
8690
symmetric_kwargs : dict, optional
8791
Keyword arguments passed to `scipy.linalg.issymmetric` or
8892
`scipy.linalg.ishermitian`.
@@ -132,7 +136,7 @@ def lqe(*args, symmetric_kwargs=None, **kwargs):
132136
# If we were passed a discrete-time system as the first arg, use dlqe()
133137
if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
134138
# Call dlqe
135-
return dlqe(*args, **kwargs)
139+
return dlqe(*args, symmetric_kwargs=symmetric_kwargs, **kwargs)
136140

137141
# Get the method to use (if specified as a keyword)
138142
method = kwargs.pop('method', None)
@@ -186,8 +190,8 @@ def lqe(*args, symmetric_kwargs=None, **kwargs):
186190

187191

188192
# contributed by Sawyer B. Fuller <minster@uw.edu>
189-
def dlqe(*args, symmetric_kwargs=None, **kwargs):
190-
r"""dlqe(A, G, C, QN, RN, [, N])
193+
def dlqe(*args, return_filter_form=False, symmetric_kwargs=None, **kwargs):
194+
r"""dlqe(A, G, C, QN, RN, [, NN])
191195
192196
Discrete-time linear quadratic estimator (Kalman filter).
193197
@@ -207,14 +211,31 @@ def dlqe(*args, symmetric_kwargs=None, **kwargs):
207211
208212
.. math:: x_e[n+1] = A x_e[n] + B u[n] + L(y[n] - C x_e[n] - D u[n])
209213
210-
produces a state estimate x_e[n] that minimizes the mean squared
211-
estimation error x[n] - x_e[n] using the sensor measurements y. The
212-
noise cross-correlation `NN` is set to zero when omitted.
214+
produces a prior state estimate x_e[n] using measurements through y[n-1].
215+
This estimate minimizes the mean squared estimation error x[n] - x_e[n].
216+
The noise cross-correlation `NN` is set to zero when omitted.
217+
218+
If `return_filter_form` is True, the returned gain is instead the
219+
measurement-update gain L_f for the current state estimate:
220+
221+
.. math::
222+
223+
x_f[n] &= x_e[n] + L_f (y[n] - C x_e[n] - D u[n]) \\
224+
L_f &= P C^T (C P C^T + RN)^{-1}
225+
226+
The next prior estimate is A x_f[n] + B u[n]. The predictor gain is
227+
L = A L_f, but computing L_f does not require A to be invertible.
228+
229+
The system matrices can also be supplied as a discrete-time `StateSpace`
230+
system: ``L, P, E = dlqe(sys, QN, RN)``.
213231
214232
Parameters
215233
----------
216234
A, G, C : 2D array_like
217235
Dynamics, process noise (disturbance), and output matrices.
236+
sys : `StateSpace`
237+
Discrete-time linear I/O system, with the process noise input taken
238+
as the system input.
218239
QN, RN : 2D array_like
219240
Process and sensor noise covariance matrices.
220241
NN : 2D array, optional
@@ -223,28 +244,40 @@ def dlqe(*args, symmetric_kwargs=None, **kwargs):
223244
Set the method used for computing the result. Current methods are
224245
'slycot' and 'scipy'. If set to None (default), try 'slycot'
225246
first and then 'scipy'.
247+
return_filter_form : bool, optional
248+
Return the measurement-update gain L_f if True. If False (default),
249+
return the predictor gain L = A L_f. The covariance `P` and poles `E`
250+
are the same for both forms.
226251
symmetric_kwargs : dict, optional
227252
Keyword arguments passed to `scipy.linalg.issymmetric` or
228253
`scipy.linalg.ishermitian`.
229254
230255
Returns
231256
-------
232257
L : 2D array
233-
Kalman estimator gain.
258+
Kalman predictor gain, or measurement-update gain if
259+
`return_filter_form` is True.
234260
P : 2D array
235-
Solution to Riccati equation.
261+
Prior estimation error covariance (before the measurement update),
262+
given by the solution to the discrete-time Riccati equation:
236263
237264
.. math::
238265
239-
A P + P A^T - (P C^T + G N) R^{-1} (C P + N^T G^T) + G Q G^T = 0
266+
P &= A P A^T + G QN G^T \\
267+
&\quad - A P C^T (C P C^T + RN)^{-1} C P A^T
240268
241269
E : 1D array
242-
Eigenvalues of estimator poles eig(A - L C).
270+
Eigenvalues of the estimator error dynamics: eig(A - L C) for the
271+
predictor form, or eig(A - A L C) for the filter form.
243272
244273
Examples
245274
--------
246275
>>> L, P, E = dlqe(A, G, C, QN, RN) # doctest: +SKIP
247276
>>> L, P, E = dlqe(A, G, C, QN, RN, NN) # doctest: +SKIP
277+
>>> import control as ct
278+
>>> M, P, E = ct.dlqe(0, 1, 1, 1, 1, return_filter_form=True)
279+
>>> M
280+
array([[0.5]])
248281
249282
See Also
250283
--------
@@ -306,6 +339,10 @@ def dlqe(*args, symmetric_kwargs=None, **kwargs):
306339
# Compute the result (dimension and symmetry checking done in dare())
307340
P, E, LT = dare(A.T, C.T, G @ QN @ G.T, RN, method=method,
308341
_Bs="C", _Qs="QN", _Rs="RN", _Ss="NN", symmetric_kwargs=symmetric_kwargs)
342+
if return_filter_form:
343+
# Solve for the measurement-update gain using the prior covariance.
344+
# This also works when A is singular.
345+
return np.linalg.solve(C @ P @ C.T + RN, C @ P).T, P, E
309346
return LT.T, P, E
310347

311348

‎control/tests/stochsys_test.py‎

Lines changed: 102 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -59,7 +59,9 @@ def test_lqe_symmetric_kwargs():
5959
symmetric_kwargs={"rtol": 1e-12},
6060
)
6161

62-
def test_dlqe_symmetric_kwargs():
62+
@pytest.mark.parametrize("return_filter_form", [False, True])
63+
@pytest.mark.parametrize("cdlqe", [lqe, dlqe])
64+
def test_dlqe_symmetric_kwargs(cdlqe, return_filter_form):
6365
A = np.array([
6466
[0.5, 0.],
6567
[0., 0.4]
@@ -71,14 +73,16 @@ def test_dlqe_symmetric_kwargs():
7173
[0.2 + 1e-15, 1.]
7274
])
7375
RN = np.eye(2)
76+
sys = ct.ss(A, G, C, 0, dt=True)
7477

7578
# Exact symmetry check should fail
7679
with pytest.raises(ControlArgument, match="symmetric"):
77-
dlqe(A, G, C, QN, RN, method="scipy")
80+
cdlqe(sys, QN, RN, method="scipy",
81+
return_filter_form=return_filter_form)
7882

7983
# Passing rtol should allow the nearly symmetric matrix
80-
dlqe(
81-
A, G, C, QN, RN,
84+
cdlqe(
85+
sys, QN, RN, return_filter_form=return_filter_form,
8286
method="scipy",
8387
symmetric_kwargs={"rtol": 1e-12},
8488
)
@@ -133,6 +137,100 @@ def test_DLQE(method):
133137
L, P, poles = dlqe(A, G, C, QN, RN, method=method)
134138
check_DLQE(L, P, poles, G, QN, RN)
135139

140+
141+
@pytest.mark.parametrize("method", [
142+
None, pytest.param('slycot', marks=pytest.mark.slycot), 'scipy'])
143+
@pytest.mark.parametrize("a", [0., 0.7, 1.2])
144+
@pytest.mark.parametrize("return_filter_form", [False, True])
145+
def test_dlqe_scalar_gain(method, a, return_filter_form):
146+
# The scalar DARE is P**2 + (r * (1 - a**2) - q) * P - q * r = 0.
147+
g, qn, r = 0.3, 2., 0.7
148+
q = g**2 * qn
149+
b = r * (1 - a**2) - q
150+
p = (-b + np.sqrt(b**2 + 4 * q * r)) / 2
151+
m = p / (p + r)
152+
153+
L, P, E = dlqe(a, g, 1, qn, r, method=method,
154+
return_filter_form=return_filter_form)
155+
assert L.shape == P.shape == (1, 1)
156+
assert E.shape == (1,)
157+
np.testing.assert_allclose(P, [[p]], rtol=1e-10)
158+
np.testing.assert_allclose(L, [[m if return_filter_form else a * m]],
159+
rtol=1e-10, atol=1e-14)
160+
np.testing.assert_allclose(E, [a * (1 - m)], rtol=1e-10, atol=1e-14)
161+
162+
163+
@pytest.mark.parametrize("method", [
164+
None, pytest.param('slycot', marks=pytest.mark.slycot), 'scipy'])
165+
@pytest.mark.parametrize("A", [
166+
[[1.1, 0.2, 0], [0, 0.7, 0.1], [0, 0, 0.3]],
167+
[[1.1, 0.2, 0], [0, 0.7, 0.1], [0, 0, 0]],
168+
])
169+
def test_dlqe_filter_form_covariance(A, method):
170+
A = np.array(A)
171+
G = np.array([[1, 0.25], [0.5, 1], [1, -0.5]])
172+
C = np.array([[1, 0, 0.25], [0.5, 1, 0]])
173+
QN = np.array([[0.5, 0.125], [0.125, 0.75]])
174+
RN = np.array([[0.75, 0.25], [0.25, 1.5]])
175+
176+
# Independently converge the measurement update and prediction, using
177+
# the Joseph form to compute the posterior covariance.
178+
prior = np.eye(3)
179+
for _ in range(200):
180+
M = np.linalg.solve(C @ prior @ C.T + RN, C @ prior).T
181+
correction = np.eye(3) - M @ C
182+
posterior = correction @ prior @ correction.T + M @ RN @ M.T
183+
next_prior = A @ posterior @ A.T + G @ QN @ G.T
184+
if np.linalg.norm(next_prior - prior) < 1e-13:
185+
break
186+
prior = next_prior
187+
else:
188+
pytest.fail("Kalman covariance iteration did not converge")
189+
190+
L, P, E = dlqe(A, G, C, QN, RN, method=method)
191+
Lpred, Ppred, Epred = dlqe(A, G, C, QN, RN, method=method,
192+
return_filter_form=False)
193+
Lfilter, Pfilter, Efilter = dlqe(A, G, C, QN, RN, method=method,
194+
return_filter_form=True)
195+
np.testing.assert_allclose(P, prior, rtol=1e-10, atol=1e-12)
196+
np.testing.assert_allclose(Lfilter, M, rtol=1e-10, atol=1e-12)
197+
np.testing.assert_allclose(L, A @ M, rtol=1e-10, atol=1e-12)
198+
np.testing.assert_allclose(Lpred, L)
199+
np.testing.assert_allclose(Ppred, P)
200+
np.testing.assert_allclose(Pfilter, P)
201+
np.testing.assert_allclose(Epred, E)
202+
np.testing.assert_allclose(Efilter, E)
203+
np.testing.assert_allclose(
204+
np.sort_complex(E), np.sort_complex(np.linalg.eigvals(A @ correction)),
205+
rtol=1e-10, atol=1e-12)
206+
assert np.max(np.abs(E)) < 1
207+
208+
# The two-step filter and the existing predictor give the same next
209+
# estimate, even when A is singular and cannot recover M from A @ M.
210+
xhat, innovation = np.array([0.1, -0.2, 0.3]), np.array([0.4, -0.5])
211+
np.testing.assert_allclose(
212+
A @ (xhat + Lfilter @ innovation), A @ xhat + L @ innovation)
213+
np.testing.assert_allclose(
214+
P, A @ posterior @ A.T + G @ QN @ G.T, rtol=1e-10, atol=1e-12)
215+
216+
217+
@pytest.mark.parametrize("return_filter_form", [False, True])
218+
@pytest.mark.parametrize("dt", [True, 0.1])
219+
def test_dlqe_filter_form_call_format(return_filter_form, dt):
220+
sys = ct.ss([[0.7, 0.2], [0, 0]], [[1], [0.3]], [[1, 0.4]], 0, dt=dt)
221+
expected = dlqe(sys.A, sys.B, sys.C, 0.5, 0.8,
222+
return_filter_form=return_filter_form)
223+
for cdlqe in (dlqe, lqe):
224+
result = cdlqe(sys, 0.5, 0.8, return_filter_form=return_filter_form)
225+
for actual, reference in zip(result, expected):
226+
np.testing.assert_allclose(actual, reference)
227+
with pytest.raises(ct.ControlNotImplemented, match="cross-covariance"):
228+
cdlqe(sys, 0.5, 0.8, 0, return_filter_form=return_filter_form)
229+
with pytest.raises(TypeError, match="unrecognized keyword"):
230+
cdlqe(sys, 0.5, 0.8, return_filter_form=return_filter_form,
231+
unknown=True)
232+
233+
136234
def test_lqe_discrete():
137235
"""Test overloading of lqe operator for discrete-time systems"""
138236
csys = ct.rss(2, 1, 1)

‎doc/stochastic.rst‎

Lines changed: 35 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -182,10 +182,42 @@ called in several forms:
182182

183183
where :code:`sys` is an :class:`LTI` object, and `A`, `G`, `C`, `QN`, `RN`,
184184
and `NN` are 2D arrays of appropriate dimension. If :code:`sys` is a
185-
discrete-time system, the first two forms will compute the discrete
186-
time optimal controller. For the second two forms, the :func:`dlqr`
185+
discrete-time system, the first two forms will compute the discrete-time
186+
optimal estimator. For the second two forms, the :func:`dlqe`
187187
function can be used. Additional arguments and details are given on
188-
the :func:`lqr` and :func:`dlqr` documentation pages.
188+
the :func:`lqe` and :func:`dlqe` documentation pages.
189+
190+
In discrete time, :func:`dlqe` returns the predictor gain :math:`L` by
191+
default. For the system :math:`x[n+1] = A x[n] + B u[n] + G w[n]`,
192+
:math:`y[n] = C x[n] + D u[n] + v[n]`, the prior estimate uses measurements
193+
through time :math:`n-1` and advances according to
194+
195+
.. math::
196+
197+
\hat{x}[n+1|n] = A \hat{x}[n|n-1] + B u[n]
198+
+ L (y[n] - C \hat{x}[n|n-1] - D u[n]).
199+
200+
To update the current state estimate using :math:`y[n]`, set
201+
`return_filter_form` to True. This returns the measurement-update gain
202+
:math:`M = P C^T (C P C^T + RN)^{-1}`, which is related to the predictor
203+
gain by :math:`L = A M`, and can be computed even when :math:`A` is singular:
204+
205+
.. code::
206+
207+
M, P, E = ct.dlqe(A, G, C, QN, RN, return_filter_form=True)
208+
209+
The two-step filter uses this gain as follows:
210+
211+
.. math::
212+
213+
\hat{x}[n|n] &= \hat{x}[n|n-1]
214+
+ M (y[n] - C \hat{x}[n|n-1] - D u[n]), \\
215+
\hat{x}[n+1|n] &= A \hat{x}[n|n] + B u[n].
216+
217+
Both gain forms return the same prior covariance `P` and estimator poles
218+
`E`. The posterior covariance is :math:`(I - M C) P`, and the poles are
219+
the eigenvalues of :math:`A (I - M C)`. The option is also supported by
220+
:func:`lqe` when passed a discrete-time system.
189221

190222
.. testsetup:: kalman
191223

0 commit comments

Comments
 (0)