def dare(A, B, Q, R, S=None, E=None, stabilizing=True): """ (X,L,G) = dare(A,B,Q,R) solves the discrete-time algebraic Riccati equation :math:`A^T X A - X - A^T X B (B^T X B + R)^{-1} B^T X A + Q = 0` where A and Q are square matrices of the same dimension. Further, Q is a symmetric matrix. The function returns the solution X, the gain matrix G = (B^T X B + R)^-1 B^T X A and the closed loop eigenvalues L, i.e., the eigenvalues of A - B G. (X,L,G) = dare(A,B,Q,R,S,E) solves the generalized discrete-time algebraic Riccati equation :math:`A^T X A - E^T X E - (A^T X B + S) (B^T X B + R)^{-1} (B^T X A + S^T) + Q = 0` where A, Q and E are square matrices of the same dimension. Further, Q and R are symmetric matrices. The function returns the solution X, the gain matrix :math:`G = (B^T X B + R)^{-1} (B^T X A + S^T)` and the closed loop eigenvalues L, i.e., the eigenvalues of A - B G , E. """ if S is not None or E is not None or not stabilizing: return dare_old(A, B, Q, R, S, E, stabilizing) else: Rmat = asmatrix(R) Qmat = asmatrix(Q) X = solve_discrete_are(A, B, Qmat, Rmat) G = solve(B.T.dot(X).dot(B) + Rmat, B.T.dot(X).dot(A)) L = eigvals(A - B.dot(G)) return X, L, G
def check_case(self, a, b, q, r): """Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true""" x = solve_discrete_are(a, b, q, r) assert_array_almost_equal( a.getH() * x * a - (a.getH() * x * b) * inv(r + b.getH() * x * b) * (b.getH() * x * a) + q - x, 0.0 )
def _test_factory(case, dec): """Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true""" a, b, q, r, knownfailure = case if knownfailure: pytest.xfail(reason=knownfailure) x = solve_discrete_are(a, b, q, r) res = a.conj().T.dot(x.dot(a)) - x + q res -= a.conj().T.dot(x.dot(b)).dot( solve(r+b.conj().T.dot(x.dot(b)), b.conj().T).dot(x.dot(a)) ) assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def S_riccatti(self): """ Solve for cost-to-go matrix using (discrete-time) Riccatti equation. """ A = self.A B = self.B Q = self.Q R = self.R S = solve_discrete_are(A, B, Q, R) S = torch.tensor(S).float() return S
def dkalman(A, C, W, V): """Solve the infinite-horizon discrete-time Kalman observer. x[k+1] = A x[k] + B u[k] + w[y] y[k] = C x[k] + v[t] """ #first, try to solve the ricatti equation S = la.solve_discrete_are(A.T, C.T, W, V) #compute the Kalman gain L = la.solve(C.dot(S).dot(C.T) + V, C.dot(S).dot(A.T)).T eigVals, eigVecs = la.eig(A - L.dot(C)) return L, S, eigVals
def dlqr(A, B, Q, R): """Solve the discrete time lqr controller. x[k+1] = A x[k] + B u[k] cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] """ #ref Bertsekas, p.151 #first, try to solve the ricatti equation X = la.solve_discrete_are(A, B, Q, R) #compute the LQR gain K = la.pinv( np.dot((np.dot(np.dot(B.T, X), B) + R), np.dot(np.dot(B.T, X), A)).T) eigVals, eigVecs = la.eig(A - np.dot(B, K)) return K, eigVals
def _dlqr(A, B, Q, R): """Solve the discrete time lqr controller. x[k+1] = A x[k] + B u[k] cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] """ #first, try to solve the ricatti equation X = la.solve_discrete_are(A, B, Q, R) #compute the LQR gain K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A) eigVals, eigVecs = la.eig(A - B @ K) return K, X, eigVals
def inf_cost(A, B, C, Q, Ru, Rv, K, L): d, p = B.shape a, b = C.shape K = K.reshape((p, d)) L = L.reshape((b, a)) R = np.linalg.diag((Ru, Rv)) B_t = np.hstack((B, C)) cl_map = A + np.matmul(B, K) + np.matmul(C, L) if np.amax(np.abs(LA.eigvals(cl_map))) < (1.0 - 1.0e-6): cost = np.trace(LA.solve_discrete_are(A, B_t, Q, R)) else: #cost = float("inf") cost = -20 return cost
def test_solve_generalized_discrete_are(): mat20170120 = np.load( os.path.join(os.path.abspath(os.path.dirname(__file__)), 'data', 'gendare_20170120_data.npz')) cases = [ # Two random examples differ by s term # in the absence of any literature for demanding examples. (np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01], [4.617139e-02, 6.948286e-01, 3.444608e-02], [9.713178e-02, 3.170995e-01, 4.387444e-01]]), np.array([[3.815585e-01, 1.868726e-01], [7.655168e-01, 4.897644e-01], [7.951999e-01, 4.455862e-01]]), np.eye(3), np.eye(2), np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01], [7.093648e-01, 6.797027e-01, 1.189977e-01], [7.546867e-01, 6.550980e-01, 4.983641e-01]]), np.zeros((3, 2)), None), (np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01], [4.617139e-02, 6.948286e-01, 3.444608e-02], [9.713178e-02, 3.170995e-01, 4.387444e-01]]), np.array([[3.815585e-01, 1.868726e-01], [7.655168e-01, 4.897644e-01], [7.951999e-01, 4.455862e-01]]), np.eye(3), np.eye(2), np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01], [7.093648e-01, 6.797027e-01, 1.189977e-01], [7.546867e-01, 6.550980e-01, 4.983641e-01]]), np.ones( (3, 2)), None), # user-reported (under PR-6616) 20-Jan-2017 # tests against the case where E is None but S is provided (mat20170120['A'], mat20170120['B'], mat20170120['Q'], mat20170120['R'], None, mat20170120['S'], None), ] min_decimal = (11, 11, 16) def _test_factory(case, dec): """Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true""" a, b, q, r, e, s, knownfailure = case if knownfailure: raise KnownFailureTest(knownfailure) x = solve_discrete_are(a, b, q, r, e, s) if e is None: e = np.eye(a.shape[0]) if s is None: s = np.zeros_like(b) res = a.conj().T.dot(x.dot(a)) - e.conj().T.dot(x.dot(e)) + q res -= (a.conj().T.dot(x.dot(b)) + s).dot( solve(r + b.conj().T.dot(x.dot(b)), (b.conj().T.dot(x.dot(a)) + s.conj().T))) assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def control(self, arm, x_des=None): """Generates a control signal to move the arm to the specified target. arm Arm: the arm model being controlled des list: the desired system position x_des np.array: desired task-space force, system goes to self.target if None """ if self.u is None: self.u = np.zeros(arm.DOF) self.Q = np.zeros((arm.DOF * 2, arm.DOF * 2)) self.Q[:arm.DOF, :arm.DOF] = np.eye(arm.DOF) * 1000.0 self.R = np.eye(arm.DOF) * 0.001 # calculate desired end-effector acceleration if x_des is None: self.x = arm.x x_des = self.x - self.target self.arm, state = self.copy_arm(arm) A, B = self.calc_derivs(state, self.u) if self.solve_continuous is True: X = sp_linalg.solve_continuous_are(A, B, self.Q, self.R) K = np.dot(np.linalg.pinv(self.R), np.dot(B.T, X)) else: X = sp_linalg.solve_discrete_are(A, B, self.Q, self.R) K = np.dot(np.linalg.pinv(self.R + np.dot(B.T, np.dot(X, B))), np.dot(B.T, np.dot(X, A))) # transform the command from end-effector space to joint space J = self.arm.gen_jacEE() u = np.hstack([np.dot(J.T, x_des), arm.dq]) self.u = -np.dot(K, u) if self.write_to_file is True: # feed recorders their signals self.u_recorder.record(0.0, self.u) self.xy_recorder.record(0.0, self.x) self.dist_recorder.record(0.0, self.target - self.x) # add in any additional signals for addition in self.additions: self.u += addition.generate(self.u, arm) return self.u
def lqr_gain(A,B,Q,R): ''' Arguments: State transition matrices (A,B) LQR Costs (Q,R) Outputs: K: optimal infinite-horizon LQR gain matrix given ''' # solve DARE: M=LA.solve_discrete_are(A,B,Q,R) # K=(B'MB + R)^(-1)*(B'MA) return np.dot(LA.inv(np.dot(np.dot(B.T,M),B)+R),(np.dot(np.dot(B.T,M),A)))
def dlqr(A, B, Q, R): """Solve the discrete time lqr controller. x[k+1] = A x[k] + B u[k] cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] """ #ref Bertsekas, p.151 #first, try to solve the ricatti equation X = la.solve_discrete_are(A, B, Q, R) #compute the LQR gain K = la.solve(B.T.dot(X).dot(B) + R, B.T.dot(X).dot(A)) eigVals, eigVecs = la.eig(A - B.dot(K)) return K, X, eigVals
def _test_factory(case, dec): """Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true""" a, b, q, r, e, s, knownfailure = case if knownfailure: raise KnownFailureTest(knownfailure) x = solve_discrete_are(a, b, q, r, e, s) res = a.conj().T.dot(x.dot(a)) - e.conj().T.dot(x.dot(e)) + q res -= (a.conj().T.dot(x.dot(b)) + s).dot( solve(r+b.conj().T.dot(x.dot(b)), (b.conj().T.dot(x.dot(a)) + s.conj().T) ) ) assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def lqr_gain(self, given_q, given_r): ''' lqr gain for the system :param given_q: :param given_r: :return: the kernel of the Lyapunov function P and the gain K ''' try: P = LA.solve_discrete_are(self.A, self.B, given_q, given_r) K = -LA.inv(self.B.T @ P @ self.B + given_r) @ self.B.T @ P @ self.A except: P = np.zeros((self.n, self.n)) K = np.zeros((self.m, self.n)) return P, K
def __init__(self, A, B, Q=None, R=None): # https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR A = np.array(A) B = np.array(B) # quadratic error matrices if Q is None: Q = np.eye(A.shape[0]) if R is None: R = np.zeros((B.shape[1], B.shape[1])) P = solve_discrete_are(A, B, Q, R) self.policy = -np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A)
def dlqr(A: np.array, B: np.array, Q: np.array, R: np.array, with_eigs=False) \ -> np.array: P = lin.solve_discrete_are(A, B, Q, R) if np.size(R) == 1: K = (np.transpose(B).dot(P)) / R else: K = nla.inv(R).dot((np.transpose(B).dot(P))) eig_vals, eig_vecs = nla.eig(A - B.dot(K)) if with_eigs: return K, P, eig_vals, eig_vecs else: return K, P
def dlqr(A, B, Q, R): ''' Solves for the optimal infinite-horizon, discrete-time LQR controller given linear system (A,B) and cost function parameterized by (Q,R) ''' S = sl.solve_discrete_are(A, B, Q, R) F = np.matmul(sl.inv(np.matmul(np.matmul(B.T, S), B) + R), (np.matmul(np.matmul(B.T, S), A))) eigVals, eigVecs = sl.eig(A - np.matmul(B, F)) return F, S, eigVals
def control(self, arm, x_des=None): """Generates a control signal to move the arm to the specified target. arm Arm: the arm model being controlled des list: the desired system position x_des np.array: desired task-space force, system goes to self.target if None """ if self.u is None: self.u = np.zeros(arm.DOF) self.Q = np.zeros((arm.DOF*2, arm.DOF*2)) self.Q[:arm.DOF, :arm.DOF] = np.eye(arm.DOF) * 1000.0 self.R = np.eye(arm.DOF) * 0.001 # calculate desired end-effector acceleration if x_des is None: self.x = arm.x x_des = self.x - self.target self.arm, state = self.copy_arm(arm) A, B = self.calc_derivs(state, self.u) if self.solve_continuous is True: X = sp_linalg.solve_continuous_are(A, B, self.Q, self.R) K = np.dot(np.linalg.pinv(self.R), np.dot(B.T, X)) else: X = sp_linalg.solve_discrete_are(A, B, self.Q, self.R) K = np.dot(np.linalg.pinv(self.R + np.dot(B.T, np.dot(X, B))), np.dot(B.T, np.dot(X, A))) # transform the command from end-effector space to joint space J = self.arm.gen_jacEE() u = np.hstack([np.dot(J.T, x_des), arm.dq]) self.u = -np.dot(K, u) if self.write_to_file is True: # feed recorders their signals self.u_recorder.record(0.0, self.u) self.xy_recorder.record(0.0, self.x) self.dist_recorder.record(0.0, self.target - self.x) # add in any additional signals for addition in self.additions: self.u += addition.generate(self.u, arm) return self.u
def compute_gains(self, position_weight=1.0, pitch_weight=1.0, control_weight=1.0, dt=0.01): Q = np.diag([position_weight, 0, pitch_weight, 0.0]) R = np.diag([control_weight]) A_de, B_de, C_de, D_de, dt = signal.cont2discrete((self.A, self.B, self.C, self.D), dt) # solve Discrete Ricatti Equation P = linalg.solve_discrete_are(A_de, B_de, Q, R) # compute optimum controller gains Bp = np.linalg.multi_dot([B_de.T, P, B_de]) Bp = np.linalg.inv((R+Bp)) gains = np.linalg.multi_dot([Bp, B_de.T, P, A_de]) return gains
def test_solve_generalized_discrete_are(): cases = [ # Two random examples differ by s term # in the absence of any literature for demanding examples. (np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01], [4.617139e-02, 6.948286e-01, 3.444608e-02], [9.713178e-02, 3.170995e-01, 4.387444e-01]]), np.array([[3.815585e-01, 1.868726e-01], [7.655168e-01, 4.897644e-01], [7.951999e-01, 4.455862e-01]]), np.eye(3), np.eye(2), np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01], [7.093648e-01, 6.797027e-01, 1.189977e-01], [7.546867e-01, 6.550980e-01, 4.983641e-01]]), np.zeros((3, 2)), None), (np.array([[2.769230e-01, 8.234578e-01, 9.502220e-01], [4.617139e-02, 6.948286e-01, 3.444608e-02], [9.713178e-02, 3.170995e-01, 4.387444e-01]]), np.array([[3.815585e-01, 1.868726e-01], [7.655168e-01, 4.897644e-01], [7.951999e-01, 4.455862e-01]]), np.eye(3), np.eye(2), np.array([[6.463130e-01, 2.760251e-01, 1.626117e-01], [7.093648e-01, 6.797027e-01, 1.189977e-01], [7.546867e-01, 6.550980e-01, 4.983641e-01]]), np.ones((3, 2)), None) ] min_decimal = (11, 11) def _test_factory(case, dec): """Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true""" a, b, q, r, e, s, knownfailure = case if knownfailure: raise KnownFailureTest(knownfailure) x = solve_discrete_are(a, b, q, r, e, s) res = a.conj().T.dot(x.dot(a)) - e.conj().T.dot(x.dot(e)) + q res -= (a.conj().T.dot(x.dot(b)) + s).dot( solve(r + b.conj().T.dot(x.dot(b)), (b.conj().T.dot(x.dot(a)) + s.conj().T) ) ) assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def LQR_control(): # Cost matrices for LQR Q = np.diag(np.array([1, 1, 1, 1])) # state_dimension = 4 R = np.eye(1) # control_dimension = 1 A, B = computeAB(np.array([0.0, 0.0, 0.0, 0.0]), np.array([0.0])) # Use if discrete forward dynamics is used X = sp_linalg.solve_discrete_are(A, B, Q, R) K = np.dot(np.linalg.pinv(R + np.dot(B.T, np.dot(X, B))), np.dot(B.T, np.dot(X, A))) # Use for continuous version of LQR # X = sp_linalg.solve_continuous_are(A, B, Q, R) # K = np.dot(np.linalg.pinv(R), np.dot(B.T, X)) return np.squeeze(K, 0)
def dlqr(A,B,Q,R): """Solve the discrete time lqr controller. x[k+1] = A x[k] + B u[k] cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] """ #ref Bertsekas, p.151 #first, try to solve the ricatti equation X = np.matrix(la.solve_discrete_are(A, B, Q, R)) #compute the LQR gain K = np.matrix(la.inv(B.T*X*B+R)*(B.T*X*A)) eigVals, eigVecs = la.eig(A-B*K) return K, X, eigVals
def dlqr(a, b, q, r): """ Get the feedback controls from linearized system at the current time step for a discrete time system Ax+Bu find the infinite horizon optimal feedback controller to steer the system to the origin with u = -K*x """ x = np.matrix(sLa.solve_discrete_are(a, b, q, r)) k = np.matrix(sLa.inv(b.T * x * b + r) * (b.T * x * a)) eigVals, eigVecs = sLa.eig(a - b * k) return np.asarray(k), np.asarray(x), eigVals
def __init__(self, Y, A0, A1, U0, U1, Q, Phi, initV='unconditional', X0 = None, V0 = None, statevar_names = None): self.Y, self.A0, self.A1, self.U0, self.U1, self.Q, self.Phi, self.initV, self.X0, self.V0, self.statevar_names = Y, A0, A1, U0, U1, \ Q, Phi, initV, X0, V0, statevar_names n = self.U0.size if self.X0 is None: # initialize X0 at unconditional mean self.X0 = np.mat(np.linalg.inv(np.identity(n) - self.U1)*self.U0) if self.V0 is None: if initV == 'steady_state': # solve discrete Ricatti equation self.V0 = np.mat(solve_discrete_are(np.array(self.U1.T), np.array(self.A1.T), np.array(self.Q), np.array(self.Phi.T))) elif initV == 'unconditional': # solve discrete Lyapunov equation self.V0 = np.mat(solve_discrete_lyapunov(np.array(self.U1.T), np.array(self.Q))) elif initV == 'identity': self.V0 = np.mat(np.identity(n)) if statevar_names is None: self.statevar_names = np.arange(len(self.X0)) Kalman.kalmanCount += 1
def compute_steady_state_P(self): ''' Computes the process to get P (covariance matrix on state) in steady state as the solution of a discrete Ricatti equation. Returns ------- ss_P: matrix Covariance matrix on state when the system is on steady state. Notes ----- This function should be used if the system is equiped with some kind of feedback linearization. ''' ss_P = solve_discrete_are(self.kf.F.T, self.kf.H.T, self.kf.Q, self.kf.R) return ss_P
def GetLQRController(A, B, C, ro): """Solve the discrete time lqr controller. x[k+1] = A*x[k] + B*u[k] cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] """ #ref Bertsekas, p.151 Q = numpy.dot(numpy.transpose(C), C) R = ro * numpy.eye(len(B[0])) #first, try to solve the ricatti equation X = linalg.solve_discrete_are(A, B, Q, R) #compute the LQR gain G = linalg.inv(numpy.transpose(B) @ X @ B + R) @ (numpy.transpose(B) @ X @ A) return G
def calc_NN_LQR(self, Kw, Cw, Bw, Q=None, R=None): N = len(Kw) if Q == None: Q = np.zeros((N + 1, N + 1)) Q[-1, -1] = 100 if R == None: R = np.array([[1]]) #Linearised model A = np.concatenate([Kw, -Cw * self.dt * self.xmax[1]], axis=0) A = np.concatenate([A, np.zeros((N + 1, 1))], axis=1) A[-1, -1] = 1 B = np.concatenate([Bw, np.zeros((1, 1))], axis=0) S = la.solve_discrete_are(A, B, Q, R) self.Knn = np.matmul(la.inv(R + np.matmul(np.matmul(B.T, S), B)), np.matmul(np.matmul(B.T, S), A)) return self.Knn
def _test_factory(case, dec): """Checks if X = A'XA-(A'XB)(R+B'XB)^-1(B'XA)+Q) is true""" a, b, q, r, e, s, knownfailure = case if knownfailure: pytest.xfail(reason=knownfailure) x = solve_discrete_are(a, b, q, r, e, s) if e is None: e = np.eye(a.shape[0]) if s is None: s = np.zeros_like(b) res = a.conj().T.dot(x.dot(a)) - e.conj().T.dot(x.dot(e)) + q res -= (a.conj().T.dot(x.dot(b)) + s).dot( solve(r+b.conj().T.dot(x.dot(b)), (b.conj().T.dot(x.dot(a)) + s.conj().T) ) ) assert_array_almost_equal(res, np.zeros_like(res), decimal=dec)
def LQR_discrete_gain(dt): A = np.array([ [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, g, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, -g, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], ]) A = np.eye(12) + A*dt B = np.array([ [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [1/mass, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 1/Ix, 0, 0], [0, 0, 1/Iy, 0], [0, 0, 0, 1/Iz], ]) B = B*dt # Q = np.eye(12)*1.0 Q = np.diag([0.4, 0.4, 0.4, 0.1, 0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1])*4 R = np.diag([0.1,0.2,0.2,0.2])*0.001 # Q = np.diag([1, 1.0, 3.0, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) # R = np.eye(4)*0.001 P = solve_discrete_are(A, B, Q, R) K = np.linalg.inv(R + B.T @ P @ B) @ (B.T @ P @ A) return K
def control(self, arm): x_desired = self.x - self.target self.arm, x = self.copy_arm(arm) self.Q = np.zeros((arm.DOF * 2, arm.DOF * 2)) self.Q[:arm.DOF, :arm.DOF] = np.eye(arm.DOF) * 1000.0 self.R = np.eye(arm.DOF) * 0.001 A, B = self.finite_diff_method(x, self.u) P = linalg.solve_discrete_are(A, B, self.Q, self.R) K = -np.dot(np.linalg.pinv(self.R + np.dot(B.T, np.dot(P, B))), np.dot(B.T, np.dot(P, A))) J = self.arm.gen_jacEE() x = np.hstack([np.dot(J.T, x_desired)], arm.dq) self.u = np.dot(K, x) return self.u
def kalGain(f): """ L = kalGain(f) solves the Discrete-time Algebraic Riccatti Equation (DARE) for X: X = AXA' - (AXC') * (R+CXC')^-1 * (CXA') + Q Then computes and returns the steady-state Kalman gain, L: L = XC' * (CXC'+R)^-1 f : agent Q : covariance of state error R : covariance of measurement error """ Q = np.matrix( f.stateCov ) R = np.matrix( f.sensorCov1 ) X = linalg.solve_discrete_are( f.A.T, f.C.T, Q, R ) L = X * f.C.T * linalg.inv( f.C * X * f.C.T + R ) return L
def dlqr(A, B, Q, R): """Solve the discrete time lqr controller. x[k+1] = A x[k] + B u[k] cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] """ #ref Bertsekas, p.151 #first, try to solve the ricatti equation X = np.array(solve_discrete_are(A, B, Q, R)) #compute the LQR gain K = np.array(inv(B.T @ X @ B + R) * (B.T @ X @ A)) eigVals, _ = eig(A - B @ K) return K, X, eigVals
def __init__(self): self.dt = 0.1 # sampling time (should prob use dw timestamp) self.xhat = np.transpose(np.array([[4., 0., 2., 0.]])) # state est self.yt = np.transpose(np.array([[0., 0., 0., 0.]])) # residual self.u = np.transpose(np.array([[0., 0.]])) # input self.z = np.transpose(np.array([[0., 0.]])) # dw measurement # Statespace matrices self.A = np.array([[1., self.dt, 0., 0.], [0., 1., 0., 0], [0., 0., 1., self.dt], [0., 0., 0., 1.]]) self.B = np.array([[0, 0], [self.dt, 0], [0, 0], [0, self.dt]]) self.AT = np.transpose(self.A) self.C = np.array([[1., 0., 0., 0.], [0., 0., 1., 0]]) self.CT = np.transpose(self.C) # Kalman matrices self.Q = np.array([[10., 0., 1., 0.], [0., 0.1, 0., 0], [1., 0., 1., 0.], [0., 0., 0., 0.1]]) self.R = np.array([[10000., 100.], [100., 10000.]]) self.S = np.array([[0., 0.], [0., 0.]]) # Riccatti self.P = linalg.solve_discrete_are(a=np.transpose(self.A), b=np.transpose(self.C), q=self.Q, r=self.R)
def control_mpc(self, x0): # LQR-MPC parameters if not hasattr(self, "Q"): self.Q = np.eye(2) if not hasattr(self, "R"): self.R = 1 if not hasattr(self, "Pinf"): self.Pinf = solve_discrete_are(self.At, self.bt, self.Q, self.R) return control_mpc( x0, self.At, self.bt, self.ct, self.Q, self.R, self.Pinf, self.u_limits[:, 0], self.u_limits[:, 1], n_mpc=10, debug=False, )
def control_systems(request): ct_sys, ref = request.param Ac, Bc, Cc = ct_sys.data Dc = np.zeros((Cc.shape[0], 1)) Q = np.eye(Ac.shape[0]) R = np.eye(Bc.shape[1] if len(Bc.shape) > 1 else 1) Sc = linalg.solve_continuous_are( Ac, Bc.reshape(-1, 1), Q, R, ) Kc = linalg.solve(R, Bc.T @ Sc).reshape(1, -1) ct_ctr = LTISystem(Kc) evals = np.sort( np.abs(linalg.eig(Ac, left=False, right=False, check_finite=False))) dT = 1 / (2 * evals[-1]) Tsim = (8 / np.min(evals[~np.isclose(evals, 0)]) if np.sum(np.isclose(evals[np.nonzero(evals)], 0)) > 0 else 8) dt_data = signal.cont2discrete((Ac, Bc.reshape(-1, 1), Cc, Dc), dT) Ad, Bd, Cd, Dd = dt_data[:-1] Sd = linalg.solve_discrete_are( Ad, Bd.reshape(-1, 1), Q, R, ) Kd = linalg.solve(Bd.T @ Sd @ Bd + R, Bd.T @ Sd @ Ad) dt_sys = LTISystem(Ad, Bd, dt=dT) dt_sys.initial_condition = ct_sys.initial_condition dt_ctr = LTISystem(Kd, dt=dT) yield ct_sys, ct_ctr, dt_sys, dt_ctr, ref, Tsim
def defVfin(x, xs, **Tcost): """ SUMMARY: It constructs the terminal cost for the dynamic optimisation objective function SYNTAX: assignment = defVfin(x, **Tcost): ARGUMENTS: + x - State symbolic variable + Tcost - Terminal weigth specified by the user/ Matrices to calculate Riccati equation OUTPUTS: + Vfin - terminal cost for the dynamic optimisation objective function """ if Tcost == {}: vfin = 0.0 else: for key in Tcost: if key == 'A': # Linear system & QP problem A = Tcost['A'] B = Tcost['B'] Q = Tcost['Q'] R = Tcost['R'] ## Solution to Riccati Equation P = DM(scla.solve_discrete_are(np.array(A), np.array(B), \ np.array(Q), np.array(R))) vfin = 0.5 * (xQx(x, P)) break elif key == 'vfin_F': vfin_F = Tcost['vfin_F'] vfin = vfin_F(x, xs) break Vfin = Function('Vfin', [x, xs], [vfin]) return Vfin
def fbGain(f, Q=cfg.Q, R=cfg.R): """ K = fbGain(f, Q, R) solves the Discrete time Algebraic Riccatti Equation (DARE) for X: X = A'XA - (A'XB) * (R+B'XB)^-1 * (B'XA) + Q Then computes and returns the feedback gain, K: K = (B'XB + R)^-1 * (B'XA) The feedback gain can be used to find the control input: u = -K * (f.X - (leader.X - r)) which is the control input required for the followers to track a leader to within a pre-specified distance, r, f : agent Q : cost matrix R : cost matrix """ X = linalg.solve_discrete_are( f.A, f.B, Q, R ) K = linalg.inv( f.B.T * X * f.B + R ) * ( f.B.T * X * f.A ) return K
def propagate(self, vector): """ :param vector: the current deviation of the goal state :param idx: the current horizon at which the dynamics are estimated :return: the next state of the system and an update to control matrix """ deviation_from_goal = vector.T[0] - self.vehicle.goal.T[0] theta = vector[-1] b = self.vehicle.control_matrix(theta) A = self.vehicle.system_matrix() P = LA.solve_discrete_are(A, b, self.Q, self.R) K = np.linalg.inv(self.R).dot(np.dot(b.T, P)) eigVals, eigVecs = LA.eig(A - np.matmul(b, K)) # Project the solver span = np.linspace(self.t0, self.tf, self.num_pts) # Drive deviation to zero by solving xdot = (A-BK)x sol = integrate.solve_ivp(fun=self.sys_func, t_span=span, y0=deviation_from_goal, args=(A, b, K), method='RK45', t_eval=span) # Optimal Trajectory and Control optimal_trajectory = sol.y optimal_control = np.matmul(-K, optimal_trajectory) # Only take the first action and limit the entries v_clipped = np.clip(optimal_control[0, [0]], self.vehicle.v_min, self.vehicle.v_max) omega_clipped = np.clip(optimal_control[1, [0]], self.vehicle.omega_min, self.vehicle.omega_max) # Apply control to current state control = np.array([v_clipped, omega_clipped], dtype=float) du = self.dT * np.matmul(b, control) return du, control
def leaderEst(*agents): """ xh_new = leaderEst(*agents) is the leader's estimation method. If only the leader is passed, the leader will estimate its state only based on its own observation. If leader and followers are passed, the leader will estimate its state using its own observations AND the followers' observation. """ # Simple Estimation if len(agents)==1: l = agents[0] xh_new = l.A * l.xh_old + l.B * l.u_old + \ l.Ls * ( l.y11 - l.C * l.A * l.xh_old - l.C * l.B * l.u_old ) # Advanced Estimation if len(agents)>1: l = agents[0] f = agents[1][0] if cfg.leaderEstSwitch: # Obtain Leader's and Follower's Data K2 = fbGain(f) L2 = kalGain(f) Q1 = np.matrix( l.stateCov ) Q2 = np.matrix( f.stateCov ) R21 = np.matrix( f.sensorCov1 ) R22 = np.matrix( f.sensorCov2 ) X = linalg.solve_discrete_are( f.A.T, f.C.T, Q2, R21 ) # fix this R2 P = (np.matrix(np.identity(4)) - L2*f.C) * X # Build Augment Matrices cfg.Abar = np.r_[ np.c_[ l.A , np.zeros_like(l.A) ], np.c_[ f.B*K2*f.C, f.A-f.B*K2*f.C ] ] cfg.Bbar = np.r_[ np.c_[ l.B , np.zeros((4,4)) ], np.c_[ np.zeros_like(l.B), f.B*K2 ] ] cfg.Cbar = np.r_[ np.c_[ l.C , np.zeros_like(l.C) ], np.c_[ np.zeros_like(l.C), l.C ] ] cfg.Qbar = np.r_[ np.c_[ Q1 , np.zeros_like(Q1) ], np.c_[ np.zeros_like(Q1), Q2+f.B*K2*f.C*P*f.C.T*\ K2.T*f.B.T+f.B*K2*R22*K2.T*f.B.T ] ] cfg.Rbar = np.r_[ np.c_[ R21 , np.zeros_like(R21) ], np.c_[ np.zeros_like(R21), R22 ] ] # Compute the Augmented Kalman Gain Xbar = linalg.solve_discrete_are( cfg.Abar.T, cfg.Cbar.T, cfg.Qbar, cfg.Rbar ) l.Lbar = Xbar * cfg.Cbar.T * linalg.inv( cfg.Cbar*Xbar*cfg.Cbar.T + cfg.Rbar ) # Initialize Augmented Estimate l.xh_old_bar = np.matrix(np.zeros((8,1))) # Turn Off Switch cfg.leaderEstSwitch = 0 # Calculate New Estimate ubar = np.r_[ l.u_old, cfg.r[:,0], np.matrix(np.zeros((2,1))) ] l.xh_new_bar = cfg.Abar * l.xh_old_bar + \ cfg.Bbar * ubar + \ l.Lbar * ( np.r_[ l.y11, l.y12 ] - cfg.Cbar * cfg.Abar * l.xh_old_bar - cfg.Cbar * cfg.Bbar * ubar ) # Update Old Estimate l.xh_old_bar = l.xh_new_bar xh_new = l.xh_new_bar[0:4,0] return xh_new
def dlqr(self, A, B, Q, R): P = np.matrix(sp_linalg.solve_discrete_are(A, B, Q, R))#solve discrete time ricatti equation K = np.matrix(sp_linalg.inv(B.T*P*B+R)*(B.T*P*A))#compute the LQR gain eigVals, eigVecs = sp_linalg.eig(A-B*K) return K, eigVals