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_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 = np.sqrt((self._state.item(3))**2 +
(self._state.item(4))**2 +
(self._state.item(5))**2)
Vg = np.array(
[self._state.item(3),
self._state.item(4),
self._state.item(5)])
Vg_M = np.linalg.norm(Vg)
Vg_h = np.array([self._state.item(3), self._state.item(4), 0.])
Vg_h_M = np.linalg.norm(Vg_h)
Va_h = np.array([self._ur, self._vr, 0.])
Va_h_M = np.linalg.norm(Va_h)
self.msg_true_state.gamma = np.arccos(Vg.dot(Vg_h) / (Vg_M * Vg_h_M))
num = Vg_h.dot(Va_h)
den = (Vg_h_M * Va_h_M)
frac = np.round(num / den, 8)
self.msg_true_state.chi = psi + self._beta + np.arccos(frac)
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_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 = self.Euler2Rotation(phi, theta, psi)
Vg_b = self.Va_b + self._wind
Vg_i = R.T @ Vg_b
self.msg_true_state.Vg = np.linalg.norm(Vg_b)
self.msg_true_state.gamma = np.arctan2(
-Vg_i[2], np.sqrt(Vg_i[0]**2 + Vg_i[1]**2))
self.msg_true_state.chi = -np.arctan2(Vg_i[1], 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 compute_tf_model(
trim_state,
trim_input): #computes the transfer function linearized at trim
# trim values
mav = mav_dynamics(SIM.ts_simulation)
mav.set_state(trim_state)
mav._update_velocity_data()
Va_trim = mav._Va
alpha_trim = mav._alpha
beta_trim = mav._beta
phi, theta_trim, psi = Quaternion2Euler(trim_state[6:10])
delta_t_trim = trim_input[3, 0]
_dT_dVa = dT_dVa(Va_trim, delta_t_trim)
_dT_ddelta_t = dT_ddelta_t(Va_trim, delta_t_trim)
# define transfer function constants
a_phi1 = -0.25 * MAV.rho * mav._Va * MAV.S_wing * (MAV.b**2) * MAV.C_p_p
a_phi2 = .5 * MAV.rho * (mav._Va**2) * MAV.S_wing * MAV.b * MAV.C_p_delta_a
a_theta1 = -MAV.rho * mav._Va * (MAV.c**
2) * MAV.S_wing * MAV.C_m_q / (4 * MAV.Jy)
a_theta2 = -MAV.rho * (mav._Va**2) * MAV.c * MAV.S_wing * MAV.C_m_alpha / (
2 * MAV.Jy)
a_theta3 = MAV.rho * (
mav._Va**2) * MAV.c * MAV.S_wing * MAV.C_m_delta_e / (2 * MAV.Jy)
a_V1 = (MAV.rho * Va_trim * MAV.S_wing /
MAV.mass) * (MAV.C_D_0 + MAV.C_D_alpha * alpha_trim +
MAV.C_D_delta_e) - _dT_dVa / MAV.mass
a_V2 = _dT_ddelta_t / MAV.mass
a_V3 = MAV.gravity * np.cos(theta_trim - alpha_trim)
return Va_trim, alpha_trim, beta_trim, theta_trim, a_phi1, a_phi2, a_theta1, a_theta2, a_theta3, a_V1, a_V2, a_V3
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], self._state[7],
self._state[8], self._state[9])
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.Vs = self._Vs
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)
self.msg_true_state.e0 = self._state[6]
self.msg_true_state.e1 = self._state[7]
self.msg_true_state.e2 = self._state[8]
self.msg_true_state.e3 = self._state[9]
def _update_velocity_data(self, wind=np.zeros((6, 1))):
# self.wind = wind_simulation(SIM.ts_simulation) # how can I know there are six vector come here?
wn_s = wind.item(0)
we_s = wind.item(1)
wd_s = wind.item(2)
wind_ss = [wn_s, we_s, wd_s]
phi, theta, psi = Quaternion2Euler(self._state[6:10])
angles = phi, theta, psi
wind_ss_b = TransformationFormInertialToBody(wind_ss, angles)
wn_b = wind_ss_b[0]
we_b = wind_ss_b[1]
wd_b = wind_ss_b[2]
u_w = wn_b + wind.item(3)
v_w = we_b + wind.item(4)
w_w = wd_b + wind.item(5)
u = self._state.item(3)
v = self._state.item(4)
w = self._state.item(5)
u_r = u - u_w
v_r = v - v_w
w_r = w - w_w
# compute airspeed
self._Va = np.sqrt(u_r**2 + v_r**2 + w_r**2)
# compute angle of attack
self._alpha = np.arctan(w_r / u_r)
# compute sideslip angle
self._beta = np.arcsin(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 = self.Euler2Rotation(phi, th, psi) @ wind[0:3]
self._wind = 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] - self._wind[0][0]],
[self._state[4][0] - self._wind[1][0]],
[self._state[5][0] - self._wind[2][0]]])
self._Va = np.linalg.norm(self.Va_b)
Vg_b = self.Va_b + self._wind
self._Vg = np.linalg.norm(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 _update_velocity_data(self, wind=np.zeros((6, 1))):
e0 = self._state.item(6)
e1 = self._state.item(7)
e2 = self._state.item(8)
e3 = self._state.item(9)
phi, theta, psi = Quaternion2Euler(np.array([e0, e1, e2, e3]))
Rvb = RotationVehicle2Body(phi, theta, psi)
# calculate wind components
self._wind = wind[0:3]
wind_combined = Rvb @ wind[0:3] + wind[3:6]
# compute relative airspeed components
self._ur = self._state.item(3) - wind_combined.item(0)
self._vr = self._state.item(4) - wind_combined.item(1)
self._wr = self._state.item(5) - wind_combined.item(2)
self.Vg = Rvb.transpose() @ self._state[3:6] # In vehicle frame
self._Vg = np.linalg.norm(self.Vg)
# compute airspeed
self._Va = np.sqrt(self._ur**2 + self._vr**2 + self._wr**2)
# compute angle of attack
self._alpha = np.arctan2(self._wr, self._ur)
# compute sideslip angle
self._beta = np.arcsin(self._vr / self._Va)
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 = np.sqrt(
self.Vg.item(0)**2 + self.Vg.item(1)**2 + self.Vg.item(2)**2)
self.msg_true_state.gamma = np.arctan2(
-self.Vg[2], np.sqrt(self.Vg[0]**2 + self.Vg[1]**2))
self.msg_true_state.chi = np.arctan2(self.Vg[1], self.Vg[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)
self.msg_true_state.phi_g = 0.0
self.msg_true_state.theta_g = 0.0
self.msg_true_state.psi_g = 0.0
def set_state(self, state):
self._state = state
temp1, temp2, temp3 = Quaternion2Euler(self._state[6:10])
self.phi = temp1[0] # roll
self.theta = temp2[0] # pitch
self.psi = temp3[0] #yaw
self.h = -self._state[2, 0]
def update_sensors(self):
"Return value of sensors on MAV: gyros, accels, static_pressure, dynamic_pressure, GPS"
pn = self._state.item(0)
pe = self._state.item(1)
h = -self._state.item(2)
e0 = self._state.item(6)
e1 = self._state.item(7)
e2 = self._state.item(8)
e3 = self._state.item(9)
phi, theta, psi = Quaternion2Euler(np.array([e0, e1, e2, e3]))
p = self._state.item(10)
q = self._state.item(11)
r = self._state.item(12)
Va = self._Va
wn = self._wind.item(0)
we = self._wind.item(1)
self.sensors.gyro_x = p + SENSOR.gyro_sigma * np.random.randn()
self.sensors.gyro_y = q + SENSOR.gyro_sigma * np.random.randn()
self.sensors.gyro_z = r + SENSOR.gyro_sigma * np.random.randn()
self.sensors.accel_x = float(self._forces[0] / MAV.mass +
MAV.gravity * np.sin(theta) +
SENSOR.accel_sigma * np.random.randn())
self.sensors.accel_y = float(self._forces[1] / MAV.mass - MAV.gravity *
np.cos(theta) * np.sin(phi) +
SENSOR.accel_sigma * np.random.randn())
self.sensors.accel_z = float(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 * h + SENSOR.static_pres_sigma * np.random.randn(
)
self.sensors.diff_pressure = MAV.rho * Va**2 / 2. + SENSOR.diff_pres_sigma * np.random.randn(
)
if self._t_gps >= SENSOR.ts_gps:
kGPS = 1.0 / 16000.
self._gps_eta_n = np.exp(
-kGPS * SENSOR.ts_gps
) * self._gps_eta_n + SENSOR.gps_n_sigma * np.random.randn()
self._gps_eta_e = np.exp(
-kGPS * SENSOR.ts_gps
) * self._gps_eta_e + SENSOR.gps_e_sigma * np.random.randn()
self._gps_eta_h = np.exp(
-kGPS * SENSOR.ts_gps
) * self._gps_eta_h + SENSOR.gps_h_sigma * np.random.randn()
self.sensors.gps_n = pn + self._gps_eta_n
self.sensors.gps_e = pe + self._gps_eta_e
self.sensors.gps_h = h + self._gps_eta_h
self.sensors.gps_Vg = np.sqrt(
(Va * np.cos(psi) + wn)**2 +
(Va * np.sin(psi) +
we)**2) + SENSOR.gps_Vg_sigma * np.random.randn()
self.sensors.gps_course = np.arctan2(
Va * np.sin(psi) + we,
Va * np.cos(psi) +
wn) + SENSOR.gps_course_sigma * np.random.randn()
self._t_gps = 0.
else:
self._t_gps += self._ts_simulation
def dtI_dq(mav, x_euler, delta):
[phi, theta, psi] = Quaternion2Euler(x_euler[6:10])
dtI_dq = np.zeros([13, 12])
dtI_dq[0:6, 0:6] = np.eye(6)
dtI_dq[10:, 9:] = np.eye(3)
dtI_dq[6:10, 6:9] = jacobian(Euler2Quaternion, mav, (phi, theta, psi),
delta, 3)
return dtI_dq
def euler_state(x_quat):
# convert state x with attitude represented by quaternion
# to x_euler with attitude represented by Euler angles
e = np.array([[x_quat[6], x_quat[7], x_quat[8], x_quat[9]]])
[phi, theta, psi] = Quaternion2Euler(e)
x_euler = np.array([[x_quat[0][0]], [x_quat[1][0]], [x_quat[2][0]],
[x_quat[3][0]], [x_quat[4][0]], [x_quat[5][0]], [phi],
[theta], [psi], [x_quat[10][0]], [x_quat[11][0]],
[x_quat[12][0]]])
return x_euler
def dtheta_dq(quat):
h = 0.005
Jacobian = np.zeros((3, 4))
for i in range(4):
q_p = np.copy(quat)
q_p[i][0] += h
q_m = np.copy(quat)
q_m[i][0] -= h
phi, theta, psi = Quaternion2Euler(q_p)
f_p = np.array([[phi, theta, psi]]).T
phi, theta, psi = Quaternion2Euler(q_m)
f_m = np.array([[phi, theta, psi]]).T
Jac_col_i = (f_p - f_m) / (2 * h)
Jacobian[:, i] = Jac_col_i[:, 0]
return Jacobian
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.empty((12, 1))
x_euler[:6] = x_quat[:6]
x_euler[6:9] = np.array([[phi], [theta], [psi]])
x_euler[9:12] = x_quat[10:13]
return x_euler
def __init__(self, Ts):
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) # inertial north position
[MAV.pe0], # (1) # inertial east position
[MAV.pd0], # (2) # inertial down position, neg of altitude
[MAV.u0], # (3) # Body frame velocity nose direction (i)
[MAV.v0], # (4) # Body frame velocity right wing direction (j)
[MAV.w0], # (5) # Body frame velocity down direction (l)
# Quaternion rotation from inertial frame to the body frame
[MAV.e0
], # (6) # related to scalar part of rotation = cos(theta/2)
[
MAV.e1
], # (7) # related to vector we are rotating about = v*sin(theta/2)
[MAV.e2], # (8) # " "
[MAV.e3], # (9) # " "
[MAV.p0], # (10) # roll rate in body frame
[MAV.q0], # (11) # pitch rate in body frame
[MAV.r0]
]) # (12) # yaw rate in body frame
#rotation from vehicle to body
self.h = -self._state[2, 0]
self.Rbv = np.array([[1, 0, 0], [0, 1, 0],
[0, 0, 1]]) #rotation from vehicle to body
# store wind data for fast recall since it is used at various points in simulation
self._wind = np.array([[0.], [0.], [
0.
]]) # wind in NED frame in meters/sec (velocity of wind [uw, vw, ww])
self._update_velocity_data()
# store forces to avoid recalculation in the sensors function
self._forces = np.array(
[[0.], [0.], [0.]]) #forces acting on mav in body frame [fx,fy,fz]
self._Va = MAV.Va0 # velocity magnitude of airframe relative to airmass
self._alpha = 0 #angle of attack
self._beta = 0 #sideslip angle
temp1, temp2, temp3 = Quaternion2Euler(self._state[6:10])
self.phi = temp1[0] # roll
self.theta = temp2[0] # pitch
self.psi = temp3[0] #yaw
self.gamma = 0 #flight path angle (pitch up from horizontal velocity)
self.chi = 0 #course angle (heading)
self.Vg = np.linalg.norm(
np.dot(self.Rbv.T, np.array([[MAV.u0], [MAV.v0], [MAV.w0]])))
# initialize true_state message
self.msg_true_state = msg_state()
self.breakpoint = True
def euler_state(x_quat):
# convert state x with attitude represented by quaternion
# to x_euler with attitude represented by Euler angles
x_euler = np.zeros((12, 1))
phi, theta, psi = Quaternion2Euler(x_quat[6:10])
x_euler[0:6] = x_quat[0:6]
x_euler[6] = phi
x_euler[7] = theta
x_euler[8] = psi
x_euler[9:12] = x_quat[10:13]
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])
cos, sin = np.cos, np.sin
self.sensors.gyro_x = self._state.item(10) + np.random.normal(
SENSOR.gyro_x_bias, SENSOR.gyro_sigma)
self.sensors.gyro_y = self._state.item(11) + np.random.normal(
SENSOR.gyro_y_bias, SENSOR.gyro_sigma)
self.sensors.gyro_z = self._state.item(12) + np.random.normal(
SENSOR.gyro_z_bias, SENSOR.gyro_sigma)
self.sensors.accel_x = np.asscalar(
(self._forces.item(0) / MAV.mass) + MAV.gravity * sin(theta) +
np.random.normal(0, SENSOR.accel_sigma))
self.sensors.accel_y = np.asscalar(
(self._forces.item(1) / MAV.mass) -
MAV.gravity * cos(theta) * sin(phi) +
np.random.normal(0, SENSOR.accel_sigma))
self.sensors.accel_z = np.asscalar(
(self._forces.item(2) / MAV.mass) -
MAV.gravity * cos(theta) * cos(phi) +
np.random.normal(0, SENSOR.accel_sigma))
self.sensors.static_pressure = MAV.rho * MAV.gravity * self._state.item(
2) + np.random.normal(0, SENSOR.static_pres_sigma)
self.sensors.diff_pressure = (MAV.rho *
self._Va**2) / 2 + np.random.normal(
0, SENSOR.diff_pres_sigma)
if self._t_gps >= SENSOR.ts_gps:
# v[n+1]
self._gps_eta_n = np.exp(-SENSOR.gps_beta * SENSOR.ts_gps
) * self._gps_eta_n + np.random.normal(
0, SENSOR.gps_n_sigma)
self._gps_eta_e = np.exp(-SENSOR.gps_beta * SENSOR.ts_gps
) * self._gps_eta_e + np.random.normal(
0, SENSOR.gps_e_sigma)
self._gps_eta_h = np.exp(-SENSOR.gps_beta * SENSOR.ts_gps
) * self._gps_eta_h + np.random.normal(
0, SENSOR.gps_h_sigma)
# yGPS,_[n] = p_[n] + v_[n]
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.asscalar(
np.sqrt((self._Va * cos(psi) + self.msg_true_state.wn)**2 +
(self._Va * sin(psi) + self.msg_true_state.we)**2) +
np.random.normal(0, SENSOR.gps_Vg_sigma))
self.sensors.gps_course = np.asscalar(
np.arctan2(self._Va * sin(psi) + self.msg_true_state.we,
self._Va * cos(psi) + self.msg_true_state.wn) +
np.random.normal(0, SENSOR.gps_Vg_sigma))
self._t_gps = 0.
else:
self._t_gps += self._ts_simulation
def _update_velocity_data(self, wind=np.zeros((6, 1))):
# compute airspeed
e0 = self._state.item(6)
e1 = self._state.item(7)
e2 = self._state.item(8)
e3 = self._state.item(9)
#print("EEEES=",e0,e1,e2,e3)
phi, theta, psi = Quaternion2Euler(np.array([e0, e1, e2, e3]))
#print("angles=",phi,theta,psi)
Rv2b = RotationVehicle2Body(phi, theta, psi)
wind_result = np.matmul(Rv2b, wind[0:3]) + wind[3:6]
self._wind = np.matmul(Rv2b, wind_result)
uw = wind_result.item(0)
vw = wind_result.item(1)
ww = wind_result.item(2)
ur = self._state[3] - uw
vr = self._state[4] - vw
wr = self._state[5] - ww
ur = ur.item(0)
vr = vr.item(0)
wr = wr.item(0)
self._Va = sqrt(ur**2 + vr**2 + wr**2)
#print("update_velocities =",self._Va)
# compute angle of attack
self._alpha = atan2(wr, ur)
# compute sideslip angle
self._beta = np.arcsin(vr / self._Va)
# Vg, chi, gamma
Rb2v = RotationBody2Vehicle(phi, theta, psi)
Vg_result = np.matmul(Rb2v, self._state[3:6])
Vg_n = Vg_result.item(0)
Vg_e = Vg_result.item(1)
Vg_d = Vg_result.item(2)
self._Vg = sqrt(Vg_n**2 + Vg_e**2 + Vg_d**2)
self.gamma = atan2(-Vg_d, sqrt(Vg_n**2 + Vg_e**2))
self.chi = atan2(Vg_e, Vg_n)
# angle wrapping for commanded phi help
if self.chi_prev > 0.75 * np.pi and self.chi < 0.0:
self.chi += 2. * np.pi
if self.chi_prev < -0.75 * np.pi and self.chi > 0.0:
self.chi -= 2. * np.pi
self.chi_prev = self.chi
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 euler_state(x_quat):
# convert state x with attitude represented by quaternion
# to x_euler with attitude represented by Euler angles
e = np.array(
[x_quat.item(6),
x_quat.item(7),
x_quat.item(8),
x_quat.item(9)])
[phi, theta, psi] = Quaternion2Euler(e)
x_euler = np.array([[x_quat.item(0)], [x_quat.item(1)], [x_quat.item(2)],
[x_quat.item(3)], [x_quat.item(4)], [x_quat.item(5)],
[phi], [theta], [psi], [x_quat.item(10)],
[x_quat.item(11)], [x_quat.item(12)]])
return x_euler
def getALat(mav, trim_state, trim_input):
# take partial of f_euler with respect to x_euler
rho = MAV.rho
m = MAV.mass
u = mav._state.item(3)
w = mav._state.item(5)
Va = mav._Va
S = MAV.S_wing
b = MAV.b
c = MAV.c
Cpp = MAV.C_p_p
Cpb = MAV.C_p_beta
Cp0 = MAV.C_p_0
# b_2Va = b / (2*Va)
# c_2Va = c / (2*Va)
# beta = mav._beta
# alpha = mav._alpha
phi, theta, psi = Quaternion2Euler(trim_state[6:10])
frac = rho * S / 2
Y_v = frac * MAV.C_Y_beta / m * Va
Y_p = w + frac * Va * b / (2 * m) * MAV.C_Y_p
Y_r = -u + frac * Va * b / (2 * m) * MAV.C_Y_r
Y_da = frac * (Va**2) / m * MAV.C_Y_delta_a
Y_dr = frac * (Va**2) / m * MAV.C_Y_delta_r
L_v = frac * b * Cpb * Va
L_p = frac * Va * (b**2) / 2.0 * Cpp
L_r = frac * Va * (b**2) / 2.0 * MAV.C_p_r
L_da = frac * (Va**2) * b * MAV.C_p_delta_a
L_dr = frac * (Va**2) * b * MAV.C_p_delta_r
N_v = frac * b * MAV.C_r_beta * Va
N_p = frac * Va * (b**2) / 2.0 * MAV.C_r_p
N_r = frac * Va * (b**2) / 2.0 * MAV.C_r_r
N_da = frac * (Va**2) * b * MAV.C_r_delta_a
N_dr = frac * (Va**2) * b * MAV.C_r_delta_r
A_lat = np.array([
[Y_v, Y_p, Y_r, MAV.gravity * np.cos(theta) * np.cos(phi), 0],
[L_v, L_p, L_r, 0, 0],
[N_v, N_p, N_r, 0, 0],
[0, 1, np.cos(phi) * np.tan(theta), 0, 0],
[0, 0, np.cos(phi) / np.cos(theta), 0, 0],
])
return A_lat
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)
self.msg_true_state.Va = self._Va
self.msg_true_state.alpha = self._alpha # see line 164 updateVelocityData
self.msg_true_state.beta = self._beta # see line 167 updateVelocityData
self.msg_true_state.Vg = np.linalg.norm(self._state[3:6])
self.calcGammaAndChi()
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)
self.msg_true_state.Va = self._Va
self.msg_true_state.alpha = self._alpha # see line 164 updateVelocityData
self.msg_true_state.beta = self._beta # see line 167 updateVelocityData
self.msg_true_state.Vg = np.linalg.norm(self._state[3:6])
self.gamma = np.arctan2(-self._state.item(5), self._state.item(3)) # is this right atan2(-w, u)
self.chi = np.arctan2(self._state.item(4), self._state.item(3)) + psi # atan2(v, u)
def _update_msg_true_state(self):
phi, theta, psi = Quaternion2Euler(self._state[6], self._state[7], self._state[8], self._state[9])
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 _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
u = self._state.item(3)
v = self._state.item(4)
w = self._state.item(5)
e0 = self._state.item(6)
e1 = self._state.item(7)
e2 = self._state.item(8)
e3 = self._state.item(9)
# 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
self.msg_true_state.Vg = np.sqrt((self._state[3][0])**2 +
(self._state[4][0])**2 +
(self._state[5][0])**2)
self.msg_true_state.gamma = np.arcsin(
np.sqrt(pn_dot**2 + pe_dot**2) /
np.sqrt(pn_dot**2 + pe_dot**2 + pd_dot**2))
self.msg_true_state.chi = np.arctan(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_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 = 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_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 = np.linalg.norm(self._state[3:6])
self.msg_true_state.gamma = np.arctan2(-self._state.item(5),self._state.item(3))
self.msg_true_state.chi = np.arctan2(self._state.item(4),self._state.item(3))
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)
self._update_gamma_chi()
def getALon(mav, trim_state, trim_input):
rho = MAV.rho
m = MAV.mass
u = mav._state.item(3)
w = mav._state.item(5)
Va = mav._Va
S = MAV.S_wing
b = MAV.b
c = MAV.c
Cpp = MAV.C_p_p
Cpb = MAV.C_p_beta
Cp0 = MAV.C_p_0
# b_2Va = b / (2*Va)
# c_2Va = c / (2*Va)
de = trim_input.item(0)
beta = mav._beta
alpha = mav._alpha
phi, theta, psi = Quaternion2Euler(trim_state[6:10])
# X_u = u*rho*S/m * (MAV.C_X_0 + MAV.C_X_alpha*alpha + MAV.C_X_delta_e*de) \
A_lon - 0
return A_lon
