def _update_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.true_state.pn = self._state.item(0)
self.true_state.pe = self._state.item(1)
self.true_state.h = -self._state.item(2)
self.true_state.Va = self._Va
self.true_state.alpha = self._alpha
self.true_state.beta = self._beta
self.true_state.phi = phi
self.true_state.theta = theta
self.true_state.psi = psi
self.true_state.Vg = np.linalg.norm(pdot)
self.true_state.gamma = np.arcsin(pdot.item(2) / self.true_state.Vg)
self.true_state.chi = np.arctan2(pdot.item(1), pdot.item(0))
self.true_state.p = self._state.item(10)
self.true_state.q = self._state.item(11)
self.true_state.r = self._state.item(12)
self.true_state.wn = self._wind.item(0)
self.true_state.we = self._wind.item(1)
self.true_state.bx = SENSOR.gyro_x_bias
self.true_state.by = SENSOR.gyro_y_bias
self.true_state.bz = SENSOR.gyro_z_bias
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])
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
R = Euler2Rotation(phi, theta, psi)
self.Vg_i = R.T @ self.Vg_b
self.msg_true_state.Vg = self._Vg
self.msg_true_state.gamma = np.arctan2(
-self.Vg_i[2], np.sqrt(self.Vg_i[0]**2 + self.Vg_i[1]**2))
self.msg_true_state.chi = -np.arctan2(self.Vg_i[1], self.Vg_i[0])[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)
"""
phi, theta, psi = Quaternion2Euler(self._state[6:10])
p = self._state.item(10)
q = self._state.item(11)
r = self._state.item(12)
delta_e = delta.item(0)
delta_a = delta.item(1)
delta_r = delta.item(2)
delta_t = delta.item(3)
fx =
fy =
fz =
Mx =
My =
Mz =
self._forces[0] = fx
self._forces[1] = fy
self._forces[2] = fz
return np.array([[fx, fy, fz, Mx, My, Mz]]).T
def _update_velocity_data(self, wind=np.zeros((6, 1))):
# compute airspeed
phi, theta, psi = Quaternion2Euler(self._state[6:10])
Wnb = Inertial2Body(phi, theta, psi, wind.item(0), wind.item(1),
wind.item(2))[0]
Web = Inertial2Body(phi, theta, psi, wind.item(0), wind.item(1),
wind.item(2))[1]
Wdb = Inertial2Body(phi, theta, psi, wind.item(0), wind.item(1),
wind.item(2))[2]
u = self._state[3]
v = self._state[4]
w = self._state[5]
u_w = Wnb + wind.item(3)
v_w = Web + wind.item(4)
w_w = Wdb + wind.item(5)
u_r = u - u_w
v_r = v - v_w
w_r = w - w_w
self._Va = math.sqrt(u_r**2 + v_r**2 + w_r**2)
# compute angle of attack
self._alpha = np.arctan2(w_r, u_r)
# compute sideslip angle
self._beta = math.asin(v_r / self._Va)
def _update_velocity_data(self, wind=np.zeros((6, 1))):
e0 = self._state[6]
e1 = self._state[7]
e2 = self._state[8]
e3 = self._state[9]
e_array = np.array([e0, e1, e2, e3])
[phi, th, psi] = Quaternion2Euler(e_array)
# compute airspeed
wind_constant = Euler2Rotation(phi, th, psi) @ wind[0:3]
wind_b = np.array([[wind_constant[0][0] + wind[3][0]],
[wind_constant[1][0] + wind[4][0]],
[wind_constant[2][0] + wind[5][0]]])
self.Va_b = np.array([[self._state[3][0] - wind_b[0][0]],
[self._state[4][0] - wind_b[1][0]],
[self._state[5][0] - wind_b[2][0]]])
self._Va = np.linalg.norm(self.Va_b)
self.Vg_b = self.Va_b + wind_b
self._Vg = np.linalg.norm(self.Vg_b)
if self._Va == 0.0:
self._alpha = 0.0
self._beta = 0.0
else:
# compute angle of attack
self._alpha = np.arctan2(self.Va_b[2], self.Va_b[0])[0]
# compute sideslip angle
self._beta = np.arcsin(self.Va_b[1][0] / self._Va)
def compute_tf_model(mav, trim_state, trim_input):
# trim values
mav._state = trim_state
mav._update_velocity_data()
Va_trim = mav._Va
alpha_trim = mav._alpha
phi, theta_trim, psi = Quaternion2Euler(trim_state[6:10])
Va_star = trim_state[3]
alpha_star = MAV.alpha0
delta_e_star = trim_input[0].item(0)
delta_t_star = trim_input[1].item(0)
# define transfer function constants
a_phi1 = -1 / 2 * MAV.rho * mav._Va**2 * MAV.S_wing * MAV.b * MAV.C_p_p * MAV.b / (
2 * mav._Va)
a_phi2 = 1 / 2 * MAV.rho * mav._Va**2 * MAV.S_wing * MAV.b * MAV.C_p_delta_a
a_theta1 = -MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing / (
2 * MAV.Jy) * MAV.C_m_q * MAV.c / (2 * mav._Va)
a_theta2 = -MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing / (
2 * MAV.Jy) * MAV.C_m_alpha
a_theta3 = MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing / (
2 * MAV.Jy) * MAV.C_m_delta_e
a_V1 = MAV.rho * Va_star * MAV.S_wing / MAV.mass * (
MAV.C_D_0 + MAV.C_D_alpha * alpha_star + MAV.C_D_delta_e *
delta_e_star) + MAV.rho * MAV.S_prop / MAV.mass * MAV.C_prop * Va_star
a_V1 = a_V1.item(0)
a_V2 = MAV.rho * MAV.S_prop / MAV.mass * MAV.C_prop * MAV.k_motor**2 * delta_t_star
a_V3 = MAV.gravity
return Va_trim, alpha_trim, theta_trim, a_phi1, a_phi2, a_theta1, a_theta2, a_theta3, a_V1, a_V2, a_V3
def _update_true_state(self):
# update the true state message:
phi, theta, psi = Quaternion2Euler(self._state[6:10])
self.true_state.pn = self._state.item(0)
self.true_state.pe = self._state.item(1)
self.true_state.h = -self._state.item(2)
self.true_state.phi = phi
self.true_state.theta = theta
self.true_state.psi = psi
self.true_state.p = self._state.item(10)
self.true_state.q = self._state.item(11)
self.true_state.r = self._state.item(12)
def compute_tf_model(mav, trim_state, trim_input):
# trim values
rho = MAV.rho
S = MAV.S_wing
Va = mav._Va
b = MAV.b
beta = mav._beta
alpha = mav._alpha
c = MAV.c
Jy = MAV.Jy
g = MAV.gravity
mass = MAV.mass
a_phi_1 = 0.5 * rho * (Va**2) * S * b * MAV.C_p_p * b / (2 * Va)
a_phi_2 = 0.5 * rho * (Va**2) * S * b * MAV.C_p_delta_a
T_phi_delta_a = TF([a_phi_2], [1, -a_phi_1, 0])
T_chi_phi = TF(np.array([g / Va]), [1, 0])
beta_dr = (rho * Va * S) / (2 * MAV.mass * np.cos(beta))
a_beta1 = -beta_dr * MAV.C_Y_beta
a_beta2 = beta_dr * MAV.C_Y_delta_r
T_beta_delta_r = TF([a_beta2], [1, a_beta1])
theta_de = rho * Va**2 * c * S / (2 * Jy)
a_theta1 = -theta_de * MAV.C_m_q * c / (2 * Va)
a_theta2 = -theta_de * MAV.C_m_alpha
a_theta3 = theta_de * MAV.C_m_delta_e
T_theta_delta_e = TF([a_theta3], [1, a_theta1, a_theta2])
T_h_theta = TF([Va], [1, 0])
_, theta, _ = Quaternion2Euler(trim_state[6:10])
T_h_Va = TF([theta], [1, 0])
C_Ds = MAV.C_D_0 + MAV.C_D_alpha * alpha + MAV.C_D_delta_e * trim_input[0]
a_V1 = ((rho * Va * S * C_Ds) - dT_dVa(mav, Va, trim_input[3])) / mass
a_V2 = dT_ddelta_t(mav, Va, trim_input[3]) / mass
a_V3 = g * np.cos(theta - alpha)
T_Va_delta_t = TF([a_V2], [1, a_V1])
T_Va_theta = TF([-a_V3], [1, a_V1])
with open("trim.pkl", 'wb') as f:
vals = [
trim_state, trim_input, a_phi_1, a_phi_2, a_beta1, a_beta2,
a_theta1, a_theta2, a_theta3
]
pkl.dump(vals, f)
data = []
with open("trim.pkl", 'rb') as f:
data = pkl.load(f)
return T_phi_delta_a, T_chi_phi, T_theta_delta_e, T_h_theta, T_h_Va, T_Va_delta_t, T_Va_theta, T_beta_delta_r
def _update_true_state(self):
phi, theta, psi = Quaternion2Euler(self._state[6:10])
self.true_state.pn = self._state[0]
self.true_state.pe = self._state[1]
self.true_state.h = -self._state[2]
self.true_state.Va = self._Va
self.true_state.alpha = self._alpha
self.true_state.beta = self._beta
self.true_state.phi = phi
self.true_state.theta = theta
self.true_state.psi = psi
self.true_state.Vg = np.linalg.norm(self._state[3:6])
self.true_state.gamma, self.true_state.chi = self.calc_gamma_chi()
self.true_state.p = self._state[10]
self.true_state.q = self._state[11]
self.true_state.r = self._state[12]
self.true_state.wn = self._wind[0]
self.true_state.we = self._wind[1]
def compute_tf_model(mav, trim_state, trim_input):
# trim values
mav._state = trim_state
mav._update_velocity_data()
Va_trim = mav._Va
alpha_trim = mav._alpha
phi, theta_trim, psi = Quaternion2Euler(trim_state[6:10])
# define transfer function constants
a_phi1 = NotImplemented
a_phi2 = NotImplemented
a_theta1 = NotImplemented
a_theta2 = NotImplemented
a_theta3 = NotImplemented
a_V1 = NotImplemented
a_V2 = NotImplemented
a_V3 = NotImplemented
return Va_trim, alpha_trim, theta_trim, a_phi1, a_phi2, a_theta1, a_theta2, a_theta3, a_V1, a_V2, a_V3
def _update_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])
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 = math.sqrt((self._state[3][0])**2 +
(self._state[4][0])**2 +
(self._state[5][0])**2)
e0 = self._state[6][0]
e1 = self._state[7][0]
e2 = self._state[8][0]
e3 = self._state[9][0]
u = self._state[3][0]
v = self._state[4][0]
w = self._state[5][0]
pn_dot = (e1**2 + e0**2 - e2**2 - e3**2) * u + 2 * (
e1 * e2 - e3 * e0) * v + 2 * (e1 * e3 + e2 * e0) * w
pe_dot = 2 * (e1 * e2 + e3 * e0) * u + (
e2**2 + e0**2 - e1**2 - e3**2) * v + 2 * (e2 * e3 - e1 * e0) * w
pd_dot = 2 * (e1 * e3 - e2 * e0) * u + 2 * (e2 * e3 + e1 * e0) * v + (
e3**2 + e0**2 - e1**2 - e2**2) * w
# pn_dot, pe_dot, vg_k = Body2Inertia(phi, theta, psi, [self.state[3][0], self.state[4][0], self.state[5][0]] )
self.msg_true_state.gamma = math.asin(
-pe_dot / math.sqrt(pn_dot**2 + pe_dot**2 + pd_dot**2))
self.msg_true_state.chi = math.atan(pe_dot / pn_dot)
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 _update_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])
self.true_state.pn = self._state[0]
self.true_state.pe = self._state[1]
self.true_state.h = -self._state[2]
self.true_state.Va = self._Va
self.true_state.alpha = self._alpha
self.true_state.beta = self._beta
self.true_state.phi = phi
self.true_state.theta = theta
self.true_state.psi = psi
self.true_state.Vg = np.linalg.norm(self._state[3:6])
self.true_state.gamma = np.arctan2(-self._state[5], self._state[3])
self.true_state.chi = np.arctan2(self._state[4], self._state[3]) + psi
self.true_state.p = self._state[10]
self.true_state.q = self._state[11]
self.true_state.r = self._state[12]
self.true_state.wn = self._wind[0]
self.true_state.we = self._wind[1]
def _forces_moments(self, delta):
"""
: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)
"""
# longitudinal coefficients
C_L_0 = MAV.C_L_0
C_L_alpha = MAV.C_L_alpha
C_L_q = MAV.C_L_q
C_L_delta_e = MAV.C_L_delta_e
C_D_0 = MAV.C_D_0
C_D_alpha = MAV.C_D_alpha
C_D_p = MAV.C_D_p
C_D_q = MAV.C_D_q
C_D_delta_e = MAV.C_D_delta_e
C_m_0 = MAV.C_m_0
C_m_alpha = MAV.C_m_alpha
C_m_q = MAV.C_m_q
C_m_delta_e = MAV.C_m_delta_e
C_prop = MAV.C_prop
M = MAV.M
alpha0 = MAV.alpha0
epsilon = MAV.epsilon
# lateral coefficients
C_Y_0 = MAV.C_Y_0
C_Y_beta = MAV.C_Y_beta
C_Y_p = MAV.C_Y_p
C_Y_r = MAV.C_Y_r
C_Y_delta_a = MAV.C_Y_delta_a
C_Y_delta_r = MAV.C_Y_delta_r
C_ell_0 = MAV.C_ell_0
C_ell_beta = MAV.C_ell_beta
C_ell_p = MAV.C_ell_p
C_ell_r = MAV.C_ell_r
C_ell_delta_a = MAV.C_ell_delta_a
C_ell_delta_r = MAV.C_ell_delta_r
C_n_0 = MAV.C_n_0
C_n_beta = MAV.C_n_beta
C_n_p = MAV.C_n_p
C_n_r = MAV.C_n_r
C_n_delta_a = MAV.C_n_delta_a
C_n_delta_r = MAV.C_n_delta_r
# common terms
al = self._alpha
beta = self._beta
Va = self._Va
s_al = np.sin(al)
c_al = np.cos(al)
[phi, th, psi] = Quaternion2Euler(self._state[6:10])
p = self._state[10][0]
q = self._state[11][0]
r = self._state[12][0]
delta_a = delta[0][0]
delta_e = delta[1][0]
delta_r = delta[2][0]
delta_t = delta[3][0]
# coefficients
sig_a = (1 + np.exp(-M *
(al - alpha0)) + np.exp(M * (al + alpha0))) / (
(1 + np.exp(-M * (al - alpha0))) *
(1 + np.exp(M * (al + alpha0))))
CL = (1 - sig_a) * (C_L_0 + C_L_alpha * al) + sig_a * (
2 * np.sign(al) * s_al**2 * c_al)
CD = C_D_p + (C_L_0 + C_L_alpha * al)**2 / (np.pi * MAV.e * MAV.AR)
# CL = C_L_0+C_L_alpha*al
# CD = C_D_0+C_D_alpha*al
Cx_a = -CD * c_al + CL * s_al
Cxq_a = -C_D_q * c_al + C_L_q * s_al
Cx_de_a = -C_D_delta_e * c_al + C_L_delta_e * s_al
Cz_a = -CD * s_al - CL * c_al
Czq_a = -C_D_q * s_al - C_L_q * c_al
Cz_de_a = -C_D_delta_e * s_al - C_L_delta_e * c_al
Tp, Qp = self._motor_thrust_torque(Va, delta_t)
# forces
fg = np.array([[-MAV.mass * MAV.gravity * np.sin(th)],
[MAV.mass * MAV.gravity * np.cos(th) * np.sin(phi)],
[MAV.mass * MAV.gravity * np.cos(th) * np.cos(phi)]])
if Va == 0.0:
fa = np.array([[0.0], [0.0], [0.0]])
else:
fa = 0.5 * MAV.rho * Va**2 * MAV.S_wing * np.array([
[Cx_a + Cxq_a * MAV.c / (2.0 * Va) * q + Cx_de_a * delta_e],
[
C_Y_0 + C_Y_beta * beta + C_Y_p * MAV.b /
(2.0 * Va) * p + C_Y_r * MAV.b / (2.0 * Va) * r +
C_Y_delta_a * delta_a + C_Y_delta_r * delta_r
], [Cz_a + Czq_a * MAV.c / (2.0 * Va) * q + Cz_de_a * delta_e]
])
fp = np.array([[Tp], [0.0], [0.0]])
if Va == 0.0:
Ma = np.array([[0.0], [0.0], [0.0]])
else:
Ma = 0.5 * MAV.rho * Va**2 * MAV.S_wing * np.array(
[[
MAV.b * (C_ell_0 + C_ell_beta * beta + C_ell_p * MAV.b /
(2 * Va) * p + C_ell_r * MAV.b / (2.0 * Va) * r +
C_ell_delta_a * delta_a + C_ell_delta_r * delta_r)
],
[
MAV.c * (C_m_0 + C_m_alpha * al + C_m_q * MAV.c /
(2.0 * Va) * q + C_m_delta_e * delta_e)
],
[
MAV.b * (C_n_0 + C_n_beta * beta + C_n_p * MAV.b /
(2.0 * Va) * p + C_n_r * MAV.b / (2.0 * Va) * r +
C_n_delta_a * delta_a + C_n_delta_r * delta_r)
]])
Mt = np.array([[Qp], [0.0], [0.0]])
fx = fg[0][0] + fa[0][0] + fp[0][0]
fy = fg[1][0] + fa[1][0] + fp[1][0]
fz = fg[2][0] + fa[2][0] + fp[2][0]
Mx = Ma[0][0] + Mt[0][0]
My = Ma[1][0] + Mt[1][0]
Mz = Ma[2][0] + Mt[2][0]
# fx = 0.0
# fy = 0.0
# fz = 0.0
# Mx = 0.0
# My = 0.0
# Mz = 0.0
self._forces[0] = fx
self._forces[1] = fy
self._forces[2] = fz
return np.array([[fx, fy, fz, Mx, My, Mz]]).T
def update_sensors(self, delta):
"Return value of sensors on MAV: gyros, accels, static_pressure, dynamic_pressure, GPS"
# Rate Gyros
p = self._state.item(10)
q = self._state.item(11)
r = self._state.item(12)
self.sensors.gyro_x = p + np.random.normal(
scale=SENSOR.gyro_sigma) + SENSOR.gyro_x_bias
self.sensors.gyro_y = q + np.random.normal(
scale=SENSOR.gyro_sigma) + SENSOR.gyro_y_bias
self.sensors.gyro_z = r + np.random.normal(
scale=SENSOR.gyro_sigma) + SENSOR.gyro_z_bias
# Accelerometers
forces = self._forces_moments(delta)
phi, theta, psi = Quaternion2Euler([
self._state.item(6),
self._state.item(7),
self._state.item(8),
self._state.item(9)
])
m, g = MAV.mass, MAV.gravity
self.sensors.accel_x = forces.item(0) / m + g * np.sin(
theta) + np.random.normal(scale=SENSOR.accel_sigma)
self.sensors.accel_y = forces.item(1) / m - g * np.cos(theta) * np.sin(
phi) + np.random.normal(scale=SENSOR.accel_sigma)
self.sensors.accel_z = forces.item(2) / m - g * np.cos(theta) * np.cos(
phi) + np.random.normal(scale=SENSOR.accel_sigma)
# Static Pressure Sensor
h_ASL = -self._state.item(2)
self.sensors.static_pressure = MAV.rho * g * h_ASL + np.random.normal(
scale=SENSOR.static_pres_sigma)
# Differencial Presssure Sensor
self.sensors.diff_pressure = 0.5 * MAV.rho * (
self._Va)**2 + np.random.normal(scale=SENSOR.diff_pres_sigma)
if self._t_gps >= SENSOR.ts_gps:
self._gps_eta_n = np.exp(-SENSOR.gps_k * SENSOR.ts_gps
) * self._gps_eta_n + np.random.normal(
scale=SENSOR.gps_n_sigma)
self._gps_eta_e = np.exp(-SENSOR.gps_k * SENSOR.ts_gps
) * self._gps_eta_e + np.random.normal(
scale=SENSOR.gps_e_sigma)
self._gps_eta_h = np.exp(-SENSOR.gps_k * SENSOR.ts_gps
) * self._gps_eta_h + np.random.normal(
scale=SENSOR.gps_h_sigma)
self.sensors.gps_n = self._state.item(0) + self._gps_eta_n
self.sensors.gps_e = self._state.item(1) + self._gps_eta_e
self.sensors.gps_h = -self._state.item(2) + self._gps_eta_h
# V_a and course measurements
Va = self._Va
w_n, w_e, w_d = self._wind.item(0), self._wind.item(
1), self._wind.item(2)
self.sensors.gps_Vg = np.sqrt(
(Va * np.cos(psi) + w_n)**2 +
(Va * np.sin(psi) + w_e)**2) + np.random.normal(
scale=SENSOR.gps_Vg_sigma)
self.sensors.gps_course = np.arctan2(
Va * np.sin(psi) + w_e,
Va * np.cos(psi) +
w_n) + np.random.normal(scale=SENSOR.gps_course_sigma)
self._t_gps = 0.
else:
self._t_gps += self._ts_simulation
elev_doublet = sigs.signals(amplitude=0.2,
frequency=1.0,
start_time=1,
duration=1,
dc_offset=chap5.u_trim[0])
for ii in range(n):
delta_e = elev_doublet.doublet(dt * ii)
delta = np.array(
[delta_e, chap5.u_trim[1], chap5.u_trim[2], chap5.u_trim[3]])
wind = np.array([[0.], [0.], [0.], [0.], [0.], [0.]])
uav.update(delta, wind)
alpha.append(uav._alpha * 180 / np.pi)
beta.append(uav._beta * 180 / np.pi)
t_history.append(ii * dt)
phi_t, theta_t, psi_t = Quaternion2Euler(uav._state[6:10])
phi.append(phi_t * 180 / np.pi)
theta.append(theta_t * 180 / np.pi)
psi.append(psi_t * 180 / np.pi)
gamma.append(uav.true_state.gamma * 180 / np.pi)
# close all figures
print(uav._state)
plt.close('all')
plt.figure()
plt.plot(t_history, alpha, 'r', label='aoa')
plt.xlabel('t')
plt.ylabel('angle [deg]')
plt.legend()
def _forces_moments(self, delta):
"""
return the forces on the UAV based on the state, wind, and control surfaces
:param delta: np.matrix(delta_e, delta_a, delta_r, delta_t)
:return: Forces and Moments on the UAV np.matrix(Fx, Fy, Fz, Ml, Mn, Mm)
"""
phi, theta, psi = Quaternion2Euler(self._state[6:10])
p = self._state.item(10)
q = self._state.item(11)
r = self._state.item(12)
delta_e = delta.item(0)
delta_a = delta.item(1)
delta_r = delta.item(2)
delta_t = delta.item(3)
# Gravitational Components of Force, Moments = 0
mg = MAV.mass*MAV.gravity
fx_grav = -mg*np.sin(theta)
fy_grav = mg* np.cos(theta) * np.sin(phi)
fz_grav = mg* np.cos(theta) * np.cos(phi)
# Thrust Components of Force and Moments
fx_thrust,Mx_thrust = self.thrust_from_prop(delta_t)
fy_thrust = 0
fz_thrust = 0
My_thrust = 0
Mz_thrust = 0
# Aerodynamic Components of Forces and Moments
b = MAV.b
cyp = MAV.C_Y_p
cyr = MAV.C_Y_r
cydeltaa = MAV.C_Y_delta_a
cydeltar = MAV.C_Y_delta_r
aero_coef = 0.5*MAV.rho*self._Va**2*MAV.S_wing
fx_aero = aero_coef * (self.Cx(self._alpha) + self.Cx_q(self._alpha)*MAV.c/(2*self._Va)*q + self.Cx_deltae(self._alpha)*delta_e)
fy_aero = aero_coef * (MAV.C_Y_0 + MAV.C_Y_beta*self._beta + MAV.C_Y_p*b/(2*self._Va)*p + cyr * b/(2*self._Va)*r + cydeltaa * delta_a + cydeltar* delta_r)
fz_aero = aero_coef * (self.Cz(self._alpha) + self.Cz_q(self._alpha)*MAV.c/(2*self._Va)*q + self.Cz_deltae(self._alpha)*delta_e)
Mx_aero = aero_coef * MAV.b * (MAV.C_ell_0 + MAV.C_ell_beta*self._beta + MAV.C_ell_p*b/(2*self._Va)*p + MAV.C_ell_r*b/(2*self._Va)*r + MAV.C_ell_delta_a*delta_a + MAV.C_ell_delta_r*delta_r)
My_aero = aero_coef * MAV.c * (MAV.C_m_0 + MAV.C_m_alpha*self._alpha + MAV.C_m_q*MAV.c/(2*self._Va)*q + MAV.C_m_delta_e*delta_e)
Mz_aero = aero_coef * MAV.b * (MAV.C_n_0 + MAV.C_n_beta*self._beta + MAV.C_n_p*MAV.b/(2*self._Va)*p + MAV.C_n_r*MAV.b/(2*self._Va)*r + MAV.C_n_delta_a*delta_a + MAV.C_n_delta_r*delta_r)
fx = fx_grav + fx_aero + fx_thrust
fy = fy_grav + fy_aero + fy_thrust
fz = fz_grav + fz_aero + fz_thrust
# print('fx = ',fx)
# print('fy = ',fy)
# print('fz = ',fz)
Mx = Mx_aero + Mx_thrust
My = My_aero + My_thrust
Mz = Mz_aero + Mz_thrust
# print('Mx = ',Mx)
# print('My = ',My)
# print('Mz = ',Mz)
self._forces[0] = fx
self._forces[1] = fy
self._forces[2] = fz
fm = np.reshape(np.array([fx, fy, fz, Mx, My, Mz]),[6,1])
return fm
def _forces_moments(self, delta):
"""
return the forces on the UAV based on the state, wind, and control surfaces
:param delta: message with attributes [aileron, elevator, rudder, and throttle]
:return: Forces and Moments on the UAV np.matrix(Fx, Fy, Fz, Ml, Mn, Mm)
"""
phi, theta, psi = Quaternion2Euler(self._state[6:10])
e0 = self._state.item(6)
e1 = self._state.item(7)
e2 = self._state.item(8)
e3 = self._state.item(9)
p = self._state.item(10)
q = self._state.item(11)
r = self._state.item(12)
delta_a = delta.aileron
delta_e = delta.elevator
delta_r = delta.rudder
delta_t = delta.throttle
# compute gravitaional forces
f_g = MAV.mass * MAV.gravity * np.array([[
2 * (e1 * e3 - e2 * e0), 2 *
(e2 * e3 + e1 * e0), e3**2 + e0**2 - e1**2 - e2**2
]])
# compute Lift and Drag coefficients
sigma = (1. + np.exp(-MAV.M * (self._alpha - MAV.alpha0)) +
np.exp(MAV.M * (self._alpha + MAV.alpha0))) / (
(1 + np.exp(-MAV.M * (self._alpha - MAV.alpha0))) *
(1. + np.exp(MAV.M * (self._alpha + MAV.alpha0))))
sa = np.sin(self._alpha)
ca = np.cos(self._alpha)
CL = (1. - sigma) * (MAV.C_L_0 +
MAV.C_L_alpha * self._alpha) + sigma * (
2 * np.sign(self._alpha) * sa**2 * ca)
CD = MAV.C_D_p + (MAV.C_L_0 + MAV.C_L_alpha * self._alpha)**2 / (
np.pi * MAV.e * MAV.AR)
# CL = MAV.C_L_0 + MAV.C_L_alpha*self._alpha
# CD = MAV.C_D_0 + MAV.C_D_alpha*self._alpha
# compute Lift and Drag Forces
scalar = 0.5 * MAV.rho * (self._Va**2) * MAV.S_wing
F_lift = scalar * (CL + MAV.C_L_q * MAV.c * q /
(2 * self._Va) + MAV.C_L_delta_e * delta_e)
F_drag = scalar * (CD + MAV.C_D_q * MAV.c * q /
(2 * self._Va) + MAV.C_D_delta_e * delta_e)
# compute propeller thrust and torque
thrust_prop, torque_prop = self._motor_thrust_torque(self._Va, delta_t)
# # rotate lift and drag to body frame
# Cx = -CD*ca + CL*sa
# Cxq = -MAV.C_D_q*ca + MAV.C_L_q*sa
# Cxde = -MAV.C_D_delta_e*ca + MAV.C_L_delta_e*sa
# Cz = -CD*sa - CL*ca
# Czq = -MAV.C_D_q*sa - MAV.C_L_q*ca
# Czde = -MAV.C_D_delta_e*sa - MAV.C_L_delta_e
# compute longitudinal forces in body frame
fx = -F_drag * ca + F_lift * sa + thrust_prop + f_g[0, 0]
fz = -F_drag * sa - F_lift * ca + f_g[0, 2]
# compute lateral forces in body frame
b2Va = MAV.b / (2 * self._Va)
fy = f_g[0, 1] + scalar * (
MAV.C_Y_0 + MAV.C_Y_beta * self._beta + MAV.C_Y_p * p * b2Va +
MAV.C_Y_r * b2Va * r + MAV.C_Y_delta_a * delta_a +
MAV.C_Y_delta_r * delta_r)
# compute logitudinal torque in body frame
My = scalar * MAV.c * (MAV.C_m_0 + MAV.C_m_alpha * self._alpha +
MAV.C_m_q * MAV.c * q /
(2 * self._Va) + MAV.C_m_delta_e * delta_e)
# compute lateral torques in body frame
Mx = scalar * MAV.b * (MAV.C_ell_0 + MAV.C_ell_beta * self._beta +
MAV.C_ell_p * b2Va * p + MAV.C_ell_r * b2Va * r
+ MAV.C_ell_delta_a * delta_a +
MAV.C_ell_delta_r * delta_r) + torque_prop
Mz = scalar * MAV.b * (MAV.C_n_0 + MAV.C_n_beta * self._beta +
MAV.C_n_p * b2Va * p + MAV.C_n_r * b2Va * r +
MAV.C_n_delta_a * delta_a +
MAV.C_n_delta_r * delta_r)
# print(thrust_prop, "\t", fx)
# from IPython import embed; embed()
self._forces[0] = fx
self._forces[1] = fy
self._forces[2] = fz
return np.array([[fx, fy, fz, Mx, My, Mz]]).T
pd0 = -100.0 # initial down position u0 = 24.971443 # initial velocity along body x-axis v0 = 0.0 # initial velocity along body y-axis w0 = 1.194576 # initial velocity along body z-axis e = np.array([0.993827, 0.000000, 0.110938, 0.000000]) 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 e0 = e.item(0) e1 = e.item(1) e2 = e.item(2) e3 = e.item(3) phi0, theta0, psi0 = Quaternion2Euler(e) ###################################################################################### # Physical Parameters ###################################################################################### mass = 11. #kg Jx = 0.8244 #kg m^2 Jy = 1.135 Jz = 1.759 # Jxz = 0.1204 Jxz = 0.0 J = np.matrix([[Jx, 0, -Jxz], [0, Jy, 0], [-Jxz, 0, Jz]]) S_wing = 0.55 b = 2.8956 c = 0.18994 S_prop = 0.2027
def update_sensors(self):
"Return value of sensors on MAV: gyros, accels, static_pressure, dynamic_pressure, GPS"
Fx = self._forces[0]
Fy = self._forces[1]
Fz = self._forces[2]
m = MAV.mass
g = MAV.gravity
e = self._state[6:10]
phi, th, psi = Quaternion2Euler(e)
p, q, r = self._state[10:13]
rho = MAV.rho
h_AGL = -self._state[2]
Va = self._Va
K_gps = SENSOR.K_gps
Ts = SENSOR.ts_gps
pn = self._state[0]
pe = self._state[1]
ph = -self._state[2]
Vg_n = self.Vg_i[0]
Vg_e = self.Vg_i[1]
n_gyro_x = np.random.normal(0.0, SENSOR.gyro_sigma)
n_gyro_y = np.random.normal(0.0, SENSOR.gyro_sigma)
n_gyro_z = np.random.normal(0.0, SENSOR.gyro_sigma)
n_accel_x = np.random.normal(0.0, SENSOR.accel_sigma)
n_accel_y = np.random.normal(0.0, SENSOR.accel_sigma)
n_accel_z = np.random.normal(0.0, SENSOR.accel_sigma)
n_abs_pres = np.random.normal(0.0, SENSOR.static_pres_sigma)
n_dif_pres = np.random.normal(0.0, SENSOR.diff_pres_sigma)
n_gps_n = np.random.normal(0.0, SENSOR.gps_n_sigma)
n_gps_e = np.random.normal(0.0, SENSOR.gps_e_sigma)
n_gps_h = np.random.normal(0.0, SENSOR.gps_h_sigma)
n_gps_v = np.random.normal(0.0, SENSOR.gps_Vg_sigma)
n_gps_chi = np.random.normal(0.0, SENSOR.gps_course_sigma)
B_gyro_x = SENSOR.gyro_x_bias
B_gyro_y = SENSOR.gyro_y_bias
B_gyro_z = SENSOR.gyro_z_bias
B_abs_pres = SENSOR.static_pres_beta
B_dif_pres = SENSOR.diff_pres_beta
self._sensors.gyro_x = (p + B_gyro_x + n_gyro_x)[0]
self._sensors.gyro_y = (q + B_gyro_y + n_gyro_y)[0]
self._sensors.gyro_z = (r + B_gyro_z + n_gyro_z)[0]
self._sensors.accel_x = (Fx / m + g * np.sin(th) + n_accel_x)[0]
self._sensors.accel_y = (Fy / m - g * np.cos(th) * np.sin(phi) +
n_accel_y)[0]
self._sensors.accel_z = (Fz / m - g * np.cos(th) * np.cos(phi) +
n_accel_z)[0]
self._sensors.static_pressure = (rho * g * h_AGL + B_abs_pres +
n_abs_pres)[0]
self._sensors.diff_pressure = rho * Va**2 / 2.0 + B_dif_pres + n_dif_pres
if self._t_gps >= Ts:
self._gps_eta_n = math.exp(
-K_gps * self._t_gps) * self._gps_eta_n + n_gps_n
self._gps_eta_e = math.exp(
-K_gps * self._t_gps) * self._gps_eta_n + n_gps_e
self._gps_eta_h = math.exp(
-K_gps * self._t_gps) * self._gps_eta_n + n_gps_h
self._sensors.gps_n = (pn + self._gps_eta_n)[0]
self._sensors.gps_e = (pe + self._gps_eta_e)[0]
self._sensors.gps_h = (ph + self._gps_eta_h)[0]
self._sensors.gps_Vg = (np.sqrt(Vg_n**2 + Vg_e**2) + n_gps_v)[0]
self._sensors.gps_course = -math.atan2(Vg_e, Vg_n) + n_gps_chi
self._t_gps = 0.
else:
self._t_gps += self._ts_simulation
def sensors(self):
"Return value of sensors on MAV: gyros, accels, absolute_pressure, dynamic_pressure, GPS"
phi, theta, psi = Quaternion2Euler(self._state[6:10])
u = self._state.item(3)
v = self._state.item(4)
w = self._state.item(5)
uvw = np.array([[u, v, w]]).T
Rib = Quaternion2Rotation(self._state[6:10])
pdot = Rib @ uvw
p = self._state.item(10)
q = self._state.item(11)
r = self._state.item(12)
# simulate rate gyros(units are rad / sec)
self._sensors.gyro_x = p + np.random.normal(SENSOR.gyro_x_bias,
SENSOR.gyro_sigma)
self._sensors.gyro_y = q + np.random.normal(SENSOR.gyro_y_bias,
SENSOR.gyro_sigma)
self._sensors.gyro_z = r + np.random.normal(SENSOR.gyro_z_bias,
SENSOR.gyro_sigma)
# simulate accelerometers(units of g)
self._sensors.accel_x = self._forces[
0, 0] / MAV.mass + MAV.gravity * np.sin(theta) + np.random.normal(
0.0, SENSOR.accel_sigma)
self._sensors.accel_y = self._forces[
1, 0] / MAV.mass - MAV.gravity * np.cos(theta) * np.sin(
phi) + np.random.normal(0.0, SENSOR.accel_sigma)
self._sensors.accel_z = self._forces[
2, 0] / MAV.mass - MAV.gravity * np.cos(theta) * np.cos(
phi) + np.random.normal(0.0, SENSOR.accel_sigma)
# simulate magnetometers
# magnetic field in provo has magnetic declination of 12.5 degrees
# and magnetic inclination of 66 degrees
iota = np.radians(66)
delta = np.radians(12.5)
R_mag = Euler2Rotation(0.0, -iota, delta).T
# magnetic field in inertial frame: unit vector
mag_inertial = R_mag @ np.array([1, 0, 0])
R = Rib.T # body to inertial
# magnetic field in body frame: unit vector
mag_body = R @ mag_inertial
self._sensors.mag_x = mag_body[0] + np.random.normal(
SENSOR.mag_beta, SENSOR.mag_sigma)
self._sensors.mag_y = mag_body[0] + np.random.normal(
SENSOR.mag_beta, SENSOR.mag_sigma)
self._sensors.mag_z = mag_body[0] + np.random.normal(
SENSOR.mag_beta, SENSOR.mag_sigma)
# simulate pressure sensors
self._sensors.abs_pressure = MAV.rho * MAV.gravity * -self._state.item(
2) + np.random.normal(0, SENSOR.abs_pres_sigma)
self._sensors.diff_pressure = MAV.rho * self._Va**2 / 2 + np.random.normal(
0.0, SENSOR.diff_pres_sigma)
# simulate GPS sensor
if self._t_gps >= SENSOR.ts_gps:
self._gps_nu_n = self._gps_nu_n * np.exp(
-SENSOR.ts_gps * SENSOR.gps_k) + np.random.normal(
0.0, SENSOR.gps_n_sigma)
self._gps_nu_e = self._gps_nu_e * np.exp(
-SENSOR.ts_gps * SENSOR.gps_k) + np.random.normal(
0.0, SENSOR.gps_e_sigma)
self._gps_nu_h = self._gps_nu_h * np.exp(
-SENSOR.ts_gps * SENSOR.gps_k) + np.random.normal(
0.0, SENSOR.gps_h_sigma)
self._sensors.gps_n = self._state.item(0) + self._gps_nu_n
self._sensors.gps_e = self._state.item(1) + self._gps_nu_e
self._sensors.gps_h = -self._state.item(2) + self._gps_nu_h
self._sensors.gps_Vg = np.sqrt(
(self._Va * np.cos(psi) + self.true_state.wn)**2 +
(self._Va * np.sin(psi) + self.true_state.we)**2
) + np.random.normal(0, SENSOR.gps_Vg_sigma)
self._sensors.gps_course = np.arctan2(
self._Va * np.sin(psi) + self.true_state.we,
self._Va * np.cos(psi) +
self.true_state.we) + np.random.normal(0.0,
SENSOR.gps_course_sigma)
self._t_gps = 0.
else:
self._t_gps += self._ts_simulation
return self._sensors
