Skip to content

Commit 3de4956

Browse files
murrayrmmurrayrm
committed
initial creation of stochsys module + create_estimator_iosystem
1 parent b1b3ad5 commit 3de4956

5 files changed

Lines changed: 643 additions & 378 deletions

File tree

‎control/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@
6161
from .rlocus import *
6262
from .statefbk import *
6363
from .statesp import *
64+
from .stochsys import *
6465
from .timeresp import *
6566
from .xferfcn import *
6667
from .ctrlutil import *

‎control/statefbk.py‎

Lines changed: 2 additions & 266 deletions
Original file line numberDiff line numberDiff line change
@@ -70,8 +70,8 @@ def sb03md(n, C, A, U, dico, job='X',fact='N',trana='N',ldwork=None):
7070
sb03od = None
7171

7272

73-
__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr', 'lqe',
74-
'dlqr', 'dlqe', 'acker', 'create_statefbk_iosystem']
73+
__all__ = ['ctrb', 'obsv', 'gram', 'place', 'place_varga', 'lqr',
74+
'dlqr', 'acker', 'create_statefbk_iosystem']
7575

7676

7777
# Pole placement
@@ -260,270 +260,6 @@ def place_varga(A, B, p, dtime=False, alpha=None):
260260
return _ssmatrix(-F)
261261

262262

