def __init__(self, Ts): self.ts_simulation = Ts self._state = np.array( [MAV.pn0, MAV.pe0, MAV.pd0, MAV.u0, MAV.v0, MAV.w0, MAV.e0, MAV.e1, MAV.e2, MAV.e3, MAV.p0, MAV.q0, MAV.r0 ]) self._wind = np.array([[0.], [0.], [0.]]) # wind in NED frame in meters/sec self._update_velocity_data() # store forces to avoid recalculation in the sensors function self._forces = np.array([[0.], [0.], [0.]]) self.f_g = np.array([0, 0, 0]) self._Va = MAV.Va0 self._Vg = self._Va self._alpha = 0 self._beta = 0 self.thrust = 0 self._chi = 0 # initialize true_state message self.msg_true_state = msg_state() # initialize the sensors message 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): self._forces = np.array([[0.], [0.], [0.]]) # initialize the sensors message 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): 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._moments = np.array([[0.], [0.], [0.]]) #moments acting on mav[Mx,My,Mz] 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() # initialize the sensors message 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): 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) # 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 self._update_velocity_data() self._forces = np.array([[0.], [0.], [0.]]) self._Va = MAV.u0 self._alpha = 0 self._beta = 0 # initialize the sensors message self.sensors = msg_sensors() # initialize true_state message self.msg_true_state = msg_state() # 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. # update velocity data and forces and moments self._update_velocity_data() self._forces_moments(delta=np.array([[0.0], [0.0], [0.0], [0.5]]))
def __init__(self, Ts): self._ts_simulation = Ts # _state = [pn, pe, pd, u, v, w, e0, e1, e2, e3, p, q, r] # 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) # 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 self._update_velocity_data() # store forces to avoid recalculation in the sensors function self._forces = np.array([[0.], [0.], [0.]]) self._Va = MAV.Va0 self.Va_b = np.array([[0.], [0.], [0.]]) self.Vg_b = np.array([[0.], [0.], [0.]]) self._Vg = MAV.Va0 self.Vg_i = np.array([[MAV.u0, MAV.v0, MAV.w0]]).T self._alpha = 0 self._beta = 0 # initialize true_state message self.msg_true_state = msg_state() self.msg_true_state.h = -MAV.pd0 # initialize the sensors message 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): 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() self._update_true_state() # 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. self.update_sensors()
def __init__(self, Ts): self._ts_simulation = Ts self._state = np.array([ [QUAD.pn0], # (0) [QUAD.pe0], # (1) [QUAD.pd0], # (2) [QUAD.u0], # (3) [QUAD.v0], # (4) [QUAD.w0], # (5) [QUAD.phi0], # (6) [QUAD.theta0], # (7) [QUAD.psi0], # (8) [QUAD.p0], # (9) [QUAD.q0], # (10) [QUAD.r0] ]) # (11) self.sensor_measurements = msg_sensors() self.sensor_measurements.pn = QUAD.pn0 self.sensor_measurements.pe = QUAD.pe0 self.sensor_measurements.pd = QUAD.pd0 self.sensor_measurements.u = QUAD.u0 self.sensor_measurements.v = QUAD.v0 self.sensor_measurements.w = QUAD.w0 self.sensor_measurements.phi = QUAD.phi0 self.sensor_measurements.theta = QUAD.theta0 self.sensor_measurements.psi = QUAD.psi0 self.sensor_measurements.p = QUAD.p0 self.sensor_measurements.q = QUAD.q0 self.sensor_measurements.r = QUAD.r0 self.Q = SENSOR.Q_N # progagation noise self.R = SENSOR.R_N # sensor noise self.msg_true_state = msg_state() self.update_msg_true_state() self.update_sensors()
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._forces = np.array([[0.], [0.], [0.]]) self._Va = MAV.Va0 self._alpha = 0 self._beta = 0 self.msg_true_state = msg_state() # 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 self._update_velocity_data() # initialize the sensors message 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.