def dlqr(*args, **keywords):
"""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
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)``
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: 2-d array
State feedback gains
S: 2-d array
Solution to Riccati equation
E: 1-d 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])
"""
# 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) < 4):
raise ControlArgument("not enough input arguments")
elif (ctrlutil.issys(args[0])):
# We were passed a system as the first argument; extract A and B
#! TODO: really just need to check for A and B attributes
A = np.array(args[0].A, ndmin=2, dtype=float)
B = np.array(args[0].B, ndmin=2, dtype=float)
index = 1
else:
# 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, 'D')
# Now compute the return value
K = np.dot(np.linalg.inv(R), (np.dot(B.T, X) + N.T))
S = X
E = w[0:nstates]
return K, S, E
def bb_dlqr(*args, **keywords):
"""Linear quadratic regulator design for discrete systems
Usage
=====
[K, S, E] = dlqr(A, B, Q, R, [N])
[K, S, E] = dlqr(sys, Q, R, [N])
The dlqr() function computes the optimal state feedback controller
that minimizes the quadratic cost
J = \sum_0^\infty x' Q x + u' R u + 2 x' N u
Inputs
------
A, B: 2-d arrays with dynamics and input matrices
sys: linear I/O system
Q, R: 2-d array with state and input weight matrices
N: optional 2-d array with cross weight matrix
Outputs
-------
K: 2-d array with state feedback gains
S: 2-d array with solution to Riccati equation
E: 1-d array with eigenvalues of the closed loop system
"""
#
# Process the arguments and figure out what inputs we received
#
# Get the system description
if (len(args) < 3):
raise ControlArgument("not enough input arguments")
elif (ctrlutil.issys(args[0])):
# We were passed a system as the first argument; extract A and B
A = array(args[0].A, ndmin=2, dtype=float)
B = array(args[0].B, ndmin=2, dtype=float)
index = 1
if args[0].dt == 0.0:
print "dlqr works only for discrete systems!"
return
else:
# Arguments should be A and B matrices
A = array(args[0], ndmin=2, dtype=float)
B = array(args[1], ndmin=2, dtype=float)
index = 2
# Get the weighting matrices (converting to matrices, if needed)
Q = array(args[index], ndmin=2, dtype=float)
R = array(args[index + 1], ndmin=2, dtype=float)
if (len(args) > index + 2):
N = array(args[index + 2], ndmin=2, dtype=float)
Nflag = 1
else:
N = zeros((Q.shape[0], R.shape[1]))
Nflag = 0
# 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")
if Nflag == 1:
Ao = A - B * inv(R) * N.T
Qo = Q - N * inv(R) * N.T
else:
Ao = A
Qo = Q
#Solve the riccati equation
# (X,L,G) = dare(Ao,B,Qo,R)
X = bb_dare(Ao, B, Qo, R)
# Now compute the return value
Phi = mat(A)
H = mat(B)
K = inv(H.T * X * H + R) * (H.T * X * Phi + N.T)
L = eig(Phi - H * K)
return K, X, L
def bode(*args, **keywords):
"""Bode plot of the frequency response
Examples
--------
>>> sys = ss("1. -2; 3. -4", "5.; 7", "6. 8", "9.")
>>> mag, phase, omega = bode(sys)
.. todo::
Document these use cases
* >>> bode(sys, w)
* >>> bode(sys1, sys2, ..., sysN)
* >>> bode(sys1, sys2, ..., sysN, w)
* >>> bode(sys1, 'plotstyle1', ..., sysN, 'plotstyleN')
"""
# If the first argument is a list, then assume python-control calling format
if (getattr(args[0], '__iter__', False)):
return freqplot.bode(*args, **keywords)
# Otherwise, run through the arguments and collect up arguments
syslist = []; plotstyle=[]; omega=None;
i = 0;
while i < len(args):
# Check to see if this is a system of some sort
if (ctrlutil.issys(args[i])):
# Append the system to our list of systems
syslist.append(args[i])
i += 1
# See if the next object is a plotsytle (string)
if (i < len(args) and isinstance(args[i], str)):
plotstyle.append(args[i])
i += 1
# Go on to the next argument
continue
# See if this is a frequency list
elif (isinstance(args[i], (list, np.ndarray))):
omega = args[i]
i += 1
break
else:
raise ControlArgument("unrecognized argument type")
# Check to make sure that we processed all arguments
if (i < len(args)):
raise ControlArgument("not all arguments processed")
# Check to make sure we got the same number of plotstyles as systems
if (len(plotstyle) != 0 and len(syslist) != len(plotstyle)):
raise ControlArgument("number of systems and plotstyles should be equal")
# Warn about unimplemented plotstyles
#! TODO: remove this when plot styles are implemented in bode()
#! TODO: uncomment unit test code that tests this out
if (len(plotstyle) != 0):
print("Warning (matabl.bode): plot styles not implemented");
# Call the bode command
return freqplot.bode(syslist, omega, **keywords)
def bode(*args, **keywords):
"""Bode plot of the frequency response
Plots a bode gain and phase diagram
Parameters
----------
sys : Lti, or list of Lti
System for which the Bode response is plotted and give. Optionally
a list of systems can be entered, or several systems can be
specified (i.e. several parameters). The sys arguments may also be
interspersed with format strings. A frequency argument (array_like)
may also be added, some examples:
* >>> bode(sys, w) # one system, freq vector
* >>> bode(sys1, sys2, ..., sysN) # several systems
* >>> bode(sys1, sys2, ..., sysN, w)
* >>> bode(sys1, 'plotstyle1', ..., sysN, 'plotstyleN') # + plot formats
omega: freq_range
Range of frequencies in rad/s
dB : boolean
If True, plot result in dB
Hz : boolean
If True, plot frequency in Hz (omega must be provided in rad/sec)
deg : boolean
If True, return phase in degrees (else radians)
Plot : boolean
If True, plot magnitude and phase
Examples
--------
>>> sys = ss("1. -2; 3. -4", "5.; 7", "6. 8", "9.")
>>> mag, phase, omega = bode(sys)
.. todo::
Document these use cases
* >>> bode(sys, w)
* >>> bode(sys1, sys2, ..., sysN)
* >>> bode(sys1, sys2, ..., sysN, w)
* >>> bode(sys1, 'plotstyle1', ..., sysN, 'plotstyleN')
"""
# If the first argument is a list, then assume python-control calling format
if (getattr(args[0], '__iter__', False)):
return freqplot.bode(*args, **keywords)
# Otherwise, run through the arguments and collect up arguments
syslist = []; plotstyle=[]; omega=None;
i = 0;
while i < len(args):
# Check to see if this is a system of some sort
if (ctrlutil.issys(args[i])):
# Append the system to our list of systems
syslist.append(args[i])
i += 1
# See if the next object is a plotsytle (string)
if (i < len(args) and isinstance(args[i], str)):
plotstyle.append(args[i])
i += 1
# Go on to the next argument
continue
# See if this is a frequency list
elif (isinstance(args[i], (list, np.ndarray))):
omega = args[i]
i += 1
break
else:
raise ControlArgument("unrecognized argument type")
# Check to make sure that we processed all arguments
if (i < len(args)):
raise ControlArgument("not all arguments processed")
# Check to make sure we got the same number of plotstyles as systems
if (len(plotstyle) != 0 and len(syslist) != len(plotstyle)):
raise ControlArgument("number of systems and plotstyles should be equal")
# Warn about unimplemented plotstyles
#! TODO: remove this when plot styles are implemented in bode()
#! TODO: uncomment unit test code that tests this out
if (len(plotstyle) != 0):
print("Warning (matabl.bode): plot styles not implemented");
# Call the bode command
return freqplot.bode(syslist, omega, **keywords)
def bode(*args, **keywords):
"""Bode plot of the frequency response
Plots a bode gain and phase diagram
Parameters
----------
sys : Lti, or list of Lti
System for which the Bode response is plotted and give. Optionally
a list of systems can be entered, or several systems can be
specified (i.e. several parameters). The sys arguments may also be
interspersed with format strings. A frequency argument (array_like)
may also be added, some examples:
* >>> bode(sys, w) # one system, freq vector
* >>> bode(sys1, sys2, ..., sysN) # several systems
* >>> bode(sys1, sys2, ..., sysN, w)
* >>> bode(sys1, 'plotstyle1', ..., sysN, 'plotstyleN') # + plot formats
omega: freq_range
Range of frequencies in rad/s
dB : boolean
If True, plot result in dB
Hz : boolean
If True, plot frequency in Hz (omega must be provided in rad/sec)
deg : boolean
If True, return phase in degrees (else radians)
Plot : boolean
If True, plot magnitude and phase
Examples
--------
>>> sys = ss("1. -2; 3. -4", "5.; 7", "6. 8", "9.")
>>> mag, phase, omega = bode(sys)
.. todo::
Document these use cases
* >>> bode(sys, w)
* >>> bode(sys1, sys2, ..., sysN)
* >>> bode(sys1, sys2, ..., sysN, w)
* >>> bode(sys1, 'plotstyle1', ..., sysN, 'plotstyleN')
"""
# If the first argument is a list, then assume python-control calling format
if (getattr(args[0], '__iter__', False)):
return freqplot.bode(*args, **keywords)
# Otherwise, run through the arguments and collect up arguments
syslist = []; plotstyle=[]; omega=None;
i = 0;
while i < len(args):
# Check to see if this is a system of some sort
if (ctrlutil.issys(args[i])):
# Append the system to our list of systems
syslist.append(args[i])
i += 1
# See if the next object is a plotsytle (string)
if (i < len(args) and isinstance(args[i], str)):
plotstyle.append(args[i])
i += 1
# Go on to the next argument
continue
# See if this is a frequency list
elif (isinstance(args[i], (list, np.ndarray))):
omega = args[i]
i += 1
break
else:
raise ControlArgument("unrecognized argument type")
# Check to make sure that we processed all arguments
if (i < len(args)):
raise ControlArgument("not all arguments processed")
# Check to make sure we got the same number of plotstyles as systems
if (len(plotstyle) != 0 and len(syslist) != len(plotstyle)):
raise ControlArgument("number of systems and plotstyles should be equal")
# Warn about unimplemented plotstyles
#! TODO: remove this when plot styles are implemented in bode()
#! TODO: uncomment unit test code that tests this out
if (len(plotstyle) != 0):
print("Warning (matabl.bode): plot styles not implemented");
# Call the bode command
return freqplot.bode(syslist, omega, **keywords)
def lqr(*args, **keywords):
"""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
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)``
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: 2-d array
State feedback gains
S: 2-d array
Solution to Riccati equation
E: 1-d 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])
"""
# 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) < 4):
raise ControlArgument("not enough input arguments")
elif (ctrlutil.issys(args[0])):
# We were passed a system as the first argument; extract A and B
#! TODO: really just need to check for A and B attributes
A = np.array(args[0].A, ndmin=2, dtype=float);
B = np.array(args[0].B, ndmin=2, dtype=float);
index = 1;
else:
# 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
K = np.dot(np.linalg.inv(R), (np.dot(B.T, X) + N.T));
S = X;
E = w[0:nstates];
return K, S, E