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 _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]
pdot = Euler2Rotation(self._state[6], self._state[7],
self._state[8]) @ 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 = self._state.item(6)
self.true_state.theta = self._state.item(7)
self.true_state.psi = self._state.item(8)
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(9)
self.true_state.q = self._state.item(10)
self.true_state.r = self._state.item(11)
self.true_state.wn = self._wind.item(0)
self.true_state.we = self._wind.item(1)
self.true_state.wd = self._wind.item(2)
def update(self, state):
mav_position = np.array([[state.pn], [state.pe], [-state.h]]) # NED coordinates
# attitude of mav as a rotation matrix from body to inertial
rotation = Euler2Rotation(state.phi, state.theta, state.psi)
for face in self.faces:
# rotate and translate points defining mav
R = rotation
facePoints = np.array([self.mav_points[face[0] - 1],
self.mav_points[face[1] - 1],
self.mav_points[face[2] - 1]]).T
# scale points for better rendering
scale = 50
facePoints = scale * facePoints
rotated_points = self.rotate_points(facePoints, R)
translated_points = self.translate_points(rotated_points, mav_position)
# convert North-East Down to East-North-Up for rendering
R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
translated_points = R @ translated_points
# convert points to triangular mesh defined as array of three 3D points (Nx3x3)
mesh = self.points_to_mesh(translated_points)
if self.faces.index(face) < 8:
i = 0 # Yellow main body
elif self.faces.index(face) < 10:
i = 1 # Blue wing
elif self.faces.index(face) < 12:
i = 2 # Green horizontal stabilizer
else:
i = 3 # Red vertical stabilizer
self.mav_body[self.faces.index(face)].setMeshData(vertexes=mesh, vertexColors=self.mav_meshColors[i])
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 update(self, measurement):
self.propagate_model(measurement)
self.measurement_update(measurement)
# write out estimate state
self.estimated_state.pn = self.xhat.item(0)
self.estimated_state.pe = self.xhat.item(1)
self.estimated_state.h = -self.xhat.item(2)
self.estimated_state.u = self.xhat.item(3)
self.estimated_state.v = self.xhat.item(4)
self.estimated_state.w = self.xhat.item(5)
self.estimated_state.phi = self.xhat.item(6)
self.estimated_state.theta = self.xhat.item(7)
self.estimated_state.psi = self.xhat.item(8)
self.estimated_state.bx = self.xhat.item(9)
self.estimated_state.by = self.xhat.item(10)
self.estimated_state.bz = self.xhat.item(11)
self.estimated_state.wn = self.xhat.item(12)
self.estimated_state.we = self.xhat.item(13)
# estimate needed quantities that are not part of state
R = Euler2Rotation(self.estimated_state.phi,
self.estimated_state.theta,
self.estimated_state.psi)
vel_body = self.xhat[3:6]
vel_world =
wind_world =
wind_body =
self.estimated_state.Va =
self.estimated_state.alpha =
self.estimated_state.beta =
self.estimated_state.Vg =
self.estimated_state.chi =
self.estimated_state.p =
self.estimated_state.q =
self.estimated_state.r =
return self.estimated_state
def __init__(self, state, window):
"""
Draw the MAV.
The input to this function is a (message) class with properties that define the state.
The following properties are assumed:
state.pn # north position
state.pe # east position
state.h # altitude
state.phi # roll angle
state.theta # pitch angle
state.psi # yaw angle
"""
# get points that define the non-rotated, non-translated mav and the mesh colors
self.mav_points, self.mav_meshColors = self.get_points()
mav_position = np.array([[state.pn], [state.pe], [-state.h]]) # NED coordinates
# attitude of mav as a rotation matrix R from body to inertial
R = Euler2Rotation(state.phi, state.theta, state.psi)
# rotate and translate points defining mav
rotated_points = self.rotate_points(self.mav_points, R)
translated_points = self.translate_points(rotated_points, mav_position)
# convert North-East Down to East-North-Up for rendering
R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
translated_points = R @ translated_points
# convert points to triangular mesh defined as array of three 3D points (Nx3x3)
mesh = self.points_to_mesh(translated_points)
self.mav_body = gl.GLMeshItem(vertexes=mesh, # defines the triangular mesh (Nx3x3)
vertexColors=self.mav_meshColors, # defines mesh colors (Nx1)
drawEdges=True, # draw edges between mesh elements
smooth=False, # speeds up rendering
computeNormals=False) # speeds up rendering
window.addItem(self.mav_body) # add body to plot
def update(self, path, state):
"""
Update the drawing of the mav.
The input to this function is a (message) class with properties that define the state.
The following properties are assumed:
state.pn # north position
state.pe # east position
state.h # altitude
state.phi # roll angle
state.theta # pitch angle
state.psi # yaw angle
"""
mav_position = np.array([[state.pn], [state.pe],
[-state.h]]) # NED coordinates
# attitude of mav as a rotation matrix R from body to inertial
R = Euler2Rotation(state.phi, state.theta, state.psi)
# rotate and translate points defining mav
rotated_points = self._rotate_points(self.points, R)
translated_points = self._translate_points(rotated_points,
mav_position)
# convert North-East Down to East-North-Up for rendering
R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
translated_points = R @ translated_points
# convert points to triangular mesh defined as array of three 3D points (Nx3x3)
mesh = self._points_to_mesh(translated_points)
# initialize the drawing the first time update() is called
if not self.plot_initialized:
if path.flag == 'line':
straight_line_object = self.straight_line_plot(path)
self.window.addItem(
straight_line_object) # add straight line to plot
else: # path.flag=='orbit
orbit_object = self.orbit_plot(path)
self.window.addItem(orbit_object)
# initialize drawing of triangular mesh.
self.body = gl.GLMeshItem(
vertexes=mesh, # defines the triangular mesh (Nx3x3)
vertexColors=self.meshColors, # defines mesh colors (Nx1)
drawEdges=True, # draw edges between mesh elements
smooth=False, # speeds up rendering
computeNormals=False) # speeds up rendering
self.window.addItem(self.body) # add body to plot
self.plot_initialized = True
# else update drawing on all other calls to update()
else:
# reset mesh using rotated and translated points
self.body.setMeshData(vertexes=mesh, vertexColors=self.meshColors)
# update the center of the camera view to the mav location
#view_location = Vector(state.pe, state.pn, state.h) # defined in ENU coordinates
#self.window.opts['center'] = view_location
# redraw
self.app.processEvents()
def update(self, delta, wind, simulation=True):
"""
Integrate the differential equations defining dynamics, update sensors
delta = (delta_a, delta_e, delta_r, delta_t) are the control inputs
wind is the wind vector in inertial coordinates
Ts is the time step between function calls.
"""
if not simulation:
gustInInertial = Euler2Rotation(self._state.item(6),
self._state.item(7),
self._state.item(8)) @ wind[3:6]
self._wind = wind[0:3] + gustInInertial
# get forces and moments acting on rigid bod
forces_moments = self._forces_moments(delta)
# Integrate ODE using Runge-Kutta RK4 algorithm
time_step = self._ts_simulation
k1 = self._derivatives(self._state, forces_moments)
k2 = self._derivatives(self._state + time_step / 2. * k1,
forces_moments)
k3 = self._derivatives(self._state + time_step / 2. * k2,
forces_moments)
k4 = self._derivatives(self._state + time_step * k3,
forces_moments)
self._state += time_step / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
# normalize the quaternion
'''e0 = self._state.item(6)
e1 = self._state.item(7)
e2 = self._state.item(8)
e3 = self._state.item(9)
normE = np.sqrt(e0**2 + e1**2 + e2**2 + e3**2)
self._state[6][0] = self._state.item(6) / normE
self._state[7][0] = self._state.item(7) / normE
self._state[8][0] = self._state.item(8) / normE
self._state[9][0] = self._state.item(9) / normE'''
# update the airspeed, angle of attack, and side slip angles using new state
self._update_velocity_data(wind)
self._update_true_state()
else:
# update the message class for the true state
self._update_sensors()
self.true_state.pn = self.sensors.gps_n
self.true_state.pe = self.sensors.gps_e
self.true_state.h = self.sensors.gps_h
self.true_state.phi = self.telemetry.vehicle.attitude.roll
self.true_state.theta = self.telemetry.vehicle.attitude.pitch
self.true_state.psi = self.telemetry.vehicle.attitude.yaw
self.true_state.p = self.sensors.gyro_x
self.true_state.q = self.sensors.gyro_y
self.true_state.r = self.sensors.gyro_z
self.true_state.Vg = self.sensors.gps_Vg
self.true_state.chi = self.sensors.gps_course
def __init__(self, state, window):
"""
Draw the MAV.
The input to this function is a (message) class with properties that define the state.
The following properties are assumed:
state.pn # north position
state.pe # east position
state.h # altitude
state.phi # roll angle
state.theta # pitch angle
state.psi # yaw angle
"""
# get points that define the non-rotated, non-translated mav and the mesh colors
self.mav_points, self.mav_meshColors = self.get_points()
self.faces = self.gatherPointNumbers()
mav_position = np.array([[state.pn], [state.pe], [-state.h]]) # NED coordinates
# attitude of mav as a rotation matrix from body to inertial
rotation = Euler2Rotation(state.phi, state.theta, state.psi)
self.mav_body = []
for face in self.faces:
R = rotation
# rotate and translate points defining mav
facePoints = np.array([self.mav_points[face[0] - 1],
self.mav_points[face[1] - 1],
self.mav_points[face[2] - 1]]).T
# scale points for better rendering
scale = 50
facePoints = scale * facePoints
rotated_points = self.rotate_points(facePoints, R)
translated_points = self.translate_points(rotated_points, mav_position)
# convert North-East Down to East-North-Up for rendering
R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
translated_points = R @ translated_points
# convert points to triangular mesh defined as array of three 3D points (Nx3x3)
mesh = self.points_to_mesh(translated_points)
if self.faces.index(face) < 8:
i = 0 # Yellow main body
elif self.faces.index(face) < 10:
i = 1 # Blue wing
elif self.faces.index(face) < 12:
i = 2 # Green horizontal stabilizer
else:
i = 3 # Red vertical stabilizer
self.mav_body.append(gl.GLMeshItem(vertexes=mesh, # defines the triangular mesh (Nx3x3)
vertexColors=self.mav_meshColors[i], # defines mesh colors (Nx1)
drawEdges=True, # draw edges between mesh elements
smooth=False, # speeds up rendering
computeNormals=False)) # speeds up rendering
window.addItem(self.mav_body[-1]) # add body to plot
def update(self, state):
mav_position = np.array([[state.pn], [state.pe], [-state.h]]) # NED coordinates
# attitude of mav as a rotation matrix R from body to inertial
R = Euler2Rotation(state.phi, state.theta, state.psi)
# rotate and translate points defining mav
rotated_points = self.rotate_points(self.mav_points, R)
translated_points = self.translate_points(rotated_points, mav_position)
# convert North-East Down to East-North-Up for rendering
R = np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]])
translated_points = R @ translated_points
# convert points to triangular mesh defined as array of three 3D points (Nx3x3)
mesh = self.points_to_mesh(translated_points)
# draw MAV by resetting mesh using rotated and translated points
self.mav_body.setMeshData(vertexes=mesh, vertexColors=self.mav_meshColors)
def sensors(self):
"Return value of sensors on MAV: gyros, accels, absolute_pressure, dynamic_pressure, GPS"
# simulate rate gyros(units are rad / sec)
self._sensors.gyro_x =
self._sensors.gyro_y =
self._sensors.gyro_z =
# simulate accelerometers(units of g)
self._sensors.accel_x =
self._sensors.accel_y =
self._sensors.accel_z =
# simulate magnetometers
# magnetic field in provo has magnetic declination of 12.5 degrees
# and magnetic inclination of 66 degrees
R_mag = Euler2Rotation(0.0, np.radians(-66), np.radians(12.5))
# magnetic field in inertial frame: unit vector
mag_inertial =
R = Quaternion2Rotation(self._state[6:10]) # body to inertial
# magnetic field in body frame: unit vector
mag_body =
self._sensors.mag_x =
self._sensors.mag_y =
self._sensors.mag_z =
# simulate pressure sensors
self._sensors.abs_pressure =
self._sensors.diff_pressure =
# simulate GPS sensor
if self._t_gps >= SENSOR.ts_gps:
self._gps_eta_n =
self._gps_eta_e =
self._gps_eta_h =
self._sensors.gps_n =
self._sensors.gps_e =
self._sensors.gps_h =
self._sensors.gps_Vg =
self._sensors.gps_course =
self._t_gps = 0.
else:
self._t_gps += self._ts_simulation
return self._sensors
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
