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_control):
# initialized estimated state message
self.estimated_state = msg_state()
self.estimated_state.pn = MAV.pn0 # initial north position
self.estimated_state.pe = MAV.pe0 # initial east position
self.estimated_state.h = -MAV.pd0 # initial down position
self.estimated_state.phi = MAV.phi0
self.estimated_state.theta = MAV.theta0
self.estimated_state.psi = MAV.psi0
self.estimated_state.Va = MAV.Va0
self.estimated_state.p = MAV.p0
self.estimated_state.q = MAV.q0
self.estimated_state.r = MAV.r0
self.estimated_state.Vg = MAV.Va0
# use alpha filters to low pass filter gyros and accels
self.lpf_gyro_x = alpha_filter(alpha=0.5)
self.lpf_gyro_y = alpha_filter(alpha=0.5)
self.lpf_gyro_z = alpha_filter(alpha=0.5)
self.lpf_accel_x = alpha_filter(alpha=0.5)
self.lpf_accel_y = alpha_filter(alpha=0.5)
self.lpf_accel_z = alpha_filter(alpha=0.5)
# use alpha filters to low pass filter static and differential pressure
self.lpf_static = alpha_filter(alpha=0.9)
self.lpf_diff = alpha_filter(alpha=0.5)
# ekf for phi and theta
self.attitude_ekf = ekf_attitude(self.estimated_state)
# ekf for pn, pe, Vg, chi, wn, we, psi
self.position_ekf = ekf_position()
def __init__(self, ts_control):
# instantiate lateral controllers
self.roll_from_aileron = pd_control_with_rate(kp=AP.roll_kp,
kd=AP.roll_kd,
limit=np.radians(45))
self.course_from_roll = pi_control(kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit=np.radians(30))
self.sideslip_from_rudder = pi_control(kp=AP.sideslip_kp,
ki=AP.sideslip_ki,
Ts=ts_control,
limit=np.radians(45))
self.yaw_damper = transfer_function(
num=np.array([[AP.yaw_damper_kp, 0]]),
den=np.array([[1, 1 / AP.yaw_damper_tau_r]]),
Ts=ts_control)
# instantiate lateral controllers
self.pitch_from_elevator = pd_control_with_rate(kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit=np.radians(45))
self.altitude_from_pitch = pi_control(kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit=np.radians(30))
self.airspeed_from_throttle = pi_control(kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit=1.0)
self.commanded_state = msg_state()
def _follow_straight_line(self, path=msg_path(), state=msg_state()):
q = np.copy(path.line_direction)
chi_q = atan2(q[1], q[0])
chi_q = self._wrap(chi_q, state.chi)
Rp_i = np.array([[cos(chi_q), sin(chi_q), 0],
[-sin(chi_q), cos(chi_q), 0], [0, 0, 1]])
r = path.line_origin
p = np.array([state.pn, state.pe, -state.h])
# chi_c: Course direction of path
ei_p = p - r
ep = Rp_i @ ei_p
chi_d = self.chi_inf * (2 / np.pi) * atan(self.k_path * ep[1])
chi_c = chi_q - chi_d
# h_c: Altitude
product = np.cross(q, np.array([0, 0, 1]))
n = product / np.linalg.norm(product)
s = ei_p - (ei_p @ n) * n
hc = -r[2] - np.sqrt(s[0]**2 + s[1]**2) / np.sqrt(q[0]**2 +
q[1]**2) * q[2]
self.autopilot_commands.airspeed_command = path.airspeed
self.autopilot_commands.course_command = chi_c
self.autopilot_commands.altitude_command = hc
self.autopilot_commands.phi_feedforward = 0
def __init__(self, ts_control):
#states
self.estimated_state = msg_state()
self.estimated_state.pn = MAV.pn0_n # inertial north position in meters
self.estimated_state.pe = MAV.pe0_n # inertial east position in meters
self.estimated_state.h = -MAV.pd0_n # inertial altitude in meters
self.estimated_state.phi = MAV.phi0_n # roll angle in radians
self.estimated_state.theta = MAV.theta0_n # pitch angle in radians
self.estimated_state.psi = MAV.psi0_n
# yaw angle in radians
self.estimated_state.Va = MAV.Va0_n # airspeed in meters/sec
self.estimated_state.alpha = 0. # angle of attack in radians
self.estimated_state.beta = 0. # sideslip angle in radians
self.estimated_state.p = 0. # roll rate in radians/sec
self.estimated_state.q = 0. # pitch rate in radians/sec
self.estimated_state.r = 0. # yaw rate in radians/sec
self.estimated_state.Vg = 0. # groundspeed in meters/sec
self.estimated_state.gamma = 0. # flight path angle in radians
self.estimated_state.chi = 0. # course angle in radians
self.estimated_state.wn = 0. # inertial windspeed in north direction in meters/sec
self.estimated_state.we = 0. # inertial windspeed in east direction in meters/sec
self.estimated_state.bx = 0. # gyro bias along roll axis in radians/sec
self.estimated_state.by = 0. # gyro bias along pitch axis in radians/sec
self.estimated_state.bz = 0. # gyro bias along yaw axis in radians/sec
# estimators
self.directEKF = indirectExtendedKalmanFilter()
self.lpf_gyro_x = alpha_filter(alpha=SENS.gyro_alpha)
self.lpf_gyro_y = alpha_filter(alpha=SENS.gyro_alpha)
self.lpf_gyro_z = alpha_filter(alpha=SENS.gyro_alpha)
self.lpf_accel_x = alpha_filter(alpha=SENS.accel_alpha)
self.lpf_accel_y = alpha_filter(alpha=SENS.accel_alpha)
self.lpf_accel_z = alpha_filter(alpha=SENS.accel_alpha)
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()
# store forces to avoid recalculation in the sensors function
self._forces = np.array([[0.], [0.], [0.]])
self._Va = MAV.u0
self._alpha = 0
self._beta = 0
# initialize true_state message
self.msg_true_state = msg_state()
def __init__(self, ts_control):
# instantiate lateral controllers
self.roll_from_aileron = pdControlWithRate(kp=AP.roll_kp,
kd=AP.roll_kd,
limit=AP.delta_a_max)
self.course_from_roll = piControl(kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit=AP.roll_max)
self.yaw_damper = transfer_function(
num=np.array([[AP.yaw_damper_kp, 0]]),
den=np.array([[1, 1 / AP.yaw_damper_tau_r]]),
Ts=ts_control)
# instantiate lateral controllers
self.pitch_from_elevator = pdControlWithRate(kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit=AP.delta_e_max)
self.altitude_from_pitch = piControl(kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit=AP.pitch_max)
self.airspeed_from_throttle = piControl(kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit=AP.throttle_max)
self.commanded_state = msg_state()
def __init__(self, ts_control):
self.control = lqr_control(CON.A, CON.B, CON.Q, CON.R, ts_control)
self.commanded_state = msg_state()
self.trim_input = CON.trim_input
self.inputs = self.trim_input
self.limit = CON.limit_lqr
self.Kp = CON.K_p
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:
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.msg_true_state = msg_state()
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]
self._state = np.array([
])
self.msg_true_state = msg_state()
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]
self._state = np.array([[MAV.pn0], [MAV.pe0], [MAV.pd0], [0], [MAV.v0],
[MAV.w0], [MAV.e0], [MAV.e1], [MAV.e2],
[MAV.e3], [MAV.p0], [MAV.q0], [MAV.r0]])
self.msg_true_state = msg_state()
def update_holodeck(env, true_state=msg_state()):
# Set up arrays for calling set_state with holodeck
pos = np.array([true_state.pn, true_state.pe, true_state.h]) * 100 * DIST_SCALE
pos += np.array([OFFSET_N, OFFSET_E, 0])
att = np.array([true_state.phi, true_state.theta, true_state.psi]) * 180.0/np.pi
vel = [0,0,0]
angvel = [0,0,0]
env.set_state("uav0", pos, att, vel, angvel)["uav0"]
def __init__(self, ts_control):
# instantiate lateral controllers
self.lat = lqr_control(mat.A_lat, mat.B_lat, mat.C_lat, mat.Klat,
mat.xlat_eq, mat.ylat_eq, mat.ulat_eq,
mat.limitlat, mat.Kilat, ts_control)
self.lon = lqr_control(mat.A_lon,
mat.B_lon,
mat.C_lon,
mat.Klon,
mat.xlon_eq,
mat.ylon_eq,
mat.ulon_eq,
mat.limitlon,
mat.Kilon,
ts_control,
throttle_flag=True)
# self.roll_from_aileron = pid_control( #pd_control_with_rate(
# kp=AP.roll_kp,
# kd=AP.roll_kd,
# Ts=ts_control,
# limit=np.radians(45))
# self.course_from_roll = pid_control( #pi_control(
# kp=AP.course_kp,
# ki=AP.course_ki,
# Ts=ts_control,
# limit=np.radians(30))
# self.sideslip_from_rudder = pid_control( #pi_control(
# kp=AP.sideslip_kp,
# ki=AP.sideslip_ki,
# Ts=ts_control,
# limit=np.radians(45))
# self.yaw_damper = matlab.tf([0.5, 0.],[1.0, ],ts_control)
# #
# # num=np.array([[AP.yaw_damper_kp, 0]]),
# # den=np.array([[1, 1/AP.yaw_damper_tau_r]]),
# # Ts=ts_control)
#
# # instantiate longitudinal controllers
# self.pitch_from_elevator = pid_control( #pd_control_with_rate(
# kp=AP.pitch_kp,
# kd=AP.pitch_kd,
# limit=np.radians(45))
# self.altitude_from_pitch = pid_control( #pi_control(
# kp=AP.altitude_kp,
# ki=AP.altitude_ki,
# Ts=ts_control,
# limit=np.radians(30))
# self.airspeed_from_throttle = pid_control( #pi_control(
# kp=AP.airspeed_throttle_kp,
# ki=AP.airspeed_throttle_ki,
# Ts=ts_control,
# limit=1.5,
# throttle_flag=True)
self.commanded_state = msg_state()
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 __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]
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._state = self._state.reshape((13, 1))
self.msg_true_state = msg_state()
def __init__(self, Ts, posVel):
self.Ts = Ts
self.posVel = posVel
#for plotting
self.state = msg_state()
self.state.pn = posVel.item(0)
self.state.pe = posVel.item(1)
self.state.h = -posVel.item(2)
self.state.phi = 0.0
self.state.theta = 0.0
self.state.psi = 0.0
self.sigma = 5.0
def __init__(self):
self.estimated_state = msg_state()
xhat0 = SENSOR.xhat_0
self.estimated_state.u = xhat0.item(0)
self.estimated_state.v = xhat0.item(1)
self.estimated_state.w = xhat0.item(2)
self.estimated_state.phi = xhat0.item(3)
self.estimated_state.theta = xhat0.item(4)
self.estimated_state.psi = xhat0.item(5)
self.estimated_state.p = xhat0.item(6)
self.estimated_state.q = xhat0.item(7)
self.estimated_state.r = xhat0.item(8)
self.kalman = kalman_filter(SIM.ts_simulation)
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_control):
# initialized estimated state message
self.estimated_state = msg_state()
# use alpha filters to low pass filter gyros and accels
self.lpf_gyro_x = alpha_filter(alpha=0.5)
self.lpf_gyro_y = alpha_filter(alpha=0.5)
self.lpf_gyro_z = alpha_filter(alpha=0.5)
self.lpf_accel_x = alpha_filter(alpha=0.5)
self.lpf_accel_y = alpha_filter(alpha=0.5)
self.lpf_accel_z = alpha_filter(alpha=0.5)
# use alpha filters to low pass filter static and differential pressure
self.lpf_static = alpha_filter(alpha=0.9)
self.lpf_diff = alpha_filter(alpha=0.5)
# ekf for phi and theta
self.attitude_ekf = ekf_attitude()
# ekf for pn, pe, Vg, chi, wn, we, psi
self.position_ekf = ekf_position()
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_control):
# initialized estimated state message
self.estimated_state = msg_state()
# use alpha filters to low pass filter gyros and accels
self.lpf_gyro_x = alpha_filter(alpha=0.5)
self.lpf_gyro_y = alpha_filter(alpha=0.5)
self.lpf_gyro_z = alpha_filter(alpha=0.5)
self.lpf_accel_x = alpha_filter(alpha=0.9)
self.lpf_accel_y = alpha_filter(alpha=0.9)
self.lpf_accel_z = alpha_filter(alpha=0.9)
# use alpha filters to low pass filter static and differential pressure
self.lpf_static = alpha_filter(alpha=0.9)
self.lpf_diff = alpha_filter(alpha=0.5)
# lowpass filter option
self.lpf_gps_n = alpha_filter(alpha=0.5)
self.lpf_gps_e = alpha_filter(alpha=0.5)
self.lpf_gps_course = alpha_filter(alpha=0.5)
def __init__(self, ts_control):
# instantiate lateral controllers
self.roll_from_aileron = pid_control( #pd_control_with_rate(
kp=AP.roll_kp,
kd=AP.roll_kd,
Ts=ts_control,
limit=np.radians(45))
self.course_from_roll = pid_control( #pi_control(
kp=AP.course_kp,
ki=AP.course_ki,
Ts=ts_control,
limit=np.radians(30))
self.sideslip_from_rudder = pid_control( #pi_control(
kp=AP.sideslip_kp,
ki=AP.sideslip_ki,
Ts=ts_control,
limit=np.radians(45))
self.yaw_damper = matlab.tf([0.5, 0.], [
1.0,
], ts_control)
#
# num=np.array([[AP.yaw_damper_kp, 0]]),
# den=np.array([[1, 1/AP.yaw_damper_tau_r]]),
# Ts=ts_control)
# instantiate lateral controllers
self.pitch_from_elevator = pid_control( #pd_control_with_rate(
kp=AP.pitch_kp,
kd=AP.pitch_kd,
limit=np.radians(45))
self.altitude_from_pitch = pid_control( #pi_control(
kp=AP.altitude_kp,
ki=AP.altitude_ki,
Ts=ts_control,
limit=np.radians(30))
self.airspeed_from_throttle = pid_control( #pi_control(
kp=AP.airspeed_throttle_kp,
ki=AP.airspeed_throttle_ki,
Ts=ts_control,
limit=1.5,
throttle_flag=True)
self.commanded_state = msg_state()
def __init__(self, ts_control):
# initialized estimated state message
self.estimated_state = msg_state()
self.estimated_state.pn = MAV.pn0 # inertial north position in meters
self.estimated_state.pe = MAV.pe0 # inertial east position in meters
self.estimated_state.h = -MAV.pd0 # inertial altitude in meters
self.estimated_state.phi = MAV.phi0 # roll angle in radians
self.estimated_state.theta = MAV.theta0 # pitch angle in radians
self.estimated_state.psi = MAV.psi0 # yaw angle in radians
self.estimated_state.Va = MAV.Va0 # airspeed in meters/sec
self.estimated_state.alpha = 0. # angle of attack in radians
self.estimated_state.beta = 0. # sideslip angle in radians
self.estimated_state.p = 0. # roll rate in radians/sec
self.estimated_state.q = 0. # pitch rate in radians/sec
self.estimated_state.r = 0. # yaw rate in radians/sec
self.estimated_state.Vg = 0. # groundspeed in meters/sec
self.estimated_state.gamma = 0. # flight path angle in radians
self.estimated_state.chi = 0. # course angle in radians
self.estimated_state.wn = 0. # inertial windspeed in north direction in meters/sec
self.estimated_state.we = 0. # inertial windspeed in east direction in meters/sec
self.estimated_state.bx = 0. # gyro bias along roll axis in radians/sec
self.estimated_state.by = 0. # gyro bias along pitch axis in radians/sec
self.estimated_state.bz = 0. # gyro bias along yaw axis in radians/sec
# use alpha filters to low pass filter gyros and accels
self.lpf_gyro_x = alpha_filter(alpha=SENS.gyro_alpha)
self.lpf_gyro_y = alpha_filter(alpha=SENS.gyro_alpha)
self.lpf_gyro_z = alpha_filter(alpha=SENS.gyro_alpha)
self.lpf_accel_x = alpha_filter(alpha=SENS.accel_alpha)
self.lpf_accel_y = alpha_filter(alpha=SENS.accel_alpha)
self.lpf_accel_z = alpha_filter(alpha=SENS.accel_alpha)
# use alpha filters to low pass filter absolute and differential pressure
self.lpf_abs = alpha_filter(alpha=SENS.static_pres_alpha,
y0=self.estimated_state.h * MAV.rho *
MAV.gravity)
self.lpf_diff = alpha_filter(alpha=SENS.diff_press_alpha,
y0=MAV.rho *
(self.estimated_state.Va**2) / 2)
# ekf for phi and theta
self.attitude_ekf = ekf_attitude()
# ekf for pn, pe, Vg, chi, wn, we, psi
self.position_ekf = ekf_position()
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
# 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]
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 #the quaternion values come from the initial states phi0, th0, psi0
]]).T
self.msg_true_state = msg_state()
def __init__(self, Ts):
self._ts_simulation = Ts
# set initial states based on parameter file
# _state is the 8x1 internal state of the aircraft that is being propagated:
# _state = [pn, pe, pd, u, v, w, psi, r]
self._state = np.array([
[TAR.pn0], # (0)
[TAR.pe0], # (1)
[TAR.pd0], # (2)
[TAR.u0], # (3)
[TAR.v0], # (4)
[TAR.w0], # (5)
[TAR.psi0], # (6)
[TAR.r0]
]) # (7)
self._update_velocity_data()
# store forces to avoid recalculation in the sensors function
self._forces = np.array([[0.], [0.], [0.]])
self._Va = TAR.u0
# initialize true_state message
self.msg_true_state = msg_state()
self._update_msg_true_state()
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=0.02):
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)
self.R_vb = Quaternion2Rotation(
self._state[6:10]) # Rotation body->vehicle
self.R_bv = np.copy(self.R_vb).T # vehicle->body
# store wind data for fast recall since it is used at various points in simulation
self._wind = np.zeros(3) # wind in NED frame in meters/sec
# store forces to avoid recalculation in the sensors function
self._update_velocity_data()
self._forces = np.zeros(3)
self._Va = MAV.u0
self._alpha = 0
self._beta = 0
# initialize true_state message
self.true_state = msg_state()
def _follow_orbit(self, path=msg_path(), state=msg_state()):
p = np.array([state.pn, state.pe, -state.h]) # NED position
d = p - path.orbit_center # radial distance from orbit center
rho = path.orbit_radius
if path.orbit_direction == 'CW':
lmbda = 1
else:
lmbda = -1
var_phi = atan2(d[1], d[0])
var_phi = self._wrap(var_phi, state.chi)
chi_0 = var_phi + lmbda * (np.pi / 2)
chi_c = chi_0 + lmbda * atan(self.k_orbit *
(np.linalg.norm(d) - rho) / rho)
Vg = state.Vg
chi = state.chi
psi = state.psi
phi_feedfw = atan(Vg**2 / (self.gravity * rho * cos(chi - psi)))
self.autopilot_commands.airspeed_command = path.airspeed
self.autopilot_commands.course_command = chi_c
self.autopilot_commands.altitude_command = -path.orbit_center[2]
self.autopilot_commands.phi_feedforward = phi_feedfw
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.
