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)
def __init__(self, Ts):
self._ts_simulation = Ts
self._state = np.array([
MAV.pn0, # (0)
MAV.pe0, # (1)
MAV.pd0, # (2)
MAV.u0, # (3)
MAV.v0, # (4)
MAV.w0, # (5)
MAV.e0, # (6)
MAV.e1, # (7)
MAV.e2, # (8)
MAV.e3, # (9)
MAV.p0, # (10)
MAV.q0, # (11)
MAV.r0
]) # (12)
self.R_vb = Quaternion2Rotation(
self._state[6:10]) # Rotation body->vehicle
self.R_bv = np.copy(self.R_vb).T # vehicle->body
self._forces = np.zeros(3)
self._wind = np.zeros(3) # wind in NED frame in meters/sec
self._update_velocity_data()
self._Va = MAV.u0
self._alpha = 0
self._beta = 0
self.true_state = msg_state()
self.sensors = msg_sensors()
# random walk parameters for GPS
self._gps_eta_n = 0.
self._gps_eta_e = 0.
self._gps_eta_h = 0.
# timer so that gps only updates every ts_gps seconds
self._t_gps = 999. # large value ensures gps updates at initial time.
def __init__(self, Ts=0.02):
self._ts_simulation = Ts
# set initial states based on parameter file
# _state is the 13x1 internal state of the aircraft that is being propagated:
# _state = [pn, pe, pd, u, v, w, e0, e1, e2, e3, p, q, r]
# We will also need a variety of other elements that are functions of the _state and the wind.
# self.true_state is a 19x1 vector that is estimated and used by the autopilot to control the aircraft:
# true_state = [pn, pe, h, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi, gyro_bx, gyro_by, gyro_bz]
self._state = np.array([
MAV.pn0, # (0)
MAV.pe0, # (1)
MAV.pd0, # (2)
MAV.u0, # (3)
MAV.v0, # (4)
MAV.w0, # (5)
MAV.e0, # (6)
MAV.e1, # (7)
MAV.e2, # (8)
MAV.e3, # (9)
MAV.p0, # (10)
MAV.q0, # (11)
MAV.r0
]) # (12)
self.R_vb = Quaternion2Rotation(
self._state[6:10]) # Rotation body->vehicle
self.R_bv = np.copy(self.R_vb).T # vehicle->body
# store wind data for fast recall since it is used at various points in simulation
self._wind = np.zeros(3) # wind in NED frame in meters/sec
# store forces to avoid recalculation in the sensors function
self._update_velocity_data()
self._forces = np.zeros(3)
self._Va = MAV.u0
self._alpha = 0
self._beta = 0
# initialize true_state message
self.true_state = msg_state()
def _derivatives(self, x, u):
"""
for the dynamics xdot = f(x, u), returns fdot(x, u)
"""
# Get force, moment (torque)
f_b = u[:3]
m_b = u[3:]
# Get position, velocity, quaternion (rotation), angular velocity
r_i = x[:3] # wrt to i-frame
v_b = x[3:6] # wrt to i-frame
q_ib = x[6:10] # for rotation b to i-frame
w_b = x[10:] # wrt to b-frame
# Normalize quat. -> rotation
q_ib = q_ib / np.linalg.norm(q_ib) # normalize
R_ib = Quaternion2Rotation(q_ib)
# Compute equations of motion
# d/dt(r_i)
rdot_i = R_ib @ v_b
# d/dt(v_b)
vdot_b = (1 / MAV.mass) * f_b - skew(w_b) @ v_b
# d/dt(q_ib)
wq_ib = np.zeros((4, 1))
wq_ib[1:] = w_b
qdot_ib = 0.5 * quat_prod(wq_ib, q_ib)
wt_b = skew(w_b)
# d/dt(w_b)
wdot_b = np.linalg.inv(MAV.J) @ (m_b - (wt_b @ (MAV.J @ w_b)))
x_out = np.concatenate([rdot_i, vdot_b, qdot_ib,
np.array(wdot_b)],
axis=0)
return x_out
def _derivatives(self, state, forces_moments):
"""
for the dynamics xdot = f(x, u), returns f(x, u)
"""
# extract the states
pn = state[0]
pe = state[1]
pd = state[2]
u = state[3]
v = state[4]
w = state[5]
e0 = state[6]
e1 = state[7]
e2 = state[8]
e3 = state[9]
p = state[10]
q = state[11]
r = state[12]
# extract forces/moments
fx = forces_moments[0]
fy = forces_moments[1]
fz = forces_moments[2]
l = forces_moments[3]
m = forces_moments[4]
n = forces_moments[5]
self.R_vb = Quaternion2Rotation(np.array([e0, e1, e2,
e3])) # body->vehicle
# position kinematics
pn_dot, pe_dot, pd_dot = self.R_vb @ np.array([u, v, w])
# position dynamics
vec_pos = np.array([r * v - q * w, p * w - r * u, q * u - p * v])
u_dot, v_dot, w_dot = vec_pos + 1 / MAV.mass * np.array([fx, fy, fz])
# rotational kinematics
mat_rot = np.array([[0, -p, -q, -r], [p, 0, r, -q], [q, -r, 0, p],
[r, q, -p, 0]])
e0_dot, e1_dot, e2_dot, e3_dot = 0.5 * mat_rot @ np.array(
[e0, e1, e2, e3])
# rotatonal dynamics
G1 = MAV.gamma1
G2 = MAV.gamma2
G3 = MAV.gamma3
G4 = MAV.gamma4
G5 = MAV.gamma5
G6 = MAV.gamma6
G7 = MAV.gamma7
G8 = MAV.gamma8
vec_rot = np.array([
G1 * p * q - G2 * q * r, G5 * p * r - G6 * (p**2 - r**2),
G7 * p * q - G1 * q * r
])
vec_rot2 = np.array([G3 * l + G4 * n, m / MAV.Jy, G4 * l + G8 * n])
p_dot, q_dot, r_dot = vec_rot + vec_rot2
# 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
])
return x_dot
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)
"""
# Unpack necessary states
delta_a = delta[0].item(0)
delta_e = delta[1].item(0)
delta_r = delta[2].item(0)
delta_t = delta[3].item(0)
if delta_t < 0:
delta_t = 0
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)
M = MAV.M
alpha = self._alpha
alpha0 = MAV.alpha0
beta = self._beta
Va = self._Va
# Calculate fg
rot_mat_vel = Quaternion2Rotation(self._state[6:10]).T
fg = rot_mat_vel @ np.array([[0, 0, MAV.mass * MAV.gravity]]).T
# Calculate fp and mp, n (Propellor thrust)
# compute t h r u s t and torque due to p r o p ell e r ( See addendum by McLain)
# map d e l t a t t h r o t t l e command(0 t o 1) i n t o motor i n p u t v o l t a g e
V_in = MAV.V_max * delta_t
D_prop = MAV.D_prop
rho = MAV.rho
a_1 = MAV.rho * MAV.D_prop**5 / ((2.0 * np.pi)**2) * MAV.C_Q0
b_1 = MAV.rho * MAV.D_prop**4 / (2.0 * np.pi) * MAV.C_Q1 * self._Va + (
MAV.KQ**2) / MAV.R_motor
c_1 = MAV.rho * MAV.D_prop**3 * MAV.C_Q2 * self._Va**2 - MAV.KQ / MAV.R_motor * V_in + MAV.KQ * MAV.i0
# Consider only positive root
Omega_op = (-b_1 + np.sqrt(b_1**2 - 4 * a_1 * c_1)) / (2.0 * a_1)
# compute advance rat io
J_op = (2 * np.pi * self._Va) / (Omega_op * D_prop)
# compute nond imens ional ized c o e f f i c i e n t s of thrus t and torque
CT = MAV.C_T2 * J_op**2 + MAV.C_T1 * J_op + MAV.C_T0
CQ = MAV.C_Q2 * J_op**2 + MAV.C_Q1 * J_op + MAV.C_Q0
# add thrust and torque due to pr o peller
n_input = Omega_op / (2 * np.pi)
f_p = MAV.rho * n_input**2 * MAV.D_prop**4 * CT
m_p = -MAV.rho * n_input**2 * MAV.D_prop**5 * CQ
f_p = np.vstack((f_p, 0, 0))
m_p = np.vstack((m_p, 0, 0))
# Calculate fa
first = 1 / 2 * MAV.rho * self._Va**2 * MAV.S_wing
e_negative = np.exp(-M * (alpha - alpha0))
e_positive = np.exp(M * (alpha + alpha0))
sigma_a = (1 + e_negative + e_positive) / ((1 + e_negative) *
(1 + e_positive))
C_L_alpha_f = (1 - sigma_a) * (MAV.C_L_0 + MAV.C_L_alpha * alpha) + \
sigma_a * (2 * np.sign(alpha) * (np.sin(alpha)**2) * np.cos(alpha))
C_D_alpha_f = MAV.C_D_p + (MAV.C_L_0 + MAV.C_L_alpha * alpha)**2 / (
np.pi * MAV.e * MAV.AR)
C_X_alpha = -C_D_alpha_f * cos(alpha) + C_L_alpha_f * sin(alpha)
C_X_q_alpha = -MAV.C_D_q * cos(alpha) + MAV.C_L_q * sin(alpha)
C_X_delta_e = -MAV.C_D_delta_e * cos(alpha) + MAV.C_L_delta_e * sin(
alpha)
C_Z_alpha = -C_D_alpha_f * sin(alpha) - C_L_alpha_f * cos(alpha)
C_Z_q = -MAV.C_D_q * sin(alpha) - MAV.C_L_q * cos(alpha)
C_Z_delta_e = -MAV.C_D_delta_e * sin(alpha) - MAV.C_L_delta_e * cos(
alpha)
fx_a = C_X_alpha + C_X_q_alpha * MAV.c / (
2 * Va) * q + C_X_delta_e * delta_e
fy_a = MAV.C_Y_0 + MAV.C_Y_beta * beta + MAV.C_Y_p * MAV.b / (
2 * Va) * p + MAV.C_Y_r * MAV.b / (
2 *
Va) * r + MAV.C_Y_delta_a * delta_a + MAV.C_Y_delta_r * delta_r
fz_a = C_Z_alpha + C_Z_q * MAV.c / (2 * Va) * q + C_Z_delta_e * delta_e
fa = first * np.vstack((fx_a, fy_a, fz_a))
# Calculate moments
Mx = MAV.b * (MAV.C_ell_0 + MAV.C_ell_beta * beta +
MAV.C_ell_p * MAV.b /
(2 * Va) * p + MAV.C_ell_r * MAV.b /
(2 * Va) * r + MAV.C_ell_delta_a * delta_a +
MAV.C_ell_delta_r * delta_r)
My = MAV.c * (MAV.C_m_0 + MAV.C_m_alpha * alpha + MAV.C_m_q * MAV.c /
(2 * Va) * q + MAV.C_m_delta_e * delta_e)
Mz = MAV.b * (MAV.C_n_0 + MAV.C_n_beta * beta + MAV.C_n_p * MAV.b /
(2 * Va) * p + MAV.C_n_r * MAV.b / (2 * Va) * r +
MAV.C_n_delta_a * delta_a + MAV.C_n_delta_r * delta_r)
Mx = Mx * first
My = My * first
Mz = Mz * first
m_a = np.vstack((Mx, My, Mz))
all_moments = m_a + m_p
total_forces = fa + f_p + fg
fx = total_forces[0].item(0)
fy = total_forces[1].item(0)
fz = total_forces[2].item(0)
self._forces[0] = fx
self._forces[1] = fy
self._forces[2] = fz
Mx = all_moments[0].item(0)
My = all_moments[1].item(0)
Mz = all_moments[2].item(0)
return np.array([[fx, fy, fz, Mx, My, Mz]]).T
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