263-
# contributed by Sawyer B. Fuller <minster@uw.edu>
264-
def lqe(*args, method=None):
265-
"""lqe(A, G, C, QN, RN, [, NN])
266-
267-
Linear quadratic estimator design (Kalman filter) for continuous-time
268-
systems. Given the system
269-
270-
.. math::
271-
272-
x &= Ax + Bu + Gw \\\\
273-
y &= Cx + Du + v
274-
275-
with unbiased process noise w and measurement noise v with covariances
276-
277-
.. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN
278-
279-
The lqe() function computes the observer gain matrix L such that the
280-
stationary (non-time-varying) Kalman filter
281-
282-
.. math:: x_e = A x_e + B u + L(y - C x_e - D u)
283-
284-
produces a state estimate x_e that minimizes the expected squared error
285-
using the sensor measurements y. The noise cross-correlation `NN` is
286-
set to zero when omitted.
287-
288-
The function can be called with either 3, 4, 5, or 6 arguments:
289-
290-
* ``L, P, E = lqe(sys, QN, RN)``
291-
* ``L, P, E = lqe(sys, QN, RN, NN)``
292-
* ``L, P, E = lqe(A, G, C, QN, RN)``
293-
* ``L, P, E = lqe(A, G, C, QN, RN, NN)``
294-
295-
where `sys` is an `LTI` object, and `A`, `G`, `C`, `QN`, `RN`, and `NN`
296-
are 2D arrays or matrices of appropriate dimension.
297-
298-
Parameters
299-
----------
300-
A, G, C : 2D array_like
301-
Dynamics, process noise (disturbance), and output matrices
302-
sys : LTI (StateSpace or TransferFunction)
303-
Linear I/O system, with the process noise input taken as the system
304-
input.
305-
QN, RN : 2D array_like
306-
Process and sensor noise covariance matrices
307-
NN : 2D array, optional
308-
Cross covariance matrix. Not currently implemented.
309-
method : str, optional
310-
Set the method used for computing the result. Current methods are
311-
'slycot' and 'scipy'. If set to None (default), try 'slycot' first
312-
and then 'scipy'.
313-
314-
Returns
315-
-------
316-
L : 2D array (or matrix)
317-
Kalman estimator gain
318-
P : 2D array (or matrix)
319-
Solution to Riccati equation
320-
321-
.. math::
322-
323-
A P + P A^T - (P C^T + G N) R^{-1} (C P + N^T G^T) + G Q G^T = 0
324-
325-
E : 1D array
326-
Eigenvalues of estimator poles eig(A - L C)
327-
328-
Notes
329-
-----
330-
1. If the first argument is an LTI object, then this object will be used
331-
to define the dynamics, noise and output matrices. Furthermore, if
332-
the LTI object corresponds to a discrete time system, the ``dlqe()``
333-
function will be called.
334-
335-
2. The return type for 2D arrays depends on the default class set for
336-
state space operations. See :func:`~control.use_numpy_matrix`.
337-
338-
Examples
339-
--------
340-
>>> L, P, E = lqe(A, G, C, QN, RN)
341-
>>> L, P, E = lqe(A, G, C, Q, RN, NN)
342-
343-
See Also
344-
--------
345-
lqr, dlqe, dlqr
346-
347-
"""
348-
349-
# TODO: incorporate cross-covariance NN, something like this,
350-
# which doesn't work for some reason
351-
# if NN is None:
352-
# NN = np.zeros(QN.size(0),RN.size(1))
353-
# NG = G @ NN
354-
355-
#
356-
# Process the arguments and figure out what inputs we received
357-
#
358-
359-
# If we were passed a discrete time system as the first arg, use dlqe()
360-
if isinstance(args[0], LTI) and isdtime(args[0], strict=True):
361-
# Call dlqe
362-
return dlqe(*args, method=method)
363-
364-
# Get the system description
365-
if (len(args) < 3):
366-
raise ControlArgument("not enough input arguments")
367-
368-
# If we were passed a state space system, use that to get system matrices
369-
if isinstance(args[0], StateSpace):
370-
A = np.array(args[0].A, ndmin=2, dtype=float)
371-
G = np.array(args[0].B, ndmin=2, dtype=float)
372-
C = np.array(args[0].C, ndmin=2, dtype=float)
373-
index = 1
374-
375-
elif isinstance(args[0], LTI):
376-
# Don't allow other types of LTI systems
377-
raise ControlArgument("LTI system must be in state space form")
378-
379-
else:
380-
# Arguments should be A and B matrices
381-
A = np.array(args[0], ndmin=2, dtype=float)
382-
G = np.array(args[1], ndmin=2, dtype=float)
383-
C = np.array(args[2], ndmin=2, dtype=float)
384-
index = 3
385-
386-
# Get the weighting matrices (converting to matrices, if needed)
387-
QN = np.array(args[index], ndmin=2, dtype=float)
388-
RN = np.array(args[index+1], ndmin=2, dtype=float)
389-
390-
# Get the cross-covariance matrix, if given
391-
if (len(args) > index + 2):
392-
NN = np.array(args[index+2], ndmin=2, dtype=float)
393-
raise ControlNotImplemented("cross-covariance not implemented")
394-
395-
else:
396-
# For future use (not currently used below)
397-
NN = np.zeros((QN.shape[0], RN.shape[1]))
398-
399-
# Check dimensions of G (needed before calling care())
400-
_check_shape("QN", QN, G.shape[1], G.shape[1])
401-
402-
# Compute the result (dimension and symmetry checking done in care())
403-
P, E, LT = care(A.T, C.T, G @ QN @ G.T, RN, method=method,
404-
B_s="C", Q_s="QN", R_s="RN", S_s="NN")
405-
return _ssmatrix(LT.T), _ssmatrix(P), E
406-
407-
408-
# contributed by Sawyer B. Fuller <minster@uw.edu>
409-
def dlqe(*args, method=None):
410-
"""dlqe(A, G, C, QN, RN, [, N])
411-
412-
Linear quadratic estimator design (Kalman filter) for discrete-time
413-
systems. Given the system
414-
415-
.. math::
416-
417-
x[n+1] &= Ax[n] + Bu[n] + Gw[n] \\\\
418-
y[n] &= Cx[n] + Du[n] + v[n]
419-
420-
with unbiased process noise w and measurement noise v with covariances
421-
422-
.. math:: E{ww'} = QN, E{vv'} = RN, E{wv'} = NN
423-
424-
The dlqe() function computes the observer gain matrix L such that the
425-
stationary (non-time-varying) Kalman filter
426-
427-
.. math:: x_e[n+1] = A x_e[n] + B u[n] + L(y[n] - C x_e[n] - D u[n])
428-
429-
produces a state estimate x_e[n] that minimizes the expected squared error
430-
using the sensor measurements y. The noise cross-correlation `NN` is
431-
set to zero when omitted.
432-
433-
Parameters
434-
----------
435-
A, G : 2D array_like
436-
Dynamics and noise input matrices
437-
QN, RN : 2D array_like
438-
Process and sensor noise covariance matrices
439-
NN : 2D array, optional
440-
Cross covariance matrix (not yet supported)
441-
method : str, optional
442-
Set the method used for computing the result. Current methods are
443-
'slycot' and 'scipy'. If set to None (default), try 'slycot' first
444-
and then 'scipy'.
445-
446-
Returns
447-
-------
448-
L : 2D array (or matrix)
449-
Kalman estimator gain
450-
P : 2D array (or matrix)
451-
Solution to Riccati equation
452-
453-
.. math::
454-
455-
A P + P A^T - (P C^T + G N) R^{-1} (C P + N^T G^T) + G Q G^T = 0
456-
457-
E : 1D array
458-
Eigenvalues of estimator poles eig(A - L C)
459-
460-
Notes
461-
-----
462-
The return type for 2D arrays depends on the default class set for
463-
state space operations. See :func:`~control.use_numpy_matrix`.
464-
465-
Examples
466-
--------
467-
>>> L, P, E = dlqe(A, G, C, QN, RN)
468-
>>> L, P, E = dlqe(A, G, C, QN, RN, NN)
469-
470-
See Also
471-
--------
472-
dlqr, lqe, lqr
473-
474-
"""
475-
476-
#
477-
# Process the arguments and figure out what inputs we received
478-
#
479-
480-
# Get the system description
481-
if (len(args) < 3):
482-
raise ControlArgument("not enough input arguments")
483-
484-
# If we were passed a continus time system as the first arg, raise error
485-
if isinstance(args[0], LTI) and isctime(args[0], strict=True):
486-
raise ControlArgument("dlqr() called with a continuous time system")
487-
488-
# If we were passed a state space system, use that to get system matrices
489-
if isinstance(args[0], StateSpace):
490-
A = np.array(args[0].A, ndmin=2, dtype=float)
491-
G = np.array(args[0].B, ndmin=2, dtype=float)
492-
C = np.array(args[0].C, ndmin=2, dtype=float)
493-
index = 1
494-
495-
elif isinstance(args[0], LTI):
496-
# Don't allow other types of LTI systems
497-
raise ControlArgument("LTI system must be in state space form")
498-
499-
else:
500-
# Arguments should be A and B matrices
501-
A = np.array(args[0], ndmin=2, dtype=float)
502-
G = np.array(args[1], ndmin=2, dtype=float)
503-
C = np.array(args[2], ndmin=2, dtype=float)
504-
index = 3
505-
506-
# Get the weighting matrices (converting to matrices, if needed)
507-
QN = np.array(args[index], ndmin=2, dtype=float)
508-
RN = np.array(args[index+1], ndmin=2, dtype=float)
509-
510-
# TODO: incorporate cross-covariance NN, something like this,
511-
# which doesn't work for some reason
512-
# if NN is None:
513-
# NN = np.zeros(QN.size(0),RN.size(1))
514-
# NG = G @ NN
515-
if len(args) > index + 2:
516-
NN = np.array(args[index+2], ndmin=2, dtype=float)
517-
raise ControlNotImplemented("cross-covariance not yet implememented")
518-
519-
# Check dimensions of G (needed before calling care())
520-
_check_shape("QN", QN, G.shape[1], G.shape[1])
521-
522-
# Compute the result (dimension and symmetry checking done in dare())
523-
P, E, LT = dare(A.T, C.T, G @ QN @ G.T, RN, method=method,
524-
B_s="C", Q_s="QN", R_s="RN", S_s="NN")
525-
return _ssmatrix(LT.T), _ssmatrix(P), E
526-
527263
# Contributed by Roberto Bucher <roberto.bucher@supsi.ch>
528264
def acker(A, B, poles):
529265
"""Pole placement using Ackermann method

0 commit comments

Comments
 (0)