def initializeGainMatrix(self): """ Calculate the gain matrix. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model n_p = 12 # number of states m = 4 # number of integral error terms # ----------------- Your Code Here ----------------- # # Compute the discretized A_d, B_d, C_d, D_d, for the computation of LQR gain # ----------------- Your Code Ends Here ----------------- # # ----------------- Example code ----------------- # # max_pos = 15.0 # max_ang = 0.2 * self.pi # max_vel = 6.0 # max_rate = 0.015 * self.pi # max_eyI = 3. # max_states = np.array([0.1 * max_pos, 0.1 * max_pos, max_pos, # max_ang, max_ang, max_ang, # 0.5 * max_vel, 0.5 * max_vel, max_vel, # max_rate, max_rate, max_rate, # 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI]) # max_inputs = np.array([0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # Q = np.diag(1/max_states**2) # R = np.diag(1/max_inputs**2) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.K = -K
def initializeGainMatrix(self): """ Calculate the gain matrix. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model n_p = 12 # number of states m = 4 # number of integral error terms # ----------------- Your Code Here ----------------- # # Compute the discretized A_d, B_d, C_d, D_d, for the computation of LQR gain Ap = np.zeros((12, 12)) Ap[0:6, 6:] = np.eye(6) Ap[6, 4] = self.g Ap[7, 3] = -self.g Bp = np.zeros((12, 4)) Bp[8, 0] = 1 / self.m Bp[9, 1] = 1 / self.Ix Bp[10, 2] = 1 / self.Iy Bp[11, 3] = 1 / self.Iz Cp = np.zeros((4, 12)) Cp[:3, 0:3] = np.eye(3) Cp[3, 5] = 1 Aupper = np.hstack((Ap, np.zeros((12, 4)))) Alower = np.hstack((Cp, np.zeros((4, 4)))) At = np.vstack((Aupper, Alower)) Bt = np.vstack((Bp, np.zeros((4, 4)))) Bc = np.vstack((np.zeros((12, 4)), -np.eye(4))) BtBc = np.hstack((Bt, Bc)) Ct = np.hstack((Cp, np.zeros((4, 4)))) Dt = np.zeros((4, 8)) delT = 0.01 CT_sys = signal.StateSpace(At, BtBc, Ct, Dt) DT_sys = CT_sys.to_discrete(delT) A_dt = DT_sys.A B_dt = DT_sys.B A_d = A_dt B_td = B_dt[:, :4] B_d = B_td # ----------------- Your Code Ends Here ----------------- # # ----------------- Example code ----------------- # max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3. max_states = np.array([ 0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI ]) max_inputs = np.array( [0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) Q = np.diag(1 / max_states**2) R = np.diag(1 / max_inputs**2) #----------------- Example code Ends ----------------- # #----------------- Your Code Here ----------------- # #Come up with reasonable values for Q and R (state and control weights) #The example code above is a good starting point, feel free to use them or write you own. #Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.K = -K
def initializeGainMatrix(self): """ Calculate the LQR gain matrix and matrices for adaptive controller. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model n_p = 12 # number of states m = 4 # number of integral error terms # ----------------- Your Code Here ----------------- # # Compute the continuous A, B, Bc, C, D and # discretized A_d, B_d, Bc_d, C_d, D_d, for the computation of LQR gain Ap = np.zeros((12, 12)) Ap[0:6, 6:] = np.eye(6) Ap[6, 4] = self.g Ap[7, 3] = -self.g Bp = np.zeros((12, 4)) Bp[8, 0] = 1 / self.m Bp[9, 1] = 1 / self.Ix Bp[10, 2] = 1 / self.Iy Bp[11, 3] = 1 / self.Iz Cp = np.zeros((4, 12)) Cp[:3, 0:3] = np.eye(3) Cp[3, 5] = 1 Aupper = np.hstack((Ap, np.zeros((12, 4)))) Alower = np.hstack((Cp, np.zeros((4, 4)))) At = np.vstack((Aupper, Alower)) Bt = np.vstack((Bp, np.zeros((4, 4)))) Bc = np.vstack((np.zeros((12, 4)), -np.eye(4))) BtBc = np.hstack((Bt, Bc)) Ct = np.hstack((Cp, np.zeros((4, 4)))) Dt = np.zeros((4, 8)) delT = 0.01 CT_sys = signal.StateSpace(At, BtBc, Ct, Dt) DT_sys = CT_sys.to_discrete(delT) A_dt = DT_sys.A B_dt = DT_sys.B A_d = A_dt B_td = B_dt[:, :4] B_d = B_td Bc_d = B_dt[:, 4:] B = Bt A = At # ----------------- Your Code Ends Here ----------------- # # Record the matrix for later use self.B = B # continuous version of B self.A_d = A_d # discrete version of A self.B_d = B_d # discrete version of B self.Bc_d = Bc_d # discrete version of Bc # ----------------- Example code ----------------- # max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3. max_states = np.array([ 0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI ]) max_inputs = np.array( [0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) Q = np.diag(1 / max_states**2) R = np.diag(1 / max_inputs**2) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.Kbl = -K [K_CT, _, _] = lqr(A, B, Q, R) Kbl_CT = -K_CT # initialize adaptive controller gain to baseline LQR controller gain self.K_ad = self.Kbl.T # ----------------- Example code ----------------- # self.Gamma = 3e-3 * np.eye(16) Q_lyap = np.copy(Q) Q_lyap[0:3, 0:3] *= 30 Q_lyap[6:9, 6:9] *= 150 Q_lyap[14, 14] *= 2e-3 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable value for Gamma matrix and Q_lyap # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # A_m = A + self.B @ Kbl_CT self.P = solve_continuous_lyapunov(A_m.T, -Q_lyap)
def initializeGainMatrix(self): """ Calculate the gain matrix. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model delT = self.timestep * 1e-3 g = self.g m = self.m Ix = self.Ix Iy = self.Iy Iz = self.Iz n_p = 12 # number of states m = 4 # number of integral error terms A = np.zeros((16, 16)) A[0, 6] = 1 A[1, 7] = 1 A[2, 8] = 1 A[3, 9] = 1 A[4, 10] = 1 A[5, 11] = 1 A[6, 4] = g A[7, 3] = -g A[12, 0] = 1 A[12, 12] = -1 A[13, 1] = 1 A[13, 13] = -1 A[14, 2] = 1 A[14, 14] = -1 A[15, 5] = 1 A[15, 15] = -1 B = np.zeros((16, 4)) B[8, 0] = 1 / m B[9, 1] = 1 / Ix B[10, 2] = 1 / Iy B[11, 3] = 1 / Iz C = np.zeros((4, 16)) C[0, 0] = 1 C[1, 1] = 1 C[2, 2] = 1 C[3, 5] = 1 D = np.zeros((4, 4)) sys_Con = signal.StateSpace(A, B, C, D) # ----------------- Your Code Here ----------------- # # Compute the discretized A_d, B_d, C_d, D_d, for the computation of LQR gain sys_Dis = sys_Con.to_discrete(delT) A_d = sys_Dis.A B_d = sys_Dis.B C_d = sys_Dis.C D_d = sys_Dis.D # ----------------- Your Code Ends Here ----------------- # # ----------------- Example code ----------------- # # max_pos = 15.0 # max_ang = 0.2 * self.pi # max_vel = 6.0 # max_rate = 0.015 * self.pi # max_eyI = 3. # max_states = np.array([0.1 * max_pos, 0.1 * max_pos, max_pos, # max_ang, max_ang, max_ang, # 0.5 * max_vel, 0.5 * max_vel, max_vel, # max_rate, max_rate, max_rate, # 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI]) # max_inputs = np.array([0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # Q = np.diag(1/max_states**2) # R = np.diag(1/max_inputs**2) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 3.0 max_rate = 0.015 * self.pi max_eyI = 0.9 max_states = np.array([ 0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI ]) max_inputs = np.array( [0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) Q = np.diag(1 / max_states**2) R = np.diag(1 / max_inputs**2) # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.K = -K
def initializeGainMatrix(self): """ Calculate the gain matrix. """ # System Parameters: Ix = 0.000913855 Iy = 0.00236242 Iz = 0.00279965 g = 9.81 mass = 0.4 delT = 0.01 n_p = 12 # number of states m = 4 # number of integral error terms # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model # Output: Cp = np.asarray([[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, 1, 0, 0, 0, 0, 0, 0]]) C = np.hstack((Cp, np.zeros((m, m)))) A = np.vstack( (np.asarray([[0, 0, 0, 0, 0, 0, 1, 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, 0, 0, 0, 0, 1, 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, 0, 0, 0, 0, 1, 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, g, 0, 0, 0, 0, 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, 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, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]), C)) Bt = np.asarray([[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], [1 / mass, 0, 0, 0], [0, 1 / Ix, 0, 0], [0, 0, 1 / Iy, 0], [0, 0, 0, 1 / Iz], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]) Bc = np.vstack((np.zeros((n_p, m)), -np.eye(m))) B = np.hstack((Bt, Bc)) D = np.zeros((4, 4 + m)) ss_CT = signal.StateSpace(A, B, C, D) # ----------------- Your Code Here ----------------- # # Compute the discretized A_d, B_d, C_d, D_d, for the computation of LQR gain ss_DT = ss_CT.to_discrete(delT) A_d = ss_DT.A B_d = ss_DT.B Bt_d = B_d[:, :4] Bc_d = B_d[:, 4:] # Passthrough (full state): C_d = np.eye(A_d.shape[0]) D_d = np.zeros(B_d.shape) # ----------------- Your Code Ends Here ----------------- # # ----------------- Example code ----------------- # # max_pos = 15.0 # max_ang = 0.2 * self.pi # max_vel = 6.0 # max_rate = 0.015 * self.pi # max_eyI = 3. # max_states = np.array([0.1 * max_pos, 0.1 * max_pos, max_pos, # max_ang, max_ang, max_ang, # 0.5 * max_vel, 0.5 * max_vel, max_vel, # max_rate, max_rate, max_rate, # 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI]) # max_inputs = np.array([0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # Q = np.diag(1/max_states**2) # R = np.diag(1/max_inputs**2) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3. max_states = np.array([ 0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI ]) max_inputs = np.array( [0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) Q = np.diag(1 / max_states**2) R = np.diag(1 / max_inputs**2) # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, Bt_d, Q, R) self.K = -K
def initializeGainMatrix(self): """ Calculate the gain matrix. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model n_p = 12 # number of states m = 4 # number of integral error terms # mass = 4 delT = 1e-3 * self.timestep # Initialize Ap,Bp,Cp,Dp A = np.zeros((16, 16)) B = np.zeros((16, 4)) C = np.zeros((4, 16)) D = np.zeros((4, 8)) for i in range(0, 6): A[i][i + 6] = 1 A[6][4] = self.g A[7][3] = -self.g B[8][0] = 1 / self.m B[9][1] = 1 / self.Ix B[10][2] = 1 / self.Iy B[11][3] = 1 / self.Iz Bc = np.zeros((12, 4)) Bc = np.vstack((Bc, -np.eye(4))) B = np.concatenate((B, Bc), axis=-1) for i in range(0, 4): C[i][i] = 1 # for i in range(12,16): for i in range(0, 4): A[i + 12][i] = 1 # ----------------- Your Code Here ----------------- # # Compute the discretized A_d, B_d, C_d, D_d, for the computation of LQR gain CT = signal.StateSpace(A, B, C, D) DT = CT.to_discrete(delT) # DT = signal.StateSpace(A, B, C, D, dt=delT) A_d = DT.A B_d = DT.B[:, :4] # B_d = DT.B # print(B_d) C_d = DT.C D_d = DT.D[:, :4] # print(C_d) # D_d = DT.D # ----------------- Your Code Ends Here ----------------- # # ----------------- Example code ----------------- # max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3. max_states = np.array([ 0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI ]) max_inputs = np.array( [0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance Q = np.diag(1 / max_states**2) R = np.diag(1 / max_inputs**2) * 0.5 # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains # S = linalg.solve_discrete_are(A_d, B_d, Q, R) # K = linalg.inv(B.T@S@B+R)@(B.T@S@A) [K, _, _] = dlqr(A_d, B_d, Q, R) self.K = -0.8 * K
def initializeGainMatrix(self): """ Calculate the LQR gain matrix and matrices for adaptive controller. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model n_p = 12 # number of states m = 4 # number of integral error terms delT = 1e-3 * self.timestep # ----------------- Your Code Here ----------------- # # Compute the continuous A, B, Bc, C, D and # discretized A_d, B_d, Bc_d, C_d, D_d, for the computation of LQR gain A = np.zeros((16, 16)) B = np.zeros((16, 4)) C = np.zeros((4, 16)) D = np.zeros((4, 4)) for i in range(0, 6): A[i][i + 6] = 1 A[6][4] = self.g A[7][3] = -self.g B[8][0] = 1 / self.m B[9][1] = 1 / self.Ix B[10][2] = 1 / self.Iy B[11][3] = 1 / self.Iz Bc = np.zeros((12, 4)) Bc = np.vstack((Bc, -np.eye(4))) Bcon = np.concatenate((B, Bc), axis=-1) Dcon = np.zeros((4, 8)) for i in range(0, 4): C[i][i] = 1 # for i in range(12,16): for i in range(0, 4): A[i + 12][i] = 1 CT = signal.StateSpace(A, Bcon, C, Dcon) DT = CT.to_discrete(delT) # DT = signal.StateSpace(A, B, C, D, dt=delT) A_d = DT.A B_d = DT.B[:, :4] Bc_d = DT.B[:, 4:] C_d = DT.C D_d = DT.D[:, :4] # ----------------- Your Code Ends Here ----------------- # # Record the matrix for later use self.B = B # continuous version of B self.A_d = A_d # discrete version of A self.B_d = B_d # discrete version of B self.Bc_d = Bc_d # discrete version of Bc # ----------------- Example code ----------------- # max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3. max_states = np.array([ 0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI ]) max_inputs = np.array( [0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # Q = np.diag(1/max_states**2)*22 Q = np.diag(1 / max_states**2) # R = np.diag(1/max_inputs**2)*80 R = np.diag(1 / max_inputs**2) * 0.5 # Q = np.eye(16)*0.5 # R = np.eye(4)*100 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.Kbl = -0.8 * K [K_CT, _, _] = lqr(A, B, Q, R) Kbl_CT = -K_CT # print("Kbl_CT size\n") # print(np.shape(Kbl_CT)) # initialize adaptive controller gain to baseline LQR controller gain self.K_ad = self.Kbl.T # ----------------- Example code ----------------- # # self.Gamma = 3e-3 * np.eye(16) # Q_lyap = np.copy(Q) # Q_lyap[0:3,0:3] *= 30 # Q_lyap[6:9,6:9] *= 150 # Q_lyap[14,14] *= 2e-3 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable value for Gamma matrix and Q_lyap # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance self.Gamma = 3e-3 * np.eye(16) Q_lyap = np.copy(Q) Q_lyap[0:3, 0:3] *= 30 Q_lyap[6:9, 6:9] *= 150 Q_lyap[14, 14] *= 2e-3 # ----------------- Your Code Ends Here ----------------- # A_m = A + self.B @ Kbl_CT print(np.shape(A_m)) self.P = solve_continuous_lyapunov(A_m.T, -Q_lyap)
def initializeGainMatrix(self): """ Calculate the LQR gain matrix and matrices for adaptive controller. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model delT = self.timestep*1e-3 g = self.g m = self.m Ix = self.Ix Iy = self.Iy Iz = self.Iz n_p = 12 # number of states m = 4 # number of integral error terms # ----------------- Your Code Here ----------------- # # Compute the continuous A, B, Bc, C, D and # discretized A_d, B_d, Bc_d, C_d, D_d, for the computation of LQR gain A = np.zeros((16, 16)) A[0,6] = 1; A[1,7] = 1; A[2,8] = 1; A[3,9] = 1; A[4,10] = 1; A[5,11] = 1 A[6,4] = g; A[7,3] = -g A[12,0] = 1 A[13,1] = 1 A[14,2] = 1 A[15,5] = 1 B = np.zeros((16, 4)) B[8,0] = 1 / m; B[9,1] = 1 / Ix; B[10,2] = 1 / Iy; B[11,3] = 1 / Iz Bc = np.zeros((16,4)) Bc[12,0] = -1; Bc[13,1] = -1; Bc[14,2] = -1; Bc[15,3] = -1 concentrate_B = np.concatenate((B, Bc), axis=1) C = np.zeros((4,16)) C[0,0] = 1; C[1,1] = 1; C[2,2] = 1; C[3,5] = 1 D = np.zeros((4,8)) sys_Con = signal.StateSpace(A,concentrate_B,C,D) sys_Dis = sys_Con.to_discrete(delT) A_d = sys_Dis.A B_d = sys_Dis.B[:,0:4] Bc_d = sys_Dis.B[:,4:8] C_d = sys_Dis.C D_d = sys_Dis.D # ----------------- Your Code Ends Here ----------------- # # Record the matrix for later use self.B = B # continuous version of B self.A_d = A_d # discrete version of A self.B_d = B_d # discrete version of B self.Bc_d = Bc_d # discrete version of Bc # ----------------- Example code ----------------- # # max_pos = 15.0 # max_ang = 0.2 * self.pi # max_vel = 6.0 # max_rate = 0.015 * self.pi # max_eyI = 3. # max_states = np.array([0.1 * max_pos, 0.1 * max_pos, max_pos, # max_ang, max_ang, max_ang, # 0.5 * max_vel, 0.5 * max_vel, max_vel, # max_rate, max_rate, max_rate, # 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI]) # max_inputs = np.array([0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # Q = np.diag(1/max_states**2) # R = np.diag(1/max_inputs**2) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance max_pos = 15.0 max_ang = 0.2 * self.pi max_vel = 6.0 max_rate = 0.015 * self.pi max_eyI = 3.0 max_states = np.array([0.1 * max_pos, 0.1 * max_pos, max_pos, max_ang, max_ang, max_ang, 0.5 * max_vel, 0.5 * max_vel, max_vel, max_rate, max_rate, max_rate, 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI]) max_inputs = np.array([0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) Q = np.diag(1/max_states**2) R = np.diag(1/max_inputs**2) # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.Kbl = -K [K_CT, _, _] = lqr(A, B, Q, R) Kbl_CT = -K_CT # initialize adaptive controller gain to baseline LQR controller gain self.K_ad = self.Kbl.T # ----------------- Example code ----------------- # # self.Gamma = 3e-3 * np.eye(16) # Q_lyap = np.copy(Q) # Q_lyap[0:3,0:3] *= 30 # Q_lyap[6:9,6:9] *= 150 # Q_lyap[14,14] *= 2e-3 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable value for Gamma matrix and Q_lyap # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance self.Gamma = 3e-3 * np.eye(16) Q_lyap = np.copy(Q) Q_lyap[0:3,0:3] *= 30 Q_lyap[6:9,6:9] *= 150 Q_lyap[14,14] *= 2e-3 # ----------------- Your Code Ends Here ----------------- # A_m = A + self.B @ Kbl_CT self.P = solve_continuous_lyapunov(A_m.T, -Q_lyap)
def initializeGainMatrix(self): """ Calculate the LQR gain matrix and matrices for adaptive controller. """ # ---------------|LQR Controller|------------------------- # Use the results of linearization to create a state-space model n_p = 12 # number of states m = 4 # number of integral error terms # ----------------- Your Code Here ----------------- # # Compute the continuous A, B, Bc, C, D and # discretized A_d, B_d, Bc_d, C_d, D_d, for the computation of LQR gain # ----------------- Your Code Ends Here ----------------- # # Record the matrix for later use self.B = B # continuous version of B self.A_d = A_d # discrete version of A self.B_d = B_d # discrete version of B self.Bc_d = Bc_d # discrete version of Bc # ----------------- Example code ----------------- # # max_pos = 15.0 # max_ang = 0.2 * self.pi # max_vel = 6.0 # max_rate = 0.015 * self.pi # max_eyI = 3. # max_states = np.array([0.1 * max_pos, 0.1 * max_pos, max_pos, # max_ang, max_ang, max_ang, # 0.5 * max_vel, 0.5 * max_vel, max_vel, # max_rate, max_rate, max_rate, # 0.1 * max_eyI, 0.1 * max_eyI, 1 * max_eyI, 0.1 * max_eyI]) # max_inputs = np.array([0.2 * self.U1_max, self.U1_max, self.U1_max, self.U1_max]) # Q = np.diag(1/max_states**2) # R = np.diag(1/max_inputs**2) # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable values for Q and R (state and control weights) # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # # solve for LQR gains [K, _, _] = dlqr(A_d, B_d, Q, R) self.Kbl = -K [K_CT, _, _] = lqr(A, B, Q, R) Kbl_CT = -K_CT # initialize adaptive controller gain to baseline LQR controller gain self.K_ad = self.Kbl.T # ----------------- Example code ----------------- # # self.Gamma = 3e-3 * np.eye(16) # Q_lyap = np.copy(Q) # Q_lyap[0:3,0:3] *= 30 # Q_lyap[6:9,6:9] *= 150 # Q_lyap[14,14] *= 2e-3 # ----------------- Example code Ends ----------------- # # ----------------- Your Code Here ----------------- # # Come up with reasonable value for Gamma matrix and Q_lyap # The example code above is a good starting point, feel free to use them or write you own. # Tune them to get the better performance # ----------------- Your Code Ends Here ----------------- # A_m = A + self.B @ Kbl_CT self.P = solve_continuous_lyapunov(A_m.T, -Q_lyap)