def _update_velocity_data(self, wind=np.zeros((6, 1))):
# The first three elements are the steady state wind in the inertial frame
# The second three elements are the gust in the body frame
Vw_b = Quaternion2Rotation(self._state[6], self._state[7], self._state[8], self._state[9]) \
@ np.array([wind[0],
wind[1],
wind[2]]) + np.array([wind[3],
wind[4],
wind[5]])
Va_b = np.array([[self._state[3]],
[self._state[4]],
[self._state[5]]]) - Vw_b
# compute airspeed (in body frame):
self._Va = np.linalg.norm(Va_b)
if self._Va == 0:
self._alpha = 0
self._beta = 0
else:
# compute angle of attack
self._alpha = np.arctan2(Va_b.item(2), Va_b.item(0))
self._alpha = self._alpha.item(0)
# compute sideslip angle
self._beta = np.arcsin(Va_b.item(1) / self._Va)
# compute ground velocity
Vg_v = Quaternion2Rotation(self._state[6], self._state[7], self._state[8], self._state[9]).T @ (Va_b + Vw_b)
self._Vg = np.linalg.norm(Vg_v)
self._chi = np.arctan2(Vg_v.item(1), Vg_v.item(0))
self._gamma = np.arctan2(Vg_v[2], Vg_v.item(0))
def calcForcesAndMoments(self, delta):
# Calculate gravitational forces in the body frame
Rb_v = Quaternion2Rotation(self._state[6:10]).T
fb_grav = Rb_v @ np.array([[0, 0, MAV.mass * MAV.gravity]]).T
# Calculating longitudinal forces and moments
fx, fz, m = self.calcLongitudinalForcesAndMoments(delta.item(0))
fx += fb_grav.item(0)
fz += fb_grav.item(2)
# Calculating lateral forces and moments
fy, l, n = self.calcLateralForcesAndMoments(delta.item(2),
delta.item(3))
fy += fb_grav.item(1)
# Propeller force and moments
#These may act a little fast
fp, qp = self.calcThrustForceAndMoment(delta.item(1), self._Va)
fx += fp
l += -qp
self._forces[0] = fx
self._forces[1] = fy
self._forces[2] = fz
return np.array([[fx, fy, fz, l, m, n]]).T
def _forces_moments(self, delta):
"""
return the forces on the UAV based on the state, wind, and control surfaces
:param delta: np.matrix(delta_a, delta_e, delta_r, delta_t)
:return: Forces and Moments on the UAV np.matrix(Fx, Fy, Fz, Ml, Mn, Mm)
"""
fb_grav = Quaternion2Rotation(self._state[6:10]).T @ np.array(
[[0, 0, MAV.mass * MAV.gravity]]).T
# longitudinal forces and moments
fx, fz, m = self.calcLonDynamics(delta.item(0))
fx += fb_grav.item(0)
fz += fb_grav.item(2)
# lateral forces and moments
fy, l, n = self.calcLatDynamics(delta.item(2), delta.item(3))
fy += fb_grav.item(1)
# propeller/motor forces and moments
fp, mp = self.calcMotorDynamics(self._Va, delta.item(1))
fx += fp
l -= mp
self._forces[0] = fx
self._forces[1] = fy
self._forces[2] = fz
return np.array([[fx, fy, fz, l, m, n]]).T
def _update_velocity_data(self, wind=np.zeros((6, 1))):
Rb_v = Quaternion2Rotation(self._state[6:10])
self._wind = Rb_v.T @ wind[:3] + wind[3:]
V = self._state[3:6]
Vr = V - self._wind
# compute airspeed
self._Va = np.linalg.norm(Vr)
ur = Vr.item(0)
vr = Vr.item(1)
wr = Vr.item(2)
# compute angle of attack
if ur == 0:
self._alpha = np.sign(wr) * np.pi / 2.0
else:
self._alpha = np.arctan2(wr, ur)
# compute sideslip angle
if ur == 0 and wr == 0:
self._beta = np.sign(vr) * np.pi / 2.0
else:
self._beta = np.arcsin(vr / self._Va)
def _update_velocity_data(self, wind=np.zeros((6, 1))):
# Pull the wind data
steady_state = wind[0:3]
gust = wind[3:6]
# Pull MAV angles and rotate
e = self._state[6:10]
R = Quaternion2Rotation(e)
# Rotate steady to body and add gusts
steady_state_NED = R.T @ steady_state + gust
# Calculate airspeed components
u, v, w = self._state[3:6]
# Get relative u,v,w componentns in NED
u_r = u - steady_state_NED[0]
v_r = v - steady_state_NED[1]
w_r = w - steady_state_NED[2]
# compute airspeed
self._Va = np.sqrt(u_r**2 + v_r**2 + w_r**2).item(0)
# compute angle of attack
self._alpha = np.arctan(w_r / u_r).item(0)
# compute sideslip angle
self._beta = np.sin(v_r / self._Va).item(0)
def _derivatives(self, state, forces_moments):
"""
for the dynamics xdot = f(x, u), returns f(x, u)
"""
# extract the states
pn = state.item(0)
pe = state.item(1)
pd = state.item(2)
u = state.item(3)
v = state.item(4)
w = state.item(5)
e0 = state.item(6)
e1 = state.item(7)
e2 = state.item(8)
e3 = state.item(9)
p = state.item(10)
q = state.item(11)
r = state.item(12)
# extract forces/moments
fx = forces_moments.item(0)
fy = forces_moments.item(1)
fz = forces_moments.item(2)
l = forces_moments.item(3)
m = forces_moments.item(4)
n = forces_moments.item(5)
# position kinematics
Rv_b = Quaternion2Rotation(np.array([e0, e1, e2, e3]))
pos_dot = Rv_b @ np.array([u, v, w]).T
pn_dot = pos_dot.item(0)
pe_dot = pos_dot.item(1)
pd_dot = pos_dot.item(2)
# position dynamics
u_dot = r * v - q * w + 1 / MAV.mass * fx
v_dot = p * w - r * u + 1 / MAV.mass * fy
w_dot = q * u - p * v + 1 / MAV.mass * fz
# rotational kinematics
e0_dot = (-p * e1 - q * e2 - r * e3) * 0.5
e1_dot = (p * e0 + r * e2 - q * e3) * 0.5
e2_dot = (q * e0 - r * e1 + p * e3) * 0.5
e3_dot = (r * e0 + q * e1 - p * e2) * 0.5
# rotatonal dynamics
p_dot = MAV.gamma1 * p * q - MAV.gamma2 * q * r + MAV.gamma3 * l + MAV.gamma4 * n
q_dot = MAV.gamma5 * p * r - MAV.gamma6 * (p**2 -
r**2) + 1 / MAV.Jy * m
r_dot = MAV.gamma7 * p * q - MAV.gamma1 * q * r + MAV.gamma4 * l + MAV.gamma8 * n
# collect the derivative of the states
x_dot = np.array([[
pn_dot, pe_dot, pd_dot, u_dot, v_dot, w_dot, e0_dot, e1_dot,
e2_dot, e3_dot, p_dot, q_dot, r_dot
]]).T
return x_dot
def _update_gamma_chi(self):
Rv_b = Quaternion2Rotation(self._state[6:10])
Vg = Rv_b @ self._state[3:6]
gamma = asin(-Vg.item(2) / np.linalg.norm(Vg))
self.msg_true_state.gamma = gamma
#Vg_horz = Vg * np.cos(gamma)
#e1 = np.array([[1,0,0]]).T
#chi = acos(e1.T @ Vg_horz / np.linalg.norm(Vg_horz))
#if Vg_horz.item(1) < 0:
# chi *= -1
chi = np.arctan2(Vg.item(1), Vg.item(0))
self.msg_true_state.chi = chi
def updateVelocityData(self, wind=np.zeros((6, 1))):
Rb_v = Quaternion2Rotation(self._state[6:10]).T
#wind in body frame
self._wind = Rb_v @ wind[0:3] + wind[3:]
V = self._state[3:6]
#Compute Va
Vr = V - self._wind
self._Va = np.linalg.norm(Vr)
#Compute alpha
self._alpha = np.arctan2(Vr.item(2), Vr.item(0))
#Compute beta
self._beta = asin(Vr.item(1)/self._Va)
def calcGammaAndChi(self):
Rv_b = Quaternion2Rotation(self._state[6:10])
Vg = Rv_b @ self._state[3:6]
gamma = asin(
-Vg.item(2) /
np.linalg.norm(Vg)) #negative because h_dot = Vg sin(gamma)
self.msg_true_state.gamma = gamma
Vg_horz = Vg * np.cos(gamma)
e1 = np.array([[1, 0, 0]]).T
chi = acos(np.dot(e1.T, Vg_horz) / np.linalg.norm(Vg_horz))
if (Vg_horz.item(1) < 0):
chi *= -1
self.msg_true_state.chi = chi
def updateVelocityData(self, wind=np.zeros((6, 1))):
Rb_v = Quaternion2Rotation(self._state[6:10]).T
#wind in body frame
self._wind = Rb_v @ wind[0:3] + wind[3:]
V = self._state[3:6]
#Compute Va
Vr = V - self._wind
self._Va = np.linalg.norm(Vr)
#Compute alpha
if Vr.item(0) == 0:
self._alpha = np.sign(Vr.item(2)) * np.pi / 2.0
else:
self._alpha = np.arctan2(Vr.item(2), Vr.item(0))
#Compute beta
temp = np.sqrt(Vr.item(0)**2 + Vr.item(2)**2)
if temp == 0:
self._beta = np.sign(Vr.item(1)) * np.pi / 2.0
else:
self._beta = asin(Vr.item(1) / self._Va)
def calcForcesAndMoments(self, delta):
# Calculate gravitational forces in the body frame
fb_grav = Quaternion2Rotation(self._state[6:10]).T @ np.array(
[[0, 0, MAV.mass * MAV.gravity]]).T
# Calculating longitudinal forces and moments
fx, fz, m = self.calcLongitudinalForcesAndMoments(delta.item(0))
fx += fb_grav[0]
fz += fb_grav[2]
# Calculating lateral forces and moments
fy, l, n = self.calcLateralForcesAndMoments(delta.item(2),
delta.item(3))
fy += fb_grav[1]
# Propeller force and moments
#These may act a little fast
fp, mp = self.calcThrustForceAndMoment(delta.item(1))
fx += fp
l += mp
return np.array([fx, fy, fz, l, m, n])
def _update_msg_true_state(self):
# update the class structure for the true state:
# [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz]
phi, theta, psi = Quaternion2Euler(self._state[6:10])
pdot = Quaternion2Rotation(self._state[6:10]) @ self._state[3:6]
self.msg_true_state.pn = self._state.item(0)
self.msg_true_state.pe = self._state.item(1)
self.msg_true_state.h = -self._state.item(2)
self.msg_true_state.Va = self._Va
self.msg_true_state.alpha = self._alpha
self.msg_true_state.beta = self._beta
self.msg_true_state.phi = phi
self.msg_true_state.theta = theta
self.msg_true_state.psi = psi
self.msg_true_state.Vg = np.linalg.norm(pdot)
self.msg_true_state.gamma = np.arcsin(
pdot.item(2) / self.msg_true_state.Vg)
self.msg_true_state.chi = np.arctan2(pdot.item(1), pdot.item(0))
self.msg_true_state.p = self._state.item(10)
self.msg_true_state.q = self._state.item(11)
self.msg_true_state.r = self._state.item(12)
self.msg_true_state.wn = self._wind.item(0)
self.msg_true_state.we = self._wind.item(1)
def _forces_moments(self, delta):
"""
return the forces on the UAV based on the state, wind, and control surfaces
:param delta: np.matrix(delta_a, delta_e, delta_r, delta_t)
:return: Forces and Moments on the UAV np.matrix(Fx, Fy, Fz, Ml, Mn, Mm)
"""
mass = UAV.mass
g = UAV.gravity
rho = UAV.rho
S = UAV.S_wing
a = self._alpha
beta = self._beta
b = UAV.b
c = UAV.c
p = self._state.item(10)
q = self._state.item(11)
r = self._state.item(12)
Va = self._Va
# control surface offsets
delta_a = delta.item(0)
delta_e = delta.item(1)
delta_r = delta.item(2)
delta_t = delta.item(3)
if delta_t < 0:
delta_t = 0.0
# drag coefficients
C_D_q = UAV.C_D_q
C_L_q = UAV.C_L_q
C_D_de = UAV.C_D_delta_e
C_L_de = UAV.C_L_delta_e
C_Y_0 = UAV.C_Y_0
C_Y_b = UAV.C_Y_beta
C_Y_p = UAV.C_Y_p
C_Y_r = UAV.C_Y_r
C_Y_da = UAV.C_Y_delta_a
C_Y_dr = UAV.C_Y_delta_r
C_l_0 = UAV.C_ell_0
C_l_b = UAV.C_ell_beta
C_l_p = UAV.C_ell_p
C_l_r = UAV.C_ell_r
C_l_da = UAV.C_ell_delta_a
C_l_dr = UAV.C_ell_delta_r
C_m_0 = UAV.C_m_0
C_m_a = UAV.C_m_alpha
C_m_q = UAV.C_m_q
C_m_de = UAV.C_m_delta_e
C_n_0 = UAV.C_n_0
C_n_b = UAV.C_n_beta
C_n_p = UAV.C_n_p
C_n_r = UAV.C_n_r
C_n_da = UAV.C_n_delta_a
C_n_dr = UAV.C_n_delta_r
f_g = Quaternion2Rotation(self._state[6:10]).transpose() @ np.array(
[[0], [0], [mass * g]]) # gravitational force
# Propeller Thrust Calculations
# map delta throttle command (0 to 1) into motor input voltage
V_in = UAV.V_max * delta_t
# Quadratic formula to solve for motor speed
a1 = UAV.rho * UAV.D_prop**5 / ((2.0 * np.pi)**2) * UAV.C_Q0
b1 = UAV.rho * UAV.D_prop**4 / (2.0 * np.pi) * UAV.C_Q1 * self._Va + (
UAV.KQ**2) / UAV.R_motor
c1 = UAV.rho * UAV.D_prop**3 * UAV.C_Q2 * self._Va**2 - UAV.KQ / UAV.R_motor * V_in + UAV.KQ * UAV.i0
# Consider only positive root
Omega_op = (-b1 + np.sqrt(b1**2 - 4 * a1 * c1)) / (2.0 * a1)
# compute advance ratio
J_op = 2 * np.pi * self._Va / (Omega_op * UAV.D_prop)
# compute non-dimensionalized coefficients of thrust and torque
C_T = UAV.C_T2 * J_op**2 + UAV.C_T1 * J_op + UAV.C_T0
C_Q = UAV.C_Q2 * J_op**2 + UAV.C_Q1 * J_op + UAV.C_Q0
# add thrust and torque due to propeller
n = Omega_op / (2.0 * np.pi)
f_p = np.array([[UAV.rho * n**2 * UAV.D_prop**4 * C_T], [0], [0]])
m_p = np.array([[-UAV.rho * n**2 * UAV.D_prop**5 * C_Q], [0], [0]])
# # Alternative Simplified Thrust model
# f_p = (0.5*UAV.rho*UAV.S_prop*UAV.C_prop) * np.array([[(UAV.k_motor*delta_t)**2 - self.msg_true_state.Va**2], [0], [0]]) # prop thrust
# m_p = np.array([[-UAV.kTp*(UAV.kOmega*delta_t)**2], [0], [0]]) # prop torque
C_D = lambda alpha: UAV.C_D_p + (UAV.C_L_0 + UAV.C_L_alpha * alpha
)**2 / (np.pi * UAV.e * UAV.AR)
sig = lambda alpha: (1 + np.exp(-UAV.M * (alpha - UAV.alpha0)) + np.
exp(UAV.M * (alpha + UAV.alpha0))) / (
(1 + np.exp(-UAV.M * (alpha - UAV.alpha0))) *
(1 + np.exp(UAV.M * (alpha + UAV.alpha0))))
C_L = lambda alpha: (1 - sig(alpha)) * (
UAV.C_L_0 + UAV.C_L_alpha * alpha) + sig(alpha) * (2 * np.sign(
alpha) * np.sin(alpha)**2 * np.cos(alpha))
C_X = lambda alpha: -C_D(alpha) * np.cos(alpha) + C_L(alpha) * np.sin(
alpha)
C_X_q = lambda alpha: -C_D_q * np.cos(alpha) + C_L_q * np.sin(alpha)
C_X_de = lambda alpha: -C_D_de * np.cos(alpha) + C_L_de * np.sin(alpha)
C_Z = lambda alpha: -C_D(alpha) * np.sin(alpha) - C_L(alpha) * np.cos(
alpha)
C_Z_q = lambda alpha: -C_D_q * np.sin(alpha) - C_L_q * np.cos(alpha)
C_Z_de = lambda alpha: -C_D_de * np.sin(alpha) - C_L_de * np.cos(alpha)
f_a = (0.5 * rho * Va**2 * S) * np.array(
[[C_X(a) + C_X_q(a) * (c / (2 * Va)) * q + C_X_de(a) * delta_e],
[
C_Y_0 + C_Y_b * beta + C_Y_p * (b / (2 * Va)) * p + C_Y_r *
(b / (2 * Va)) * r + C_Y_da * delta_a + C_Y_dr * delta_r
], [C_Z(a) + C_Z_q(a) * (c /
(2 * Va)) * q + C_Z_de(a) * delta_e]])
f_total = f_g + f_a + f_p
self.thrust = f_p.item(0)
self._forces = np.array([[f_total.item(0)], [f_total.item(1)],
[f_total.item(2)]])
m_a = (0.5 * rho * Va**2 * S) * np.array(
[[
b * (C_l_0 + C_l_b * beta + C_l_p * (b /
(2 * Va)) * p + C_l_r *
(b / (2 * Va)) * r + C_l_da * delta_a + C_l_dr * delta_r)
],
[
c * (C_m_0 + C_m_a * a + C_m_q *
(c / (2 * Va)) * q + C_m_de * delta_e)
],
[
b * (C_n_0 + C_n_b * beta + C_n_p * (b /
(2 * Va)) * p + C_n_r *
(b / (2 * Va)) * r + C_n_da * delta_a + C_n_dr * delta_r)
]])
m_total = m_a + m_p
return np.array([[
f_total.item(0),
f_total.item(1),
f_total.item(2),
m_total.item(0),
m_total.item(1),
m_total.item(2)
]]).T