Movatterモバイル変換

[0]ホーム
URL:

Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

 Sign up
Appearance settings

Commit81a4f38

Browse files
authored
Merge pull request#673 from murrayrm/enh-lqe
Improved lqe calling functionality
2 parents382a297 +31729c8 commit81a4f38

File tree

2 files changed

+157
-22
lines changed

2 files changed

+157
-22
lines changed

‎control/statefbk.py

Lines changed: 86 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -44,8 +44,10 @@
4444

4545
from .importstatesp
4646
from .mateqnimportcare
47-
from .statespimport_ssmatrix
48-
from .exceptionimportControlSlycot,ControlArgument,ControlDimension
47+
from .statespimport_ssmatrix,_convert_to_statespace
48+
from .ltiimportLTI
49+
from .exceptionimportControlSlycot,ControlArgument,ControlDimension, \
50+
ControlNotImplemented
4951

5052
# Make sure we have access to the right slycot routines
5153
try:
@@ -257,8 +259,8 @@ def place_varga(A, B, p, dtime=False, alpha=None):
257259

258260

259261
# contributed by Sawyer B. Fuller <minster@uw.edu>
260-
deflqe(A,G,C,QN,RN,NN=None):
261-
"""lqe(A, G, C,QN, RN, [, N])
262+
deflqe(*args,**keywords):
263+
"""lqe(A, G, C,Q, R, [, N])
262264
263265
Linear quadratic estimator design (Kalman filter) for continuous-time
264266
systems. Given the system
@@ -270,25 +272,38 @@ def lqe(A, G, C, QN, RN, NN=None):
270272
271273
with unbiased process noise w and measurement noise v with covariances
272274
273-
.. math:: E{ww'} =QN, E{vv'} =RN, E{wv'} =NN
275+
.. math:: E{ww'} =Q, E{vv'} =R, E{wv'} =N
274276
275277
The lqe() function computes the observer gain matrix L such that the
276278
stationary (non-time-varying) Kalman filter
277279
278280
.. math:: x_e = A x_e + B u + L(y - C x_e - D u)
279281
280282
produces a state estimate x_e that minimizes the expected squared error
281-
using the sensor measurements y. The noise cross-correlation `NN` is
283+
using the sensor measurements y. The noise cross-correlation `N` is
282284
set to zero when omitted.
283285
286+
The function can be called with either 3, 4, 5, or 6 arguments:
287+
288+
* ``L, P, E = lqe(sys, Q, R)``
289+
* ``L, P, E = lqe(sys, Q, R, N)``
290+
* ``L, P, E = lqe(A, G, C, Q, R)``
291+
* ``L, P, E = lqe(A, B, C, Q, R, N)``
292+
293+
where `sys` is an `LTI` object, and `A`, `G`, `C`, `Q`, `R`, and `N` are
294+
2D arrays or matrices of appropriate dimension.
295+
284296
Parameters
285297
----------
286-
A, G : 2D array_like
287-
Dynamics and noise input matrices
288-
QN, RN : 2D array_like
298+
A, G, C : 2D array_like
299+
Dynamics, process noise (disturbance), and output matrices
300+
sys : LTI (StateSpace or TransferFunction)
301+
Linear I/O system, with the process noise input taken as the system
302+
input.
303+
Q, R : 2D array_like
289304
Process and sensor noise covariance matrices
290-
NN : 2D array, optional
291-
Cross covariance matrix
305+
N : 2D array, optional
306+
Cross covariance matrix. Not currently implemented.
292307
293308
Returns
294309
-------
@@ -326,11 +341,60 @@ def lqe(A, G, C, QN, RN, NN=None):
326341
# NN = np.zeros(QN.size(0),RN.size(1))
327342
# NG = G @ NN
328343

329-
# LT, P, E = lqr(A.T, C.T, G @ QN @ G.T, RN)
330-
# P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN)
331-
A,G,C=np.array(A,ndmin=2),np.array(G,ndmin=2),np.array(C,ndmin=2)
332-
QN,RN=np.array(QN,ndmin=2),np.array(RN,ndmin=2)
333-
P,E,LT=care(A.T,C.T,np.dot(np.dot(G,QN),G.T),RN)
344+
#
345+
# Process the arguments and figure out what inputs we received
346+
#
347+
348+
# Get the system description
349+
if (len(args)<3):
350+
raiseControlArgument("not enough input arguments")
351+
352+
try:
353+
sys=args[0]# Treat the first argument as a system
354+
ifisinstance(sys,LTI):
355+
# Convert LTI system to state space
356+
sys=_convert_to_statespace(sys)
357+
358+
# Extract A, G (assume disturbances come through input), and C
359+
A=np.array(sys.A,ndmin=2,dtype=float)
360+
G=np.array(sys.B,ndmin=2,dtype=float)
361+
C=np.array(sys.C,ndmin=2,dtype=float)
362+
index=1
363+
364+
exceptAttributeError:
365+
# Arguments should be A and B matrices
366+
A=np.array(args[0],ndmin=2,dtype=float)
367+
G=np.array(args[1],ndmin=2,dtype=float)
368+
C=np.array(args[2],ndmin=2,dtype=float)
369+
index=3
370+
371+
# Get the weighting matrices (converting to matrices, if needed)
372+
Q=np.array(args[index],ndmin=2,dtype=float)
373+
R=np.array(args[index+1],ndmin=2,dtype=float)
374+
375+
# Get the cross-covariance matrix, if given
376+
if (len(args)>index+2):
377+
N=np.array(args[index+2],ndmin=2,dtype=float)
378+
raiseControlNotImplemented("cross-covariance not implemented")
379+
380+
else:
381+
N=np.zeros((Q.shape[0],R.shape[1]))
382+
383+
# Check dimensions for consistency
384+
nstates=A.shape[0]
385+
ninputs=G.shape[1]
386+
noutputs=C.shape[0]
387+
if (A.shape[0]!=nstatesorA.shape[1]!=nstatesor
388+
G.shape[0]!=nstatesorC.shape[1]!=nstates):
389+
raiseControlDimension("inconsistent system dimensions")
390+
391+
elif (Q.shape[0]!=ninputsorQ.shape[1]!=ninputsor
392+
R.shape[0]!=noutputsorR.shape[1]!=noutputsor
393+
N.shape[0]!=ninputsorN.shape[1]!=noutputs):
394+
raiseControlDimension("incorrect covariance matrix dimensions")
395+
396+
# P, E, LT = care(A.T, C.T, G @ Q @ G.T, R)
397+
P,E,LT=care(A.T,C.T,np.dot(np.dot(G,Q),G.T),R)
334398
return_ssmatrix(LT.T),_ssmatrix(P),E
335399

336400

@@ -394,17 +458,17 @@ def lqr(*args, **keywords):
394458
395459
The function can be called with either 3, 4, or 5 arguments:
396460
397-
* ``lqr(sys, Q, R)``
398-
* ``lqr(sys, Q, R, N)``
399-
* ``lqr(A, B, Q, R)``
400-
* ``lqr(A, B, Q, R, N)``
461+
* ``K, S, E =lqr(sys, Q, R)``
462+
* ``K, S, E =lqr(sys, Q, R, N)``
463+
* ``K, S, E =lqr(A, B, Q, R)``
464+
* ``K, S, E =lqr(A, B, Q, R, N)``
401465
402466
where `sys` is an `LTI` object, and `A`, `B`, `Q`, `R`, and `N` are
403-
2d arrays or matrices of appropriate dimension.
467+
2D arrays or matrices of appropriate dimension.
404468
405469
Parameters
406470
----------
407-
A, B : 2Darray
471+
A, B : 2Darray_like
408472
Dynamics and input matrices
409473
sys : LTI (StateSpace or TransferFunction)
410474
Linear I/O system

‎control/tests/statefbk_test.py

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
importnumpyasnp
77
importpytest
88

9+
importcontrolasct
910
fromcontrolimportlqe,pole,rss,ss,tf
1011
fromcontrol.exceptionimportControlDimension
1112
fromcontrol.mateqnimportcare,dare
@@ -338,6 +339,39 @@ def testLQR_warning(self):
338339
withpytest.warns(UserWarning):
339340
(K,S,E)=lqr(A,B,Q,R,N)
340341

342+
@slycotonly
343+
deftest_lqr_call_format(self):
344+
# Create a random state space system for testing
345+
sys=rss(2,3,2)
346+
347+
# Weighting matrices
348+
Q=np.eye(sys.nstates)
349+
R=np.eye(sys.ninputs)
350+
N=np.zeros((sys.nstates,sys.ninputs))
351+
352+
# Standard calling format
353+
Kref,Sref,Eref=lqr(sys.A,sys.B,Q,R)
354+
355+
# Call with system instead of matricees
356+
K,S,E=lqr(sys,Q,R)
357+
np.testing.assert_array_almost_equal(Kref,K)
358+
np.testing.assert_array_almost_equal(Sref,S)
359+
np.testing.assert_array_almost_equal(Eref,E)
360+
361+
# Pass a cross-weighting matrix
362+
K,S,E=lqr(sys,Q,R,N)
363+
np.testing.assert_array_almost_equal(Kref,K)
364+
np.testing.assert_array_almost_equal(Sref,S)
365+
np.testing.assert_array_almost_equal(Eref,E)
366+
367+
# Inconsistent system dimensions
368+
withpytest.raises(ct.ControlDimension,match="inconsistent system"):
369+
K,S,E=lqr(sys.A,sys.C,Q,R)
370+
371+
# incorrect covariance matrix dimensions
372+
withpytest.raises(ct.ControlDimension,match="incorrect weighting"):
373+
K,S,E=lqr(sys.A,sys.B,sys.C,R,Q)
374+
341375
defcheck_LQE(self,L,P,poles,G,QN,RN):
342376
P_expected=asmatarrayout(np.sqrt(G.dot(QN.dot(G).dot(RN))))
343377
L_expected=asmatarrayout(P_expected/RN)
@@ -352,6 +386,43 @@ def test_LQE(self, matarrayin):
352386
L,P,poles=lqe(A,G,C,QN,RN)
353387
self.check_LQE(L,P,poles,G,QN,RN)
354388

389+
@slycotonly
390+
deftest_lqe_call_format(self):
391+
# Create a random state space system for testing
392+
sys=rss(4,3,2)
393+
394+
# Covariance matrices
395+
Q=np.eye(sys.ninputs)
396+
R=np.eye(sys.noutputs)
397+
N=np.zeros((sys.ninputs,sys.noutputs))
398+
399+
# Standard calling format
400+
Lref,Pref,Eref=lqe(sys.A,sys.B,sys.C,Q,R)
401+
402+
# Call with system instead of matricees
403+
L,P,E=lqe(sys,Q,R)
404+
np.testing.assert_array_almost_equal(Lref,L)
405+
np.testing.assert_array_almost_equal(Pref,P)
406+
np.testing.assert_array_almost_equal(Eref,E)
407+
408+
# Compare state space and transfer function (SISO only)
409+
sys_siso=rss(4,1,1)
410+
L_ss,P_ss,E_ss=lqe(sys_siso,np.eye(1),np.eye(1))
411+
L_tf,P_tf,E_tf=lqe(tf(sys_siso),np.eye(1),np.eye(1))
412+
np.testing.assert_array_almost_equal(E_ss,E_tf)
413+
414+
# Make sure we get an error if we specify N
415+
withpytest.raises(ct.ControlNotImplemented):
416+
L,P,E=lqe(sys,Q,R,N)
417+
418+
# Inconsistent system dimensions
419+
withpytest.raises(ct.ControlDimension,match="inconsistent system"):
420+
L,P,E=lqe(sys.A,sys.C,sys.B,Q,R)
421+
422+
# incorrect covariance matrix dimensions
423+
withpytest.raises(ct.ControlDimension,match="incorrect covariance"):
424+
L,P,E=lqe(sys.A,sys.B,sys.C,R,Q)
425+
355426
@slycotonly
356427
deftest_care(self,matarrayin):
357428
"""Test stabilizing and anti-stabilizing feedbacks, continuous"""

0 commit comments

Comments
 (0)
[8]ページ先頭
©2009-2025 Movatter.jp