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.