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.
