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