def dq_dtheta(euler):
h = 0.005
Jacobian = np.zeros((4, 3))
for i in range(3):
e_p = np.copy(euler)
e_p[i][0] += h
e_m = np.copy(euler)
e_m[i][0] -= h
f_p = Euler2Quaternion(e_p.item(0), e_p.item(1), e_p.item(2))
f_m = Euler2Quaternion(e_m.item(0), e_m.item(1), e_m.item(2))
Jac_col_i = (f_p - f_m) / (2 * h)
Jacobian[:, i] = Jac_col_i[:, 0]
return Jacobian
def convert(self):
quat = Euler2Quaternion(self.phi, self.theta, self.psi)
e0 = quat.item(0)
e1 = quat.item(1)
e2 = quat.item(2)
e3 = quat.item(3)
# put the quaternion on the GUI
self.e0box.delete(0.0, 20.20)
self.e0box.insert(0.0, 'quaternion = \n' + str(quat))
self.THETA = np.arccos(e0) * 2.0
# put the number on the GUI
self.ThetaBox.delete(0.0, 20.20)
self.ThetaBox.insert(
0.0, 'theta = \n' + str(round(np.degrees(self.THETA), 5)) + ' deg')
self.vector_v = quat[1:, :] * np.sin(self.THETA / 2)
# put the number on the GUI
self.VectorBox.delete(0.0, 20.20)
showv = np.copy(self.vector_v)
showv[2] = -showv[2]
self.VectorBox.insert(0.0,
'v = \n' + str(showv / np.linalg.norm(showv)))
# make the vector_v big for plotting purposes
test = 8 * self.vector_v / np.linalg.norm(self.vector_v)
if not np.isnan(self.vector_v.item(0)):
R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
self.vector_v = R @ test
self.plot()
def compute_trim(mav, Va, gamma):
# define initial state and input
e = Euler2Quaternion(0., gamma, 0.)
state0 = np.array([[0], # (0)
[0], # (1)
[mav._state[2]], # (2)
[Va], # (3)
[0], # (4)
[0], # (5)
[e.item(0)], # (6)
[e.item(1)], # (7)
[e.item(2)], # (8)
[e.item(3)], # (9)
[0], # (10)
[0], # (11)
[0] # (12)
])
delta0 = np.array([
[0.], #de
[0.5], #dt
[0.0], #da
[0.0], #dr
])
x0 = np.concatenate((state0, delta0), axis=0)
# define equality constraints
bnds = ((None, None),(None, None),(None, None),(None, None),\
(None, None),(None, None),(None, None),(None, None),\
(None, None),(None, None),(None, None),(None, None),(None, None),\
(-1.0,1.0),(-1.0,1.0),(-1.0,1.0),(-1.0,1.0))
cons = ({'type': 'eq',
'fun': lambda x: np.array([
x[3]**2 + x[4]**2 + x[5]**2 - Va**2, # magnitude of velocity vector is Va
x[4], # v=0, force side velocity to be zero
x[6]**2 + x[7]**2 + x[8]**2 + x[9]**2 - 1., # force quaternion to be unit length
x[7], # e1=0 - forcing e1=e3=0 ensures zero roll and zero yaw in trim
x[9], # e3=0
x[10], # p=0 - angular rates should all be zero
x[11], # q=0
x[12], # r=0
]),
'jac': lambda x: np.array([
[0., 0., 0., 2*x[3], 2*x[4], 2*x[5], 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., 0., 0., 2*x[6], 2*x[7], 2*x[8], 2*x[9], 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., 0., 0., 1., 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., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.],
])
})
# solve the minimization problem to find the trim states and inputs
res = minimize(trim_objective, x0, method='SLSQP', args = (mav, Va, gamma),bounds=bnds,
constraints=cons, options={'ftol': 1e-10, 'disp': True})
# extract trim state and input and return
trim_state = np.array([res.x[0:13]]).T
trim_input = np.array([res.x[13:17]]).T
return trim_state, trim_input
def quaternion_state(x_euler):
# convert state x_euler with attitude represented by Euler angles
# to x_quat with attitude represented by quaternions
e = Euler2Quaternion(x_euler.item(6), x_euler.item(7), x_euler.item(8))
x_quat = np.array([[x_euler.item(0)], [x_euler.item(1)], [x_euler.item(2)],
[x_euler.item(3)], [x_euler.item(4)], [x_euler.item(5)],
[e.item(0)], [e.item(1)], [e.item(2)], [e.item(3)],
[x_euler.item(9)], [x_euler.item(10)],
[x_euler.item(11)]])
return x_quat
def quaternion_state(x_euler):
# convert state x_euler with attitude represented by Euler angles
# to x_quat with attitude represented by quaternions
phi = x_euler[6]
theta = x_euler[7]
psi = x_euler[8]
e0, e1, e2, e3 = Euler2Quaternion(phi, theta, psi)
x_quat = x_euler.copy()
x_quat[6] = e0
x_quat[7] = e1
x_quat[8] = e2
x_quat = np.insert(x_quat, 9, [e3], axis=0)
return x_quat
def quaternion_state(x_euler):
# convert state x_euler with attitude represented by Euler angles
# to x_quat with attitude represented by quaternions
phi = x_euler.item(6)
theta = x_euler.item(7)
psi = x_euler.item(8)
q = Euler2Quaternion(phi, theta, psi)
x_quat = np.empty((13, 1))
x_quat[:6] = x_euler[:6]
x_quat[6:10] = q
x_quat[10:13] = x_euler[9:12]
return x_quat
def showMeQuat(self):
beginquat = np.array([[1, 0, 0, 0]]).T
endquat = Euler2Quaternion(self.phi, self.theta, self.psi)
dist = np.linalg.norm(beginquat - endquat)
howmany = int(dist * 100 / 1.2)
e0 = np.linspace(beginquat.item(0), endquat.item(0), num=howmany)
e1 = np.linspace(beginquat.item(1), endquat.item(1), num=howmany)
e2 = np.linspace(beginquat.item(2), endquat.item(2), num=howmany)
e3 = np.linspace(beginquat.item(3), endquat.item(3), num=howmany)
for i in range(howmany):
quat = np.array([[e0[i], e1[i], e2[i], e3[i]]]).T
phi, theta, psi = Quaternion2Euler(quat)
self.plot2(phi, theta, psi)
self.plot2(self.phi, self.theta, self.psi)
def quaternion_state(x_euler):
# convert state x_euler with attitude represented by Euler angles
# to x_quat with attitude represented by quaternions
e = Euler2Quaternion(x_euler[6], x_euler[7], x_euler[8])
x_quat = np.array([
[x_euler[0][0]], # (0)
[x_euler[1][0]], # (1)
[x_euler[2][0]], # (2)
[x_euler[3][0]], # (3)
[x_euler[4][0]], # (4)
[x_euler[5][0]], # (5)
[e[0][0]], # (6)
[e[1][0]], # (7)
[e[2][0]], # (8)
[e[3][0]], # (9)
[x_euler[9][0]], # (10)
[x_euler[10][0]], # (11)
[x_euler[11][0]]
]) # (12)
return x_quat
def compute_trim(mav, Va, gamma, R, display=False):
# define initial state and input
e = Euler2Quaternion(0.0, gamma, 0.0)
state0 = np.array([
[mav._state.item(0)], # (0) Position North
[mav._state.item(1)], # (1) Position East
[mav._state.item(2)], # (2) Position Down
[Va], # (3) Velocity body x
[0.0], # (4) Velocity body y
[0.0], # (5) Velocity body z
[e.item(0)], # (6) Quaternion e0
[e.item(1)], # (7) Quaternion e1
[e.item(2)], # (8) Quaternion e2
[e.item(3)], # (9) Quaternion e3
[0.0], # (10) Angular velocity body x
[0.0], # (11) Angular velocity body y
[0.0]
]) # (12) Angular velocity body z
delta0 = np.array([
[0.0], # Aeleron
[0.0], # Elevator
[0.0], # Rudder
[0.5]
]) # Throttle
x0 = np.concatenate((state0, delta0), axis=0)
# define equality constraints
cons = ({
'type':
'eq',
'fun':
lambda x: np.array([
x[3]**2 + x[4]**2 + x[
5]**2 - Va**2, # magnitude of velocity vector is Va
x[4], # v=0, force side velocity to be zero
x[6]**2 + x[7]**2 + x[8]**2 + x[9]**2 -
1., # force quaternion to be unit length
x[
7
], # e1=0 - forcing e1=e3=0 ensures zero roll and zero yaw in trim
x[9], # e3=0
x[10], # p=0 - angular rates should all be zero
x[11], # q=0
x[12], # r=0
]),
'jac':
lambda x: np.array([
[
0., 0., 0., 2 * x[3], 2 * x[4], 2 * x[5], 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., 0., 0., 2 * x[6], 2 * x[7], 2 * x[8], 2 * x[9],
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., 0., 0., 1., 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., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
0.
],
[
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.,
0.
],
])
})
# solve the minimization problem to find the trim states and inputs
res = minimize(trim_objective,
x0,
method='SLSQP',
args=(mav, Va, gamma, R),
constraints=cons,
options={
'ftol': 1e-10,
'disp': True
})
# extract trim state and input and return
trim_state = np.array([res.x[0:13]]).T
trim_input = np.array([res.x[13:17]]).T
if display:
print("Optimized Trim Output")
print("trim_states: \n", trim_state, "\n")
print("trim_inputs: \n", trim_input, "\n")
return trim_state, trim_input
# Initial conditions for MAV
pn0 = 0. # initial north position
pe0 = 0. # initial east position
pd0 = -100.0 # initial down position
u0 = 25. # initial velocity along body x-axis
v0 = 0. # initial velocity along body y-axis
w0 = 0. # initial velocity along body z-axis
phi0 = 0. # initial roll angle
theta0 = 0.1 # initial pitch angle
psi0 = 0.0 # initial yaw angle
p0 = 0.0 # initial roll rate
q0 = 0.0 # initial pitch rate
r0 = 0.0 # initial yaw rate
Va0 = np.sqrt(u0**2 + v0**2 + w0**2)
# Quaternion State
e = Euler2Quaternion(phi0, theta0, psi0)
e0 = e.item(0)
e1 = e.item(1)
e2 = e.item(2)
e3 = e.item(3)
# Initial conditions noise for MAV
p_sigma = 0 #10
pn0_n = pn0 + np.random.normal(0, p_sigma) # initial north position
pe0_n = pe0 + np.random.normal(0, p_sigma) # initial east position
pd0_n = pd0 + np.random.normal(0, p_sigma) # initial down position
vel_sigma = 0 #1
u0_n = u0 + np.random.normal(0,
vel_sigma) # initial velocity along body x-axis
v0_n = v0 + np.random.normal(0,
vel_sigma) # initial velocity along body y-axis
pn0 = 0. # initial north position pe0 = 0. # initial east position pd0 = -20.0 # initial down position u0 = 25. # initial velocity along body x-axis v0 = 0. # initial velocity along body y-axis w0 = 0. # initial velocity along body z-axis phi0 = 0. # initial roll angle theta0 = 0. # initial pitch angle psi0 = 0.0 # initial yaw angle p0 = 0 # initial roll rate q0 = 0 # initial pitch rate r0 = 0 # initial yaw rate Va0 = np.sqrt(u0**2 + v0**2 + w0**2) # Quaternion State angles = [phi0, theta0, psi0] e = Euler2Quaternion(angles) # e0 = e.item(0) # e1 = e.item(1) # e2 = e.item(2) # e3 = e.item(3) e0 = e[0] e1 = e[1] e2 = e[2] e3 = e[3] ###################################################################################### # Physical Parameters ###################################################################################### mass = 13.5 #kg Jx = 0.8244 #kg m^2 Jy = 1.135
def compute_trim(mav, Va, gamma):
phi = 0.0
theta = gamma
psi = 0.0
e0, e1, e2, e3 = Euler2Quaternion(phi, theta, psi)
# define initial state and input
state0 = np.array([
[mav._state.item(0)], #pn
[mav._state.item(1)], #pe
[mav._state.item(2)], #pd
[Va], # u0
[0.0], # v0
[0.0], # w0
[e0], # e0
[e1], # e1
[e2], # e2
[e3], # e3
[0.0], # p0
[0.0], # q0
[0.0]
]) # r0
delta_a = 0.0
delta_e = 0.0
delta_r = 0.0
delta_t = 0.5
delta0 = np.array([[delta_a, delta_e, delta_r, delta_t]]).T
x0 = np.concatenate((state0, delta0), axis=0)
# define equality constraints
cons = ({
'type':
'eq',
'fun':
lambda x: np.array([
x[3]**2 + x[4]**2 + x[
5]**2 - Va**2, # magnitude of velocity vector is Va
x[4], # v=0, force side velocity to be zero
x[6]**2 + x[7]**2 + x[8]**2 + x[9]**2 -
1., # force quaternion to be unit length
x[
7
], # e1=0 - forcing e1=e3=0 ensures zero roll and zero yaw in trim
x[9], # e3=0
x[10], # p=0 - angular rates should all be zero
x[11], # q=0
x[12], # r=0
]),
'jac':
lambda x: np.array([
[
0., 0., 0., 2 * x[3], 2 * x[4], 2 * x[5], 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., 0., 0., 2 * x[6], 2 * x[7], 2 * x[8], 2 * x[9],
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., 0., 0., 1., 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., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
0.
],
[
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.,
0.
],
])
})
# solve the minimization problem to find the trim states and inputs
res = minimize(trim_objective,
x0,
method='SLSQP',
args=(mav, Va, gamma),
constraints=cons,
options={
'ftol': 1e-10,
'disp': True
})
# extract trim state and input and return
trim_state = np.array([res.x[0:13]]).T
trim_input = np.array([res.x[13:17]]).T
return trim_state, trim_input
wind = wind_simulation(SIM.ts_simulation)
mav = mav_dynamics(SIM.ts_simulation)
ctrl = quatopilot(SIM.ts_simulation)
# # autopilot commands -- Hover (In development)
# commands = msg_quatopilot()
# commands.p_ref = np.array([0, 0, -100])
# rpy = np.array([0, 90, 0])
# commands.q_ref = Euler2Quaternion(np.radians(rpy[0]), np.radians(rpy[1]), np.radians(rpy[2]))
# commands.u_ref = 0
# autopilot commands -- Off Axis
commands = msg_quatopilot()
commands.p_ref = np.array([10, 0, -100])
rpy = np.array([15, 10, -10])
commands.q_ref = Euler2Quaternion(np.radians(rpy[0]), np.radians(rpy[1]),
np.radians(rpy[2]))
commands.u_ref = 20
# # autopilot commands -- Straight and Level
# commands = msg_quatopilot()
# commands.p_ref = np.array([10, 0, -100])
# rpy = np.array([0, 30, 0])
# commands.q_ref = Euler2Quaternion(np.radians(rpy[0]), np.radians(rpy[1]), np.radians(rpy[2]))
# commands.u_ref = 20
# # autopilot commands -- Straight-Up
# commands = msg_quatopilot()
# commands.p_ref = np.array([40, 0, -110])
# rpy = np.array([0, 85, 0])
# commands.q_ref = Euler2Quaternion(np.radians(rpy[0]), np.radians(rpy[1]), np.radians(rpy[2]))
# commands.u_ref = 15
def compute_trim(mav, Va, gamma):
# define initial state and input
e = Euler2Quaternion(0, gamma, 0)
state0 = np.array([[
0., 0., -100., Va, 0., 0.,
e.item(0),
e.item(1),
e.item(2),
e.item(3), 0., 0., 0.
]]).T
delta0 = np.array([[0., 0.5, 0., 0.]]).T
x0 = np.concatenate((state0, delta0), axis=0)
# define equality constraints
cons = ({
'type':
'eq',
'fun':
lambda x: np.array([
x[3]**2 + x[4]**2 + x[
5]**2 - Va**2, # magnitude of velocity vector is Va
x[4], # v=0, force side velocity to be zero
x[6]**2 + x[7]**2 + x[8]**2 + x[9]**2 -
1., # force quaternion to be unit length
x[
7
], # e1=0 - forcing e1=e3=0 ensures zero roll and zero yaw in trim
x[9], # e3=0
x[10], # p=0 - angular rates should all be zero
x[11], # q=0
x[12], # r=0
]),
'jac':
lambda x: np.array([
[
0., 0., 0., 2 * x[3], 2 * x[4], 2 * x[5], 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., 0., 0., 2 * x[6], 2 * x[7], 2 * x[8], 2 * x[9],
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., 0., 0., 1., 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., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0.,
0.
],
[
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0.,
0.
],
])
})
# solve the minimization problem to find the trim states and inputs
res = minimize(trim_objective,
x0,
method='SLSQP',
args=(mav, Va, gamma),
constraints=cons,
options={
'ftol': 1e-10,
'disp': True
})
# extract trim state and input and return
trim_state = np.array([res.x[0:13]]).T
trim_input = np.array([
res.x[13:17]
]).T #These inputs are the same. Do I need to recalculate them?
return trim_state, trim_input