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