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)
