def has_stable_solution(g, A, B, Q, R, eps):
R[0:Bdist.shape[1], 0:Bdist.shape[1]] = -g**(2)*np.eye(Bdist.shape[1], Bdist.shape[1])
#Riccati equation prerequisites:
if not analysis.is_stabilisable(A, B):
return False, None
if not analysis.is_detectable(Q, A):
return False, None
try:
X = scipy.linalg.solve_continuous_are(A, B, Q, R)
except np.linalg.linalg.LinAlgError:
return False, None
eigsX = np.linalg.eigvals(X)
if (np.min(np.real(eigsX)) < 0) or (np.sum(np.abs(np.imag(eigsX)))>eps):
#The ARE has to return a pos. semidefinite solution, but X is not
return False, None
CL = A - Binput*np.linalg.inv(D12.T*D12)*Binput.T*X + g**(-2)*Bdist*Bdist.T*X
eigs = np.linalg.eigvals(CL)
return (np.max(np.real(eigs)) < -eps), X
def has_stable_solution(g, A, B, Q, R, eps):
R[0:Bdist.shape[1], 0:Bdist.shape[1]] = -g**(2)*np.eye(Bdist.shape[1], Bdist.shape[1])
#Riccati equation prerequisites:
if not analysis.is_stabilisable(A, B):
return False, None
if not analysis.is_detectable(Q, A):
return False, None
try:
X = scipy.linalg.solve_continuous_are(A, B, Q, R)
except np.linalg.linalg.LinAlgError:
return False, None
eigsX = np.linalg.eigvals(X)
if (np.min(np.real(eigsX)) < 0) or (np.sum(np.abs(np.imag(eigsX)))>eps):
#The ARE has to return a pos. semidefinite solution, but X is not
return False, None
CL = A - Binput*np.linalg.inv(D12.T*D12)*Binput.T*X + g**(-2)*Bdist*Bdist.T*X
eigs = np.linalg.eigvals(CL)
return (np.max(np.real(eigs)) < -eps), X
def controller_Hinf_state_feedback(A, Binput, Bdist, C1, D12, stabilityBoundaryEps=1e-16, gammaRelTol=1e-3, gammaLB = 0, gammaUB = np.Inf, subOptimality = 1.0):
"""Solve for the optimal H_infinity static state feedback controller.
A, Bdist, and Binput are system matrices, describing the systems dynamics:
dx/dt = A*x + Binput*u + Bdist*v
where x is the system state, u is the input, and v is the disturbance
The goal is to minimize the output Z, in the H_inf sense, defined as
z = C1*x + D12*u
The optimal output is given by a static feedback gain:
u = - K*x
The optimizing gain is found by a bisection search to within a relative
tolerance gammaRelTol, which may be supplied by the user. The search may
be initialised with lower and upper bounds gammaLB and gammaUB.
The user may also specify a desired suboptimality, so that the returned
controller does not achieve the minimum Hinf norm. This may be desirable
because of numerical issues near the optimal solution.
Parameters
----------
A : (n, n) Matrix
Input
Bdist : (n, m) Matrix
Input
Binput : (n, p) Matrix
Input
C1 : (n, q) Matrix
Input
D12: (q, p) Matrix
Input
stabilityBoundaryEps: float
Input (optional)
gammaRelTol: float
Input (optional)
gammaLB: float
Input (optional)
gammaUB: float
Input (optional)
Returns
-------
K : (m, n) Matrix
Hinf optimal controller gain
X : (n, n) Matrix
Solution to the Ricatti equation
J : Minimum cost value (gamma)
"""
assert analysis.is_stabilisable(A, Binput), '(A, Binput) must be stabilisable'
assert np.linalg.det(D12.T*D12), 'D12.T*D12 must be invertible'
assert np.max(np.abs(D12.T*C1))==0, 'D12.T*C1 must be zero'
tmp = analysis.unobservable_modes(C1, A, returnEigenValues=True)[1]
if tmp:
assert np.max(np.abs(np.real(tmp)))>0, 'The pair (C1,A) must have no unobservable modes on imag. axis'
#First, solve the ARE:
# A.T*X+X*A - X*Binput*inv(D12.T*D12)*Binput.T*X + gamma**(-2)*X*Bdist*Bdist.T*X + C1.T*C1 = 0
#Let:
# R = [[-gamma**(-2)*eye, 0],[0, D12.T*D12]]
# B = [Bdist, Binput]
# Q = C1.T*C1
#then we have to solve
# A.T*X+X*A - X*B*inv(R)*B.T*X + Q = 0
B = np.matrix(np.zeros([Bdist.shape[0],(Bdist.shape[1]+Binput.shape[1])]))
B[:,:Bdist.shape[1]] = Bdist
B[:,Bdist.shape[1]:] = Binput
R = np.matrix(np.zeros([B.shape[1], B.shape[1]]))
#we fill the upper left of R later.
R[Bdist.shape[1]:,Bdist.shape[1]:] = D12.T*D12
Q = C1.T*C1
#Define a helper function:
def has_stable_solution(g, A, B, Q, R, eps):
R[0:Bdist.shape[1], 0:Bdist.shape[1]] = -g**(2)*np.eye(Bdist.shape[1], Bdist.shape[1])
#Riccati equation prerequisites:
if not analysis.is_stabilisable(A, B):
return False, None
if not analysis.is_detectable(Q, A):
return False, None
try:
X = scipy.linalg.solve_continuous_are(A, B, Q, R)
except np.linalg.linalg.LinAlgError:
return False, None
eigsX = np.linalg.eigvals(X)
if (np.min(np.real(eigsX)) < 0) or (np.sum(np.abs(np.imag(eigsX)))>eps):
#The ARE has to return a pos. semidefinite solution, but X is not
return False, None
CL = A - Binput*np.linalg.inv(D12.T*D12)*Binput.T*X + g**(-2)*Bdist*Bdist.T*X
eigs = np.linalg.eigvals(CL)
return (np.max(np.real(eigs)) < -eps), X
X = None
if np.isinf(gammaUB):
#automatically choose an UB
gammaUB = np.max([1, gammaLB])
#Find an upper bound:
counter = 1
while True:
stab, X2 = has_stable_solution(gammaUB, A, B, Q, R, stabilityBoundaryEps)
if stab:
X = X2.copy()
break
gammaUB *= 2
counter += 1
assert counter < 1024, 'Exceeded max number of iterations searching for upper gamma bound!'
#Find the minimising gain
while (gammaUB-gammaLB)>gammaRelTol*gammaUB:
g = 0.5*(gammaUB+gammaLB)
stab, X2 = has_stable_solution(g, A, B, Q, R, stabilityBoundaryEps)
if stab:
gammaUB = g
X = X2
else:
gammaLB = g
assert X is not None, 'No solution found! Check supplied upper bound'
g = gammaUB
if subOptimality > 1.0:
#compute a sub optimal solution
g *= subOptimality
stab, X = has_stable_solution(g, A, B, Q, R, stabilityBoundaryEps)
assert stab, 'Sub-optimal solution not found!'
K = np.linalg.inv(D12.T*D12)*Binput.T*X
J = g
return K, X, J
def controller_Hinf_state_feedback(A, Binput, Bdist, C1, D12, stabilityBoundaryEps=1e-16, gammaRelTol=1e-3, gammaLB = 0, gammaUB = np.Inf, subOptimality = 1.0):
"""Solve for the optimal H_infinity static state feedback controller.
A, Bdist, and Binput are system matrices, describing the systems dynamics:
dx/dt = A*x + Binput*u + Bdist*v
where x is the system state, u is the input, and v is the disturbance
The goal is to minimize the output Z, in the H_inf sense, defined as
z = C1*x + D12*u
The optimal output is given by a static feedback gain:
u = - K*x
The optimizing gain is found by a bisection search to within a relative
tolerance gammaRelTol, which may be supplied by the user. The search may
be initialised with lower and upper bounds gammaLB and gammaUB.
The user may also specify a desired suboptimality, so that the returned
controller does not achieve the minimum Hinf norm. This may be desirable
because of numerical issues near the optimal solution.
Parameters
----------
A : (n, n) Matrix
Input
Bdist : (n, m) Matrix
Input
Binput : (n, p) Matrix
Input
C1 : (n, q) Matrix
Input
D12: (q, p) Matrix
Input
stabilityBoundaryEps: float
Input (optional)
gammaRelTol: float
Input (optional)
gammaLB: float
Input (optional)
gammaUB: float
Input (optional)
Returns
-------
K : (m, n) Matrix
Hinf optimal controller gain
X : (n, n) Matrix
Solution to the Ricatti equation
J : Minimum cost value (gamma)
"""
assert analysis.is_stabilisable(A, Binput), '(A, Binput) must be stabilisable'
assert np.linalg.det(D12.T*D12), 'D12.T*D12 must be invertible'
assert np.max(np.abs(D12.T*C1))==0, 'D12.T*C1 must be zero'
tmp = analysis.unobservable_modes(C1, A, returnEigenValues=True)[1]
if tmp:
assert np.max(np.abs(np.real(tmp)))>0, 'The pair (C1,A) must have no unobservable modes on imag. axis'
#First, solve the ARE:
# A.T*X+X*A - X*Binput*inv(D12.T*D12)*Binput.T*X + gamma**(-2)*X*Bdist*Bdist.T*X + C1.T*C1 = 0
#Let:
# R = [[-gamma**(-2)*eye, 0],[0, D12.T*D12]]
# B = [Bdist, Binput]
# Q = C1.T*C1
#then we have to solve
# A.T*X+X*A - X*B*inv(R)*B.T*X + Q = 0
B = np.matrix(np.zeros([Bdist.shape[0],(Bdist.shape[1]+Binput.shape[1])]))
B[:,:Bdist.shape[1]] = Bdist
B[:,Bdist.shape[1]:] = Binput
R = np.matrix(np.zeros([B.shape[1], B.shape[1]]))
#we fill the upper left of R later.
R[Bdist.shape[1]:,Bdist.shape[1]:] = D12.T*D12
Q = C1.T*C1
#Define a helper function:
def has_stable_solution(g, A, B, Q, R, eps):
R[0:Bdist.shape[1], 0:Bdist.shape[1]] = -g**(2)*np.eye(Bdist.shape[1], Bdist.shape[1])
#Riccati equation prerequisites:
if not analysis.is_stabilisable(A, B):
return False, None
if not analysis.is_detectable(Q, A):
return False, None
try:
X = scipy.linalg.solve_continuous_are(A, B, Q, R)
except np.linalg.linalg.LinAlgError:
return False, None
eigsX = np.linalg.eigvals(X)
if (np.min(np.real(eigsX)) < 0) or (np.sum(np.abs(np.imag(eigsX)))>eps):
#The ARE has to return a pos. semidefinite solution, but X is not
return False, None
CL = A - Binput*np.linalg.inv(D12.T*D12)*Binput.T*X + g**(-2)*Bdist*Bdist.T*X
eigs = np.linalg.eigvals(CL)
return (np.max(np.real(eigs)) < -eps), X
X = None
if np.isinf(gammaUB):
#automatically choose an UB
gammaUB = np.max([1, gammaLB])
#Find an upper bound:
counter = 1
while True:
stab, X2 = has_stable_solution(gammaUB, A, B, Q, R, stabilityBoundaryEps)
if stab:
X = X2.copy()
break
gammaUB *= 2
counter += 1
assert counter < 1024, 'Exceeded max number of iterations searching for upper gamma bound!'
#Find the minimising gain
while (gammaUB-gammaLB)>gammaRelTol*gammaUB:
g = 0.5*(gammaUB+gammaLB)
stab, X2 = has_stable_solution(g, A, B, Q, R, stabilityBoundaryEps)
if stab:
gammaUB = g
X = X2
else:
gammaLB = g
assert X is not None, 'No solution found! Check supplied upper bound'
g = gammaUB
if subOptimality > 1.0:
#compute a sub optimal solution
g *= subOptimality
stab, X = has_stable_solution(g, A, B, Q, R, stabilityBoundaryEps)
assert stab, 'Sub-optimal solution not found!'
K = np.linalg.inv(D12.T*D12)*Binput.T*X
J = g
return K, X, J
