LSR/env/lib/python3.6/site-packages/control/statefbk.py
2020-06-04 17:24:47 +02:00

604 lines
18 KiB
Python

# statefbk.py - tools for state feedback control
#
# Author: Richard M. Murray, Roberto Bucher
# Date: 31 May 2010
#
# This file contains routines for designing state space controllers
#
# Copyright (c) 2010 by California Institute of Technology
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# 3. Neither the name of the California Institute of Technology nor
# the names of its contributors may be used to endorse or promote
# products derived from this software without specific prior
# written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CALTECH
# OR THE CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
# USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
# OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
# SUCH DAMAGE.
#
# $Id$
# External packages and modules
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', 'lqe', 'acker']
# Pole placement
def place(A, B, p):
"""Place closed loop eigenvalues
K = place(A, B, p)
Parameters
----------
A : 2-d array
Dynamics matrix
B : 2-d array
Input matrix
p : 1-d list
Desired eigenvalue locations
Returns
-------
K : 2-d array
Gain such that A - B K has eigenvalues given in p
Algorithm
---------
This is a wrapper function for scipy.signal.place_poles, which
implements the Tits and Yang algorithm [1]. It will handle SISO,
MISO, and MIMO systems. If you want more control over the algorithm,
use scipy.signal.place_poles directly.
[1] A.L. Tits and Y. Yang, "Globally convergent algorithms for robust
pole assignment by state feedback, IEEE Transactions on Automatic
Control, Vol. 41, pp. 1432-1452, 1996.
Limitations
-----------
The algorithm will not place poles at the same location more
than rank(B) times.
Examples
--------
>>> A = [[-1, -1], [0, 1]]
>>> B = [[0], [1]]
>>> K = place(A, B, [-2, -5])
See Also
--------
place_varga, acker
"""
from scipy.signal import place_poles
# Convert the system inputs to NumPy arrays
A_mat = np.array(A)
B_mat = np.array(B)
if (A_mat.shape[0] != A_mat.shape[1]):
raise ControlDimension("A must be a square matrix")
if (A_mat.shape[0] != B_mat.shape[0]):
err_str = "The number of rows of A must equal the number of rows in B"
raise ControlDimension(err_str)
# Convert desired poles to numpy array
placed_eigs = np.array(p)
result = place_poles(A_mat, B_mat, placed_eigs, method='YT')
K = result.gain_matrix
return _ssmatrix(K)
def place_varga(A, B, p, dtime=False, alpha=None):
"""Place closed loop eigenvalues
K = place_varga(A, B, p, dtime=False, alpha=None)
Required Parameters
----------
A : 2-d array
Dynamics matrix
B : 2-d array
Input matrix
p : 1-d list
Desired eigenvalue locations
Optional Parameters
---------------
dtime: False for continuous time pole placement or True for discrete time.
The default is dtime=False.
alpha: double scalar
If DICO='C', then place_varga will leave the eigenvalues with real
real part less than alpha untouched.
If DICO='D', the place_varga will leave eigenvalues with modulus
less than alpha untouched.
By default (alpha=None), place_varga computes alpha such that all
poles will be placed.
Returns
-------
K : 2D array
Gain such that A - B K has eigenvalues given in p.
Algorithm
---------
This function is a wrapper for the slycot function sb01bd, which
implements the pole placement algorithm of Varga [1]. In contrast to
the algorithm used by place(), the Varga algorithm can place
multiple poles at the same location. The placement, however, may not
be as robust.
[1] Varga A. "A Schur method for pole assignment."
IEEE Trans. Automatic Control, Vol. AC-26, pp. 517-519, 1981.
Examples
--------
>>> A = [[-1, -1], [0, 1]]
>>> B = [[0], [1]]
>>> K = place_varga(A, B, [-2, -5])
See Also:
--------
place, acker
"""
# Make sure that SLICOT is installed
try:
from slycot import sb01bd
except ImportError:
raise ControlSlycot("can't find slycot module 'sb01bd'")
# Convert the system inputs to NumPy arrays
A_mat = np.array(A)
B_mat = np.array(B)
if (A_mat.shape[0] != A_mat.shape[1] or
A_mat.shape[0] != B_mat.shape[0]):
raise ControlDimension("matrix dimensions are incorrect")
# Compute the system eigenvalues and convert poles to numpy array
system_eigs = np.linalg.eig(A_mat)[0]
placed_eigs = np.array(p)
# Need a character parameter for SB01BD
if dtime:
DICO = 'D'
else:
DICO = 'C'
if alpha is None:
# SB01BD ignores eigenvalues with real part less than alpha
# (if DICO='C') or with modulus less than alpha
# (if DICO = 'D').
if dtime:
# For discrete time, slycot only cares about modulus, so just make
# alpha the smallest it can be.
alpha = 0.0
else:
# Choosing alpha=min_eig is insufficient and can lead to an
# error or not having all the eigenvalues placed that we wanted.
# Evidently, what python thinks are the eigs is not precisely
# the same as what slicot thinks are the eigs. So we need some
# numerical breathing room. The following is pretty heuristic,
# but does the trick
alpha = -2*abs(min(system_eigs.real))
elif dtime and alpha < 0.0:
raise ValueError("Need alpha > 0 when DICO='D'")
# Call SLICOT routine to place the eigenvalues
A_z,w,nfp,nap,nup,F,Z = \
sb01bd(B_mat.shape[0], B_mat.shape[1], len(placed_eigs), alpha,
A_mat, B_mat, placed_eigs, DICO)
# 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
Call:
K = acker(A, B, poles)
Parameters
----------
A, B : 2-d arrays
State and input matrix of the system
poles: 1-d list
Desired eigenvalue locations
Returns
-------
K: matrix
Gains such that A - B K has given eigenvalues
"""
# Convert the inputs to matrices
a = _ssmatrix(A)
b = _ssmatrix(B)
# Make sure the system is controllable
ct = ctrb(A, B)
if np.linalg.matrix_rank(ct) != a.shape[0]:
raise ValueError("System not reachable; pole placement invalid")
# Compute the desired characteristic polynomial
p = np.real(np.poly(poles))
# Place the poles using Ackermann's method
# TODO: compute pmat using Horner's method (O(n) instead of O(n^2))
n = np.size(p)
pmat = p[n-1] * np.linalg.matrix_power(a, 0)
for i in np.arange(1,n):
pmat = pmat + np.dot(p[n-i-1], np.linalg.matrix_power(a, i))
K = np.linalg.solve(ct, pmat)
K = K[-1][:] # Extract the last row
return _ssmatrix(K)
def lqr(*args, **keywords):
"""lqr(A, B, Q, R[, N])
Linear quadratic regulator design
The lqr() function computes the optimal state feedback controller
that minimizes the quadratic cost
.. math:: J = \\int_0^\\infty (x' Q x + u' R u + 2 x' N u) dt
The function can be called with either 3, 4, or 5 arguments:
* ``lqr(sys, Q, R)``
* ``lqr(sys, Q, R, N)``
* ``lqr(A, B, Q, R)``
* ``lqr(A, B, Q, R, N)``
where `sys` is an `LTI` object, and `A`, `B`, `Q`, `R`, and `N` are
2d arrays or matrices of appropriate dimension.
Parameters
----------
A, B: 2-d array
Dynamics and input matrices
sys: LTI (StateSpace or TransferFunction)
Linear I/O system
Q, R: 2-d array
State and input weight matrices
N: 2-d array, optional
Cross weight matrix
Returns
-------
K: 2D array
State feedback gains
S: 2D array
Solution to Riccati equation
E: 1D array
Eigenvalues of the closed loop system
Examples
--------
>>> 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
try:
from slycot import sb02md
from slycot import sb02mt
except ImportError:
raise ControlSlycot("can't find slycot module 'sb02md' or 'sb02nt'")
#
# Process the arguments and figure out what inputs we received
#
# Get the system description
if (len(args) < 3):
raise ControlArgument("not enough input arguments")
try:
# If this works, we were (probably) passed a system as the
# first argument; extract A and B
A = np.array(args[0].A, ndmin=2, dtype=float);
B = np.array(args[0].B, ndmin=2, dtype=float);
index = 1;
except AttributeError:
# Arguments should be A and B matrices
A = np.array(args[0], ndmin=2, dtype=float);
B = np.array(args[1], ndmin=2, dtype=float);
index = 2;
# Get the weighting matrices (converting to matrices, if needed)
Q = np.array(args[index], ndmin=2, dtype=float);
R = np.array(args[index+1], ndmin=2, dtype=float);
if (len(args) > index + 2):
N = np.array(args[index+2], ndmin=2, dtype=float);
else:
N = np.zeros((Q.shape[0], R.shape[1]));
# Check dimensions for consistency
nstates = B.shape[0];
ninputs = B.shape[1];
if (A.shape[0] != nstates or A.shape[1] != nstates):
raise ControlDimension("inconsistent system dimensions")
elif (Q.shape[0] != nstates or Q.shape[1] != nstates or
R.shape[0] != ninputs or R.shape[1] != ninputs or
N.shape[0] != nstates or N.shape[1] != ninputs):
raise ControlDimension("incorrect weighting matrix dimensions")
# Compute the G matrix required by SB02MD
A_b,B_b,Q_b,R_b,L_b,ipiv,oufact,G = \
sb02mt(nstates, ninputs, B, R, A, Q, N, jobl='N');
# Call the SLICOT function
X,rcond,w,S,U,A_inv = sb02md(nstates, A_b, G, Q_b, 'C')
# Now compute the return value
# We assume that R is positive definite and, hence, invertible
K = np.linalg.solve(R, np.dot(B.T, X) + N.T);
S = X;
E = w[0:nstates];
return _ssmatrix(K), _ssmatrix(S), E
def ctrb(A, B):
"""Controllabilty matrix
Parameters
----------
A, B: array_like or string
Dynamics and input matrix of the system
Returns
-------
C: matrix
Controllability matrix
Examples
--------
>>> C = ctrb(A, B)
"""
# Convert input parameters to matrices (if they aren't already)
amat = _ssmatrix(A)
bmat = _ssmatrix(B)
n = np.shape(amat)[0]
# Construct the controllability matrix
ctrb = np.hstack([bmat] + [np.dot(np.linalg.matrix_power(amat, i), bmat)
for i in range(1, n)])
return _ssmatrix(ctrb)
def obsv(A, C):
"""Observability matrix
Parameters
----------
A, C: array_like or string
Dynamics and output matrix of the system
Returns
-------
O: matrix
Observability matrix
Examples
--------
>>> O = obsv(A, C)
"""
# Convert input parameters to matrices (if they aren't already)
amat = _ssmatrix(A)
cmat = _ssmatrix(C)
n = np.shape(amat)[0]
# Construct the observability matrix
obsv = np.vstack([cmat] + [np.dot(cmat, np.linalg.matrix_power(amat, i))
for i in range(1, n)])
return _ssmatrix(obsv)
def gram(sys,type):
"""Gramian (controllability or observability)
Parameters
----------
sys: StateSpace
State-space system to compute Gramian for
type: String
Type of desired computation.
`type` is either 'c' (controllability) or 'o' (observability). To compute the
Cholesky factors of gramians use 'cf' (controllability) or 'of' (observability)
Returns
-------
gram: array
Gramian of system
Raises
------
ValueError
* if system is not instance of StateSpace class
* if `type` is not 'c', 'o', 'cf' or 'of'
* if system is unstable (sys.A has eigenvalues not in left half plane)
ImportError
if slycot routine sb03md cannot be found
if slycot routine sb03od cannot be found
Examples
--------
>>> Wc = gram(sys,'c')
>>> Wo = gram(sys,'o')
>>> Rc = gram(sys,'cf'), where Wc=Rc'*Rc
>>> Ro = gram(sys,'of'), where Wo=Ro'*Ro
"""
#Check for ss system object
if not isinstance(sys,statesp.StateSpace):
raise ValueError("System must be StateSpace!")
if type not in ['c', 'o', 'cf', 'of']:
raise ValueError("That type is not supported!")
#TODO: Check for continous or discrete, only continuous supported right now
# if isCont():
# dico = 'C'
# elif isDisc():
# dico = 'D'
# else:
dico = 'C'
#TODO: Check system is stable, perhaps a utility in ctrlutil.py
# or a method of the StateSpace class?
if np.any(np.linalg.eigvals(sys.A).real >= 0.0):
raise ValueError("Oops, the system is unstable!")
if type=='c' or type=='o':
#Compute Gramian by the Slycot routine sb03md
#make sure Slycot is installed
try:
from slycot import sb03md
except ImportError:
raise ControlSlycot("can't find slycot module 'sb03md'")
if type=='c':
tra = 'T'
C = -np.dot(sys.B,sys.B.transpose())
elif type=='o':
tra = 'N'
C = -np.dot(sys.C.transpose(),sys.C)
n = sys.states
U = np.zeros((n,n))
A = np.array(sys.A) # convert to NumPy array for slycot
X,scale,sep,ferr,w = sb03md(n, C, A, U, dico, job='X', fact='N', trana=tra)
gram = X
return _ssmatrix(gram)
elif type=='cf' or type=='of':
#Compute cholesky factored gramian from slycot routine sb03od
try:
from slycot import sb03od
except ImportError:
raise ControlSlycot("can't find slycot module 'sb03od'")
tra='N'
n = sys.states
Q = np.zeros((n,n))
A = np.array(sys.A) # convert to NumPy array for slycot
if type=='cf':
m = sys.B.shape[1]
B = np.zeros_like(A)
B[0:m,0:n] = sys.B.transpose()
X,scale,w = sb03od(n, m, A.transpose(), Q, B, dico, fact='N', trans=tra)
elif type=='of':
m = sys.C.shape[0]
C = np.zeros_like(A)
C[0:n,0:m] = sys.C.transpose()
X,scale,w = sb03od(n, m, A, Q, C.transpose(), dico, fact='N', trans=tra)
gram = X
return _ssmatrix(gram)