def dT_d_x_q(e):
eps = .01
dtheta_de = np.zeros((3, 4))
phi_at_e, theta_at_e, psi_at_e = Quaternion2Euler(e)
for i in range(0, 4):
e_eps = np.copy(e)
e_eps[i][0] += eps # add eps to the ith state
normE = np.sqrt(e_eps[0][0]**2 + e_eps[1][0]**2 + e_eps[2][0]**2 +
e_eps[3][0]**2)
e_eps[0][0] = e_eps[0][0] / normE
e_eps[1][0] = e_eps[1][0] / normE
e_eps[2][0] = e_eps[2][0] / normE
e_eps[3][0] = e_eps[3][0] / normE
phi_at_e_eps, theta_at_e_eps, psi_at_e_eps = Quaternion2Euler(e_eps)
d_theta_dxi = np.array([[(phi_at_e_eps - phi_at_e) / eps],
[(theta_at_e_eps - theta_at_e) / eps],
[(psi_at_e_eps - psi_at_e_eps) / eps]])
dtheta_de[:, i] = d_theta_dxi[:, 0]
eye3_3 = np.eye(3)
zero3_3 = np.zeros((3, 3))
zero3_4 = np.zeros((3, 4))
R1 = np.concatenate((eye3_3, zero3_3, zero3_4, zero3_3), 1)
R2 = np.concatenate((zero3_3, eye3_3, zero3_4, zero3_3), 1)
R3 = np.concatenate((zero3_3, zero3_3, dtheta_de, zero3_3), 1)
R4 = np.concatenate((zero3_3, zero3_3, zero3_4, eye3_3), 1)
dT_dxq = np.concatenate((R1, R2, R3, R4), 0)
return dT_dxq
def euler_state(x_quat):
# convert state x with attitude represented by quaternion
# to x_euler with attitude represented by Euler angles
phi, theta, psi = Quaternion2Euler(x_quat[6:10])
x_euler = np.array([[x_quat[0]], [x_quat[1]], [x_quat[2]], [x_quat[3]],
[x_quat[4]], [x_quat[5]], [phi], [theta], [psi],
[x_quat[10]], [x_quat[11]], [x_quat[12]]])
return x_euler
def _update_msg_true_state(self):
# update the true state message:
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.phi = phi
self.msg_true_state.theta = theta
self.msg_true_state.psi = psi
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)
def update_sensors(self):
"Return value of sensors on MAV: gyros, accels, static_pressure, dynamic_pressure, GPS"
phi, theta, psi = Quaternion2Euler(self._state[6:10])
self.sensors.gyro_x = self._state.item(
10) + SENSOR.gyro_x_bias + SENSOR.gyro_sigma * np.random.randn()
self.sensors.gyro_y = self._state.item(
11) + SENSOR.gyro_y_bias + SENSOR.gyro_sigma * np.random.randn()
self.sensors.gyro_z = self._state.item(
12) + SENSOR.gyro_z_bias + SENSOR.gyro_sigma * np.random.randn()
self.sensors.accel_x = self._forces[
0] / MAV.mass + MAV.gravity * np.sin(
theta) + SENSOR.accel_sigma * np.random.randn()
self.sensors.accel_y = self._forces[
1] / MAV.mass - MAV.gravity * np.cos(theta) * np.sin(
phi) + SENSOR.accel_sigma * np.random.randn()
self.sensors.accel_z = self._forces[
2] / MAV.mass - MAV.gravity * np.cos(theta) * np.cos(
phi) + SENSOR.accel_sigma * np.random.randn()
self.sensors.static_pressure = MAV.rho * MAV.gravity * -self._state.item(
2) + SENSOR.static_pres_sigma * np.random.randn()
self.sensors.diff_pressure = MAV.rho * self._Va**2 / 2. + SENSOR.diff_pres_sigma * np.random.randn(
)
# self.sensors.mag = Quaternion2Rotation(self._state[6:10]).T@Euler2Rotation(0,-66*np.pi/180.,12.5*np.pi/180.)@np.array([[1.],[0.],[0.]]) \
# + SENSOR.mag_beta + np.random.normal(0, SENSOR.mag_sigma, 1)
if self._t_gps >= SENSOR.ts_gps:
self._gps_eta_n = np.exp(
-SENSOR.gps_beta * SENSOR.ts_gps
) * self._gps_eta_n + SENSOR.gps_n_sigma * np.random.randn()
self._gps_eta_e = np.exp(
-SENSOR.gps_beta * SENSOR.ts_gps
) * self._gps_eta_e + SENSOR.gps_e_sigma * np.random.randn()
self._gps_eta_h = np.exp(
-SENSOR.gps_beta * SENSOR.ts_gps
) * self._gps_eta_h + SENSOR.gps_h_sigma * np.random.randn()
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
self.sensors.gps_Vg = np.sqrt(
(self._Va * np.cos(psi) + self._wind.item(0))**2 +
(self._Va * np.sin(psi) + self._wind.item(1))**2
) + SENSOR.gps_Vg_sigma * np.random.randn()
self.sensors.gps_course = np.arctan2(
self._Va * np.sin(psi) + self._wind.item(1),
self._Va * np.cos(psi) + self._wind.item(0)
) + SENSOR.gps_course_sigma * np.random.randn()
self._t_gps = 0.
else:
self._t_gps += self._ts_simulation
return self.sensors
def compute_ss_model(mav, trim_state, trim_input):
A_q = df_dx(mav, trim_state, trim_input)
B_q = df_du(mav, trim_state, trim_input)
dT_dxq = dT_d_x_q(trim_state[6:10])
phi, theta, psi = Quaternion2Euler(trim_state[6:10])
dT_inv_dxq = dT_inv_d_x_q(phi, theta, psi)
A_e = dT_dxq @ A_q @ dT_inv_dxq
B_e = dT_dxq @ B_q
A_lat = A_e[[[4], [9], [11], [6], [8]], [4, 9, 11, 6, 8]]
B_lat = B_e[[[4], [9], [11], [6], [8]], [0, 2]]
A_lon = A_e[[[3], [5], [10], [7], [2]], [3, 5, 10, 7, 2]]
B_lon = B_e[[[3], [5], [10], [7], [2]], [1, 2]]
A_lon[4, :] = A_lon[4, :] * -1.0
return A_lon, B_lon, A_lat, B_lat #B_lat completely right # 5,3 and 3,2 in A_lat wrong #A_lon close to right # B_lon right
def compute_tf_model(mav, trim_state, trim_input):
# trim values
phi, theta, psi = Quaternion2Euler(trim_state[6:10])
T_phi_delta_a_a1 = -.5 * MAV.rho * mav._Va**2 * MAV.S_wing * MAV.b * MAV.C_p_p * MAV.b / (
2. * mav._Va) #What is SB??
T_phi_delta_a_a2 = .5 * MAV.rho * mav._Va**2 * MAV.S_wing * MAV.b * MAV.C_p_delta_a
T_chi_phi = MAV.gravity / mav._Vg
T_theta_delta_e_a1 = (-MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing /
(2. * MAV.Jy)) * MAV.C_m_q * MAV.c / (2. * mav._Va)
T_theta_delta_e_a2 = (-MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing /
(2. * MAV.Jy)) * MAV.C_m_alpha
T_theta_delta_e_a3 = (MAV.rho * mav._Va**2 * MAV.c * MAV.S_wing /
(2. * MAV.Jy)) * MAV.C_m_delta_e
T_h_theta = mav._Va
T_h_Va = theta
T_Va_delta_t_a1 = (MAV.rho*mav._Va*MAV.S_wing/MAV.mass)*(MAV.C_D_0 + MAV.C_D_alpha*mav._alpha + MAV.C_D_delta_e*trim_input[1]) \
- dT_dVa(mav, mav._Va, trim_input[3])/MAV.mass
#Aren't they all about trim values?? What other Va use?
T_Va_delta_t_a2 = dT_ddelta_t(
mav, mav._Va, trim_input[3]
) / MAV.mass #(MAV.rho*MAV.S_wing/MAV.mass)*C_prop*k**2*trim_input[3] #Where is k???
T_Va_theta = MAV.gravity * np.cos(theta - mav._alpha)
T_beta_delta_r_a1 = -MAV.rho * mav._Va * MAV.S_wing * MAV.C_Y_beta / (
2. * MAV.mass)
T_beta_delta_r_a2 = -MAV.rho * mav._Va * MAV.S_wing * MAV.C_Y_delta_r / (
2. * MAV.mass)
f = open('../chap5/tf_coefficients.py', 'w')
f.write('T_phi_delta_a_a1 = ' + str(T_phi_delta_a_a1) + '\n')
f.write('T_phi_delta_a_a2 = ' + str(T_phi_delta_a_a2) + '\n')
f.write('T_chi_phi = ' + str(T_chi_phi) + '\n')
f.write('T_theta_delta_e_a1 = ' + str(T_theta_delta_e_a1) + '\n')
f.write('T_theta_delta_e_a2 = ' + str(T_theta_delta_e_a2) + '\n')
f.write('T_theta_delta_e_a3 = ' + str(T_theta_delta_e_a3) + '\n')
f.write('T_h_theta = ' + str(T_h_theta) + '\n')
f.write('T_h_Va = ' + str(T_h_Va) + '\n')
f.write('T_Va_delta_t_a1 = ' + str(T_Va_delta_t_a1) + '\n')
f.write('T_Va_delta_t_a2 = ' + str(T_Va_delta_t_a2) + '\n')
f.write('T_Va_theta = ' + str(T_Va_theta) + '\n')
f.write('T_beta_delta_r_a1 = ' + str(T_beta_delta_r_a1) + '\n')
f.write('T_beta_delta_r_a2 = ' + str(T_beta_delta_r_a2) + '\n')
f.close()
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
self.msg_true_state.Vg = self._Vg
self.msg_true_state.gamma = self._gamma
self.msg_true_state.chi = self._chi
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 _derivatives(state, delta, gamma, Va):
# 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)
phi, theta, psi = Quaternion2Euler(state[6:10])
p = state.item(10)
q = state.item(11)
r = state.item(12)
alpha = theta - gamma
# Forces due to gravity
# phi, theta, psi = Quaternion2Euler(self._state[6:10])
# fx = -MAV.mass*MAV.gravity * np.sin(theta)
# fy = MAV.mass*MAV.gravity*np.cos(theta)*np.sin(phi)
# fz = MAV.mass*MAV.gravity*np.cos(theta)*np.cos(phi)
fx = MAV.mass * MAV.gravity * 2 * (e1 * e3 - e2 * e0)
fy = MAV.mass * MAV.gravity * 2 * (e2 * e3 + e1 * e0)
fz = MAV.mass * MAV.gravity * (e3**2 + e0**2 - e1**2 - e2**2)
# Forces due to aerodynamics
cA = np.cos(alpha)
sA = np.sin(alpha)
Cd = MAV.C_D_p + (MAV.C_L_0 +
MAV.C_L_alpha * alpha)**2 / (np.pi * np.exp(1) * MAV.AR)
sigmaA = (1 + np.exp(-MAV.M * (alpha - MAV.alpha0)) + np.exp(MAV.M * (alpha + MAV.alpha0))) \
/ ((1 + np.exp(-MAV.M * (alpha - MAV.alpha0))) * (1 + np.exp(MAV.M * (alpha + MAV.alpha0))))
Cl = (1 - sigmaA) * (MAV.C_L_0 + MAV.C_L_alpha * alpha) + sigmaA * (
2 * np.sign(alpha) * sA**2 * cA)
Cx = -Cd * cA + Cl * sA
Cxq = -MAV.C_D_q * cA + MAV.C_L_q * sA
Cxdeltae = -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
Czdeltae = -MAV.C_D_delta_e * sA - MAV.C_L_delta_e * cA
fx += .5 * MAV.rho * Va**2 * MAV.S_wing * (Cx + Cxq * MAV.c * q /
(2 * Va) + Cxdeltae * delta[1])
fy += .5 * MAV.rho * Va ** 2 * MAV.S_wing * (
MAV.C_Y_0 + MAV.C_Y_beta * 0. + MAV.C_Y_p * MAV.b * p / (2 * Va) \
+ MAV.C_Y_r * MAV.b * r / (2 * Va) + MAV.C_Y_delta_a * delta[0] + MAV.C_Y_delta_r *
delta[2])
fz += .5 * MAV.rho * Va**2 * MAV.S_wing * (Cz + Czq * MAV.c * q /
(2 * Va) + Czdeltae * delta[1])
l = .5 * MAV.rho * Va ** 2 * MAV.S_wing * (MAV.b * (
MAV.C_ell_0 + MAV.C_ell_beta * 0. + MAV.C_ell_p * MAV.b * p / (2 * Va) \
+ MAV.C_ell_r * MAV.b * r / (2 * Va) + MAV.C_ell_delta_a * delta[
0] + MAV.C_ell_delta_r * delta[2]))
m = .5 * MAV.rho * Va**2 * MAV.S_wing * (
MAV.c * (MAV.C_m_0 + MAV.C_m_alpha * alpha + MAV.C_m_q * MAV.c * q /
(2 * Va) + MAV.C_m_delta_e * delta[1]))
n = .5 * MAV.rho * Va ** 2 * MAV.S_wing * (
MAV.b * (MAV.C_n_0 + MAV.C_n_beta * 0. + MAV.C_n_p * MAV.b * p / (2 * Va) \
+ MAV.C_n_r * MAV.b * r / (2 * Va) + MAV.C_n_delta_a * delta[
0] + MAV.C_n_delta_r * delta[2]))
# Compute thrust and torque due to propeller
# map delta_t throttle command (0 to 1) into motor input voltage
V_in = MAV.V_max * delta[3]
# Quadratic formula to solve for motor speed
a = MAV.C_Q0 * MAV.rho * np.power(MAV.D_prop, 5) \
/ ((2. * np.pi) ** 2)
b = (MAV.C_Q1 * MAV.rho * np.power(MAV.D_prop, 4) \
/ (2. * np.pi)) * Va + MAV.KQ ** 2 / MAV.R_motor
c = MAV.C_Q2 * MAV.rho * np.power(MAV.D_prop, 3) \
* Va ** 2 - (MAV.KQ / MAV.R_motor) * V_in + MAV.KQ * MAV.i0
# Consider only positive _rotate_points
Omega_op = (-b + np.sqrt(b**2. - 4. * a * c)) / (2. * a)
# compute advance ratio
J_op = 2. * np.pi * Va / (Omega_op * MAV.D_prop)
# compute non-dimensionalized coefficients of thrust and torque
C_T = MAV.C_T2 * J_op**2 + MAV.C_T1 * J_op + MAV.C_T0
C_Q = MAV.C_Q2 * J_op**2 + MAV.C_Q1 * J_op + MAV.C_Q0
# add thrust and torque due to propeller
n = Omega_op / (2 * np.pi)
fx += MAV.rho * n**2 * np.power(MAV.D_prop, 4) * C_T
l += -MAV.rho * n**2 * np.power(MAV.D_prop, 5) * C_Q
"""
for the dynamics xdot = f(x, u), returns f(x, u)
"""
# 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
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
# position dynamics
u_dot = r * v - q * w + fx / MAV.mass
v_dot = p * w - r * u + fy / MAV.mass
w_dot = q * u - p * v + fz / MAV.mass
# rotational kinematics
e0_dot = (-p * e1 - q * e2 - r * e3) / 2
e1_dot = (p * e0 + r * e2 - q * e3) / 2
e2_dot = (q * e0 - r * e1 + p * e3) / 2
e3_dot = (r * e0 + q * e1 - p * e2) / 2
# 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) + m / MAV.Jy
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