def test_skywalkerX8_force_x_dir():
control_input = ControlInput()
control_input.throttle = 0.0
t = 0
state = State()
state.vx = 20.0
for j in range(3):
state.pitch = state.pitch + 0.05
state.roll = state.roll + 0.05
state.yaw = state.yaw + 0.05
x_update = np.zeros(11)
constants = SkywalkerX8Constants()
for i in range(0, 11):
control_input.throttle = i * 0.1
prop = IcedSkywalkerX8Properties(control_input)
params = {"prop": prop, "wind": no_wind()}
update = dynamics_kinetmatics_update(t,
x=state.state,
u=control_input.control_input,
params=params)
x_update[i] = update[6]
S_p = constants.propeller_area
C_p = constants.motor_efficiency_fact
k_m = constants.motor_constant
m = constants.mass
K = 2 * m / (AIR_DENSITY * S_p * C_p * k_m**2)
for i in range(0, 10):
throttle_0 = i * 0.1
throttle_1 = (i + 1) * 0.1
assert np.allclose(K * (x_update[i + 1] - x_update[i]),
throttle_1**2 - throttle_0**2)
def side_force_coeff_test_cases():
constants = SkywalkerX8Constants()
b = constants.wing_span
state1 = State()
state1.vx = 20.0
state1.vy = 0.0
state1.vz = 0.0
state2 = State()
state2.vx = 28.6362532829
state2.vy = 1.0
state2.vz = 0.0
state2.ang_rate_x = 5 * np.pi / 180
state2.ang_rate_z = 5 * np.pi / 180
wind = np.zeros(6)
airspeed = np.sqrt(np.sum(calc_airspeed(state2, wind)**2))
zero_input = ControlInput()
aileron_input = ControlInput()
aileron_input.aileron_deflection = 2.0 * np.pi / 180.0
return [
(state1, wind, 0.0, zero_input, 2.99968641720902E-08),
(state1, wind, 1.0, zero_input, -7.94977329537982E-06),
(state2, wind, 0.0, aileron_input,
-0.008604744865183 + -0.085 * b / (2 * airspeed) * state2.ang_rate_x +
0.005 * b / (2 * airspeed) * state2.ang_rate_z +
0.0433 * aileron_input.aileron_deflection),
(state2, wind, 1.0, aileron_input,
-0.007089388672593 + -0.133 * b / (2 * airspeed) * state2.ang_rate_x +
0.002 * b / (2 * airspeed) * state2.ang_rate_z +
0.0433 * aileron_input.aileron_deflection)
]
def aircraft_property_from_dct(aircraft: dict) -> AircraftProperties:
"""
Return an aircraft property class initialized with the passed dictionary
"""
known_types = [
SKYWALKERX8, FRICTIONLESS_BALL, AIRCRAFT_NO_FORCE,
SKYWALKERX8_ASYMETRIC
]
aircraft_type = aircraft.get('type', "")
if aircraft_type not in known_types:
raise ValueError(f"Aircraft type must be one of {known_types}")
if aircraft_type == SKYWALKERX8:
return IcedSkywalkerX8Properties(ControlInput(),
aircraft.get('icing', 0))
elif aircraft_type == SKYWALKERX8_ASYMETRIC:
return AsymetricIcedSkywalkerX8Properties(
ControlInput(), aircraft.get('icing_left_wing', 0),
aircraft.get('icing_right_wing', 0))
elif aircraft_type == FRICTIONLESS_BALL:
return FrictionlessBall(ControlInput())
elif aircraft_type == AIRCRAFT_NO_FORCE:
return SimpleTestAircraftNoForces(ControlInput())
raise ValueError("Unknown aircraft type")
def test_rudder_deflection():
control_input = ControlInput()
rudder_deflection = 0.2
control_input.rudder_deflection = rudder_deflection
assert control_input.rudder_deflection == pytest.approx(rudder_deflection)
assert np.allclose(control_input.control_input,
[0.0, 0.0, rudder_deflection, 0.0])
def roll_moment_coeff_test_cases():
constants = SkywalkerX8Constants()
b = constants.wing_span
state1 = State()
state1.vx = 20.0
state1.vy = 0.0
state1.vz = 0.0
state2 = State()
state2.vx = 28.6362532829
state2.vy = 1.0
state2.vz = 0.0
state2.ang_rate_x = 5 * np.pi / 180
state2.ang_rate_z = 5 * np.pi / 180
wind = np.zeros(6)
airspeed = np.sqrt(np.sum(calc_airspeed(state2, wind)**2))
zero_input = ControlInput()
aileron_input = ControlInput()
aileron_input.aileron_deflection = 2.0 * np.pi / 180.0
return [
(state1, wind, 0.0, zero_input, -8.40821757613653E-05),
(state1, wind, 1.0, zero_input, -7.34515369827804E-05),
(state2, wind, 0.0, aileron_input,
-0.00380800071177 + -0.409 * b / (2 * airspeed) * state2.ang_rate_x +
0.039 * b / (2 * airspeed) * state2.ang_rate_z +
0.12 * aileron_input.aileron_deflection),
(state2, wind, 1.0, aileron_input,
-0.003067251004494 + -0.407 * b / (2 * airspeed) * state2.ang_rate_x +
0.158 * b / (2 * airspeed) * state2.ang_rate_z +
0.12 * aileron_input.aileron_deflection)
]
def yaw_moment_coeff_test_cases():
constants = SkywalkerX8Constants()
b = constants.wing_span
state1 = State()
state1.vx = 20.0
state1.vy = 0.0
state1.vz = 0.0
state2 = State()
state2.vx = 28.6362532829
state2.vy = 1.0
state2.vz = 0.0
state2.ang_rate_x = 5 * np.pi / 180
state2.ang_rate_z = 5 * np.pi / 180
wind = np.zeros(6)
airspeed = np.sqrt(np.sum(calc_airspeed(state2, wind)**2))
zero_input = ControlInput()
aileron_input = ControlInput()
aileron_input.aileron_deflection = 2.0 * np.pi / 180.0
return [
(state1, wind, 0.0, zero_input, 4.9176697574439E-06),
(state1, wind, 1.0, zero_input, 1.96093394589053E-05),
(state2, wind, 0.0, aileron_input,
0.000825947539055 + 0.027 * b / (2 * airspeed) * state2.ang_rate_x +
-0.022 * b / (2 * airspeed) * state2.ang_rate_z -
0.00339 * aileron_input.aileron_deflection),
(state2, wind, 1.0, aileron_input,
0.001052911121301 + 0.017 * b / (2 * airspeed) * state2.ang_rate_x +
-0.049 * b / (2 * airspeed) * state2.ang_rate_z -
0.00339 * aileron_input.aileron_deflection)
]
def test_elevator_deflection():
control_input = ControlInput()
elevator_deflection = 0.2
control_input.elevator_deflection = elevator_deflection
assert control_input.elevator_deflection == pytest.approx(
elevator_deflection)
assert np.allclose(control_input.control_input,
[elevator_deflection, 0.0, 0.0, 0.0])
def test_aileron_deflection():
control_input = ControlInput()
aileron_deflection = 0.2
control_input.aileron_deflection = aileron_deflection
assert control_input.aileron_deflection == pytest.approx(
aileron_deflection)
assert np.allclose(control_input.control_input,
[0.0, aileron_deflection, 0.0, 0.0])
def test_aileron_elevator2elevon():
control_input = ControlInput()
test_vec1 = np.zeros(2)
test_vec2 = np.ones(2)
val1 = control_input.aileron_elevator2elevon(test_vec1)
val2 = control_input.aileron_elevator2elevon(test_vec2)
expect1 = np.zeros(2)
expect2 = np.array([0, 2])
assert np.allclose(val1, expect1)
assert np.allclose(val2, expect2)
def main():
# Initialize gain scheduled controller
# Using two simple P-controllers, see also fcat.longitudinal_controller for more complicated examples
# Note that this is just an example on how to build and simulate an aircraft system
# The controllers are simple not tuned to get desired reference tracking
controllers = [
SaturatedStateSpaceMatricesGS(
A = -1*np.eye(1),
B = np.zeros((1,1)),
C = np.eye(1),
D = np.zeros((1,1)),
upper = 1,
lower = -1,
switch_signal = 0.3
),
SaturatedStateSpaceMatricesGS(
A = -1*np.eye(1),
B = np.zeros((1,1)),
C = np.eye(1),
D = np.zeros((1,1)),
upper = 1,
lower = -1,
switch_signal = 0.6
)
]
# Using similar controllers for simplicity
lat_gs_controller = init_gs_controller(controllers, Direction.LATERAL, 'icing')
lon_gs_controller = init_gs_controller(controllers, Direction.LONGITUDINAL, 'icing')
airspeed_pi_controller = init_airspeed_controller()
control_input = ControlInput()
control_input.throttle = 0.5
state = State()
state.vx = 20
prop = IcedSkywalkerX8Properties(control_input)
aircraft_model = build_nonlin_sys(prop, no_wind(), outputs=State.names+['icing', 'airspeed'])
motor_time_constant = 0.001
elevon_time_constant = 0.001
actuator_model = build_flying_wing_actuator_system(elevon_time_constant, motor_time_constant)
closed_loop = build_closed_loop(actuator_model, aircraft_model, lon_gs_controller, lat_gs_controller, airspeed_pi_controller)
X0 = get_init_vector(closed_loop, control_input, state)
constant_input = np.array([20, 0.04, -0.00033310605950459315])
sim_time = 15
t = np.linspace(0, sim_time, sim_time*5, endpoint=True)
u = np.array([constant_input, ]*(len(t))).transpose()
T, yout_non_lin, _ = input_output_response(
closed_loop, U=u, T=t, X0=X0, return_x=True, method='BDF')
def test_from_dict_ctrl_vect():
dct = {
'elevator_deflection': 0.5,
'aileron_deflection': 0.2,
'rudder_deflection': 0,
'throttle': 0.5
}
control_input = ControlInput.from_dict(dct)
cotnrol_input_expect = np.array([0.5, 0.2, 0, 0.5])
assert np.allclose(control_input.control_input, cotnrol_input_expect)
def pitch_moment_coeff_test_cases():
state1 = State()
state1.vx = 20.0
state1.vz = 0.0
wind1 = np.zeros(6)
wind2 = np.zeros(6)
wind2[0] = 1.0
wind2[2] = -19.0 * np.tan(6 * np.pi / 180.0)
zero_input = ControlInput()
elevator_input = ControlInput()
elevator_input.elevator_deflection = 2.0 * np.pi / 180.0
return [(state1, wind1, 0.0, zero_input, 0.001161535578433),
(state1, wind1, 1.0, zero_input, -0.004297270367523),
(state1, wind2, 0.0, zero_input, -0.064477317568596),
(state1, wind2, 1.0, zero_input, -0.01808304889307),
(state1, wind1, 0.0, elevator_input,
0.001161535578433 - 0.206 * elevator_input.elevator_deflection),
(state1, wind1, 1.0, elevator_input,
-0.004297270367523 - 0.206 * elevator_input.elevator_deflection)]
def dynamics_kinetmatics_update(t: float, x: np.ndarray, u: np.ndarray, params: dict) -> np.ndarray:
prop = params['prop']
wind = params['wind'].get(t)
prop_updater = params.get('prop_updater', None)
state = State(init=x)
# wind_translational = inertial2body(wind[:3], state)
# wind = np.array([wind_translational[0], wind_translational[1], wind_translational[2], 0, 0, 0])
# Update the control inputs
prop.control_input = ControlInput(init=u)
if prop_updater is not None:
prop.update_params(prop_updater.get_param_dict(t))
update = np.zeros_like(x)
V_a = np.sqrt(np.sum(calc_airspeed(state, wind)**2))
b = prop.wing_span()
S = prop.wing_area()
c = prop.mean_chord()
S_prop = prop.propeller_area()
C_prop = prop.motor_efficiency_fact()
k_motor = prop.motor_constant()
qS = 0.5*AIR_DENSITY*S*V_a**2
force_aero_wind_frame = qS*np.array([-prop.drag_coeff(state, wind),
prop.side_force_coeff(state, wind),
-prop.lift_coeff(state, wind)])
force_aero_body_frame = wind2body(force_aero_wind_frame, state, wind)
moment_coeff_vec = np.array([b*prop.roll_moment_coeff(state, wind),
c*prop.pitch_moment_coeff(state, wind),
b*prop.yaw_moment_coeff(state, wind)])
moment_vec = qS*moment_coeff_vec
omega = np.array([state.ang_rate_x, state.ang_rate_y, state.ang_rate_z]) - wind[3:]
velocity = np.array([state.vx, state.vy, state.vz]) - wind[:3]
gravity_body_frame = inertial2body([0.0, 0.0, prop.mass()*GRAVITY_CONST], state)
F_propulsion = 0.5*AIR_DENSITY*S_prop*C_prop * \
np.array([(k_motor*prop.control_input.throttle)**2-V_a**2, 0, 0])
v_dot = (force_aero_body_frame + gravity_body_frame + F_propulsion) / \
prop.mass() - np.cross(omega, velocity)
# Velocity update
update[StateVecIndices.V_X:StateVecIndices.V_Z+1] = v_dot
# Momentum equations
omega_dot = \
prop.inv_inertia_matrix().dot(moment_vec - np.cross(omega, prop.inertia_matrix().dot(omega)))
update[StateVecIndices.ANG_RATE_X:StateVecIndices.ANG_RATE_Z+1] = omega_dot
# Kinematics
# Position updates
update[StateVecIndices.X:StateVecIndices.Z +
1] = body2inertial([state.vx, state.vy, state.vz], state)
# Angle updates
update[StateVecIndices.ROLL:StateVecIndices.YAW +
1] = body2euler([state.ang_rate_x, state.ang_rate_y, state.ang_rate_z], state)
return update
def drag_coeff_test_cases():
state1 = State()
state1.vx = 20.0
state1.vz = 0.0
wind1 = np.zeros(6)
wind2 = np.zeros(6)
wind2[0] = 1.0
wind2[2] = -19.0 * np.tan(6 * np.pi / 180.0)
zero_input = ControlInput()
elevator_input = ControlInput()
elevator_input.elevator_deflection = 2.0 * np.pi / 180.0
return [(state1, wind1, 0.0, zero_input, 0.015039166436721),
(state1, wind1, 1.0, zero_input, 0.043224285541117),
(state1, wind2, 0.0, zero_input, 0.027939336576642),
(state1, wind2, 1.0, zero_input, 0.082879203470842),
(state1, wind1, 0.0, elevator_input,
0.015039166436721 + 0.0633 * elevator_input.elevator_deflection),
(state1, wind1, 1.0, elevator_input,
0.043224285541117 + 0.0633 * elevator_input.elevator_deflection)]
def lift_coeff_test_cases():
constants = SkywalkerX8Constants()
c = constants.mean_chord
state1 = State()
state1.vx = 20.0
state1.vz = 0.0
state2 = State()
state2.vx = 20.0
state2.vz = 0.0
state2.ang_rate_y = 5 * np.pi / 180
wind1 = np.zeros(6)
wind2 = np.zeros(6)
wind3 = np.zeros(6)
wind2[0] = 1.0
wind2[2] = -19.0 * np.tan(8 * np.pi / 180.0)
wind3[0] = 1.0
wind3[2] = -19.0 * np.tan(8 * np.pi / 180.0)
wind3[4] = 3 * np.pi / 180
ang_rate_y2 = state2.ang_rate_y - wind3[4]
airspeed2 = np.sqrt(np.sum(calc_airspeed(state2, wind2)**2))
zero_input = ControlInput()
elevator_input = ControlInput()
elevator_input.elevator_deflection = 2.0 * np.pi / 180.0
return [(state1, wind1, 0.0, zero_input, 0.030075562375465),
(state1, wind1, 1.0, zero_input, 0.018798581619545),
(state1, wind2, 0.0, zero_input, 0.609296679062686),
(state1, wind2, 1.0, zero_input, 0.454153721254944),
(state1, wind1, 0.0, elevator_input,
0.030075562375465 + 0.278 * elevator_input.elevator_deflection),
(state1, wind1, 1.0, elevator_input,
0.018798581619545 + 0.278 * elevator_input.elevator_deflection),
(state2, wind3, 0.0, elevator_input,
0.609296679062686 + 4.60 * c / (2 * airspeed2) * ang_rate_y2 +
0.278 * elevator_input.elevator_deflection),
(state2, wind3, 1.0, elevator_input,
0.454153721254944 - 3.51 * c / (2 * airspeed2) * ang_rate_y2 +
0.278 * elevator_input.elevator_deflection)]
def test_elevon2aileron_elevator():
control_input = ControlInput()
test_vec1 = np.zeros(2)
control_input.control_input[:2] = test_vec1
val1 = control_input.elevon2aileron_elevator(test_vec1)
test_vec2 = np.ones(2)
control_input.control_input[:2] = test_vec2
val2 = control_input.elevon2aileron_elevator(test_vec2)
expect1 = np.zeros(2)
expect2 = np.array([1, 0])
assert np.allclose(val1, expect1)
assert np.allclose(val2, expect2)
def test_interconnected_system():
control_input = ControlInput()
control_input.throttle = 0.5
wind_model = no_wind()
state = State()
state.vx = 20
prop = IcedSkywalkerX8Properties(control_input)
outputs = [
"x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz", "ang_rate_x",
"ang_rate_y", "ang_rate_z"
]
aircraft_model = build_nonlin_sys(prop, wind_model, outputs)
initial_control_input_state = ControlInput()
initial_control_input_state.throttle = 0.4
motor_time_constant = 0.001
elevon_time_constant = 0.001
actuator_model = build_flying_wing_actuator_system(elevon_time_constant,
motor_time_constant)
x0 = np.concatenate(
(initial_control_input_state.control_input, state.state))
connected_system = add_actuator(actuator_model, aircraft_model)
t = np.linspace(0.0, 0.5, 10, endpoint=True)
u = np.array([
control_input.control_input,
] * len(t)).transpose()
T, yout_without_actuator = input_output_response(aircraft_model,
t,
U=u,
X0=state.state)
T, yout_with_actuator = input_output_response(connected_system,
t,
U=u,
X0=x0)
assert np.allclose(yout_with_actuator[6, :],
yout_without_actuator[6, :],
atol=5.e-3)
assert np.allclose(yout_with_actuator[7, :],
yout_without_actuator[7, :],
atol=5.e-3)
assert np.allclose(yout_with_actuator[8, :],
yout_without_actuator[8, :],
atol=5.e-3)
assert np.allclose(yout_with_actuator[9, :],
yout_without_actuator[9, :],
atol=5.e-3)
assert np.allclose(yout_with_actuator[10, :],
yout_without_actuator[10, :],
atol=5.e-3)
assert np.allclose(yout_with_actuator[11, :],
yout_without_actuator[11, :],
atol=5.e-3)
def test_flying_wing_setters():
control_input = ControlInput()
throttle = 1
elevon_left = 2
elevon_right = 1
elevon_vec = np.array([elevon_right, elevon_left])
control_input.elevon_left = elevon_left
control_input.elevon_right = elevon_right
control_input.throttle = throttle
expect_throttle = 1
expect_elev_ail = control_input.elevon2aileron_elevator(elevon_vec)
expect_control_input = np.array(
[expect_elev_ail[0], expect_elev_ail[1], 0, expect_throttle])
assert np.allclose(control_input.control_input, expect_control_input)
def linearize_system(infile: str) -> str:
with open(infile) as f:
data = load(f)
aircraft = aircraft_property_from_dct(data['aircraft'])
ctrl = ControlInput.from_dict(data['init_control'])
state = State.from_dict(data['init_state'])
config_dict = {
"outputs": [
"x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz",
"ang_rate_x", "ang_rate_y", "ang_rate_z"
]
}
sys = build_nonlin_sys(aircraft, no_wind(), config_dict['outputs'], None)
actuator = actuator_from_dct(data['actuator'])
xeq = state.state
ueq = ctrl.control_input
aircraft_with_actuator = add_actuator(actuator, sys)
states_lin = np.concatenate((ueq, xeq))
linearized_sys = aircraft_with_actuator.linearize(states_lin, ueq)
A, B, C, D = ssdata(linearized_sys)
linsys = StateSpaceMatrices(A, B, C, D)
return linsys
def test_dynamics_forces():
control_input = ControlInput()
prop = SimpleTestAircraftNoMoments(control_input)
t = 0
for i in range(-50, 101, 50):
control_input.throttle = 0.8
control_input.elevator_deflection = i
control_input.aileron_deflection = i
control_input.rudder_deflection = i
state = State()
state.vx = 20.0
state.vy = 1
state.vz = 0
params = {"prop": prop, "wind": no_wind()}
update = dynamics_kinetmatics_update(t=t,
x=state.state,
u=control_input.control_input,
params=params)
V_a = np.sqrt(np.sum(calc_airspeed(state, params['wind'].get(0.0))**2))
forces_aero_wind_frame = np.array([
-np.abs(control_input.elevator_deflection),
control_input.aileron_deflection, -control_input.rudder_deflection
])
forces_aero_body_frame = wind2body(forces_aero_wind_frame, state,
params['wind'].get(0))
force_propulsion = np.array([(2 * control_input.throttle)**2 - V_a**2,
0, 0])
force_gravity = inertial2body(
np.array([0, 0, prop.mass() * GRAVITY_CONST]), state)
forces_body = forces_aero_body_frame + force_propulsion + force_gravity
vx_update_expect = (1 / prop.mass()) * forces_body[0]
vy_update_expect = (1 / prop.mass()) * forces_body[1]
vz_update_expect = (1 / prop.mass()) * forces_body[2]
# No moments
ang_rate_x_update_expect = 0
ang_rate_y_update_expect = 0
ang_rate_z_update_expect = 0
assert np.allclose(vx_update_expect, update[6])
assert np.allclose(vy_update_expect, update[7])
assert np.allclose(vz_update_expect, update[8])
assert np.allclose(ang_rate_x_update_expect, update[9])
assert np.allclose(ang_rate_y_update_expect, update[10])
assert np.allclose(ang_rate_z_update_expect, update[11])
def test_flying_wing_get():
control_input = ControlInput()
throttle = 0.2
aileron = 0.2
elevator = 0.2
control_input.throttle = throttle
control_input.aileron = aileron
control_input.elevator = elevator
expect_elevon = control_input.aileron_elevator2elevon(
control_input.control_input[:2])
expect_flying_wing_input = np.array(
[expect_elevon[0], expect_elevon[1], throttle])
assert control_input.throttle == pytest.approx(throttle)
assert np.allclose(
np.array([
control_input.elevon_right, control_input.elevon_left,
control_input.throttle
]), expect_flying_wing_input)
def test_actuator_model():
control_input = ControlInput()
control_input.elevon_right = 2
control_input.elevon_left = 3
control_input.throttle = 0.5
control_input.rudder = 0
initial_value_elevator = 3
initial_value_aileron = 2
initial_value_throttle = 2
initial_values = np.array([
initial_value_elevator, initial_value_aileron, 0,
initial_value_throttle
])
initial_values_flying_wing = np.linalg.inv(
flying_wing2ctrl_input_matrix()).dot(initial_values)
initial_value_elevon_right = initial_values_flying_wing[0]
initial_value_elevon_left = initial_values_flying_wing[1]
initial_value_throttle = initial_values_flying_wing[3]
elevon_time_constant = 0.3
motor_time_constant = 0.2
lin_model = build_flying_wing_actuator_system(elevon_time_constant,
motor_time_constant)
t = np.linspace(0.0, 10, 500, endpoint=True)
u = np.array([
control_input.control_input,
] * len(t)).transpose()
T, yout = input_output_response(lin_model, t, U=u, X0=initial_values)
yout = np.linalg.inv(flying_wing2ctrl_input_matrix()).dot(yout)
expect_elevon_r = control_input.elevon_right + \
(initial_value_elevon_right - control_input.elevon_right)*np.exp(-t/elevon_time_constant)
expect_elevon_l = control_input.elevon_left + \
(initial_value_elevon_left - control_input.elevon_left)*np.exp(-t/elevon_time_constant)
expect_motor = control_input.throttle + \
(initial_value_throttle - control_input.throttle)*np.exp(-t/motor_time_constant)
assert np.allclose(expect_elevon_r, yout[0, :], atol=5e-3)
assert np.allclose(expect_elevon_l, yout[1, :], atol=5e-3)
assert np.allclose(expect_motor, yout[3, :], atol=5e-3)
def test_dynamics_moments():
control_input = ControlInput()
t = 0
for i in range(-50, 51, 50):
control_input.throttle = i
control_input.elevator_deflection = i
control_input.aileron_deflection = i
control_input.rudder_deflection = i
prop = SimpleTestAircraftNoForces(control_input)
state = State()
state.vx = 20.0
state.vy = 1
state.vz = 0
state.ang_rate_x = 0.157079633
state.ang_rate_y = 0.157079633
state.ang_rate_z = 0.157079633
params = {"prop": prop, "wind": no_wind()}
update = dynamics_kinetmatics_update(t=t,
x=state.state,
u=control_input.control_input,
params=params)
moments_aero = np.array([
control_input.elevator_deflection,
control_input.aileron_deflection, control_input.rudder_deflection
])
omega = np.array(
[state.ang_rate_x, state.ang_rate_y, state.ang_rate_z])
coreolis_term = prop.inv_inertia_matrix().dot(
np.cross(omega,
prop.inertia_matrix().dot(omega)))
ang_rate_x_update_expect = (2 / 3) * moments_aero[0] - (
1 / 3) * moments_aero[2] - coreolis_term[0]
ang_rate_y_update_expect = (1 / 2) * moments_aero[1] - coreolis_term[1]
ang_rate_z_update_expect = (2 / 3) * moments_aero[2] - (
1 / 3) * moments_aero[0] - coreolis_term[2]
assert np.allclose(ang_rate_x_update_expect, update[9])
assert np.allclose(ang_rate_y_update_expect, update[10])
assert np.allclose(ang_rate_z_update_expect, update[11])
def main():
# Script showing how to simulate icedskywalkerX8 from config_file using controllers saved in file
config_file = "examples/skywalkerX8_linearize.yml"
lat_controller_file = "examples/inner_loop_controllers/single_robust_roll_ctrl.json"
lon_controller_file = "examples/inner_loop_controllers/single_robust_pitch_ctrl.json"
K_lat = get_state_space_from_file(lat_controller_file)
K_lon = get_state_space_from_file(lon_controller_file)
K_lat = SaturatedStateSpaceController(A=np.array(K_lat.A),
B=np.array(K_lat.B),
C=np.array(K_lat.C),
D=np.array(K_lat.D),
lower=-0.4,
upper=0.4)
K_lon = SaturatedStateSpaceController(A=np.array(K_lon.A),
B=np.array(K_lon.B),
C=np.array(K_lon.C),
D=np.array(K_lon.D),
lower=-0.4,
upper=0.4)
# Using similar controllers for simplicity
lat_controller = init_robust_controller(K_lat, Direction.LATERAL)
lon_controller = init_robust_controller(K_lon, Direction.LONGITUDINAL)
airspeed_pi_controller = init_airspeed_controller()
with open(config_file, 'r') as infile:
data = load(infile)
aircraft = aircraft_property_from_dct(data['aircraft'])
ctrl = ControlInput.from_dict(data['init_control'])
state = State.from_dict(data['init_state'])
updates = {
'icing': [PropUpdate(0.5, 0.5),
PropUpdate(5.0, 1.0)],
}
updater = PropertyUpdater(updates)
aircraft_model = build_nonlin_sys(aircraft,
no_wind(),
outputs=State.names +
['icing', 'airspeed'],
prop_updater=updater)
actuator = actuator_from_dct(data['actuator'])
closed_loop = build_closed_loop(actuator, aircraft_model, lon_controller,
lat_controller, airspeed_pi_controller)
X0 = get_init_vector(closed_loop, ctrl, state)
constant_input = np.array([20, 0.2, -0.2])
sim_time = 15
t = np.linspace(0, sim_time, sim_time * 5, endpoint=True)
u = np.array([
constant_input,
] * (len(t))).transpose()
T, yout_non_lin, _ = input_output_response(closed_loop,
U=u,
T=t,
X0=X0,
return_x=True,
method='BDF')
plot_respons(T, yout_non_lin)
plt.show()
def test_throttle():
control_input = ControlInput()
throttle = 0.2
control_input.throttle = throttle
assert control_input.throttle == pytest.approx(throttle)
assert np.allclose(control_input.control_input, [0.0, 0.0, 0.0, throttle])
def linearize(config: str,
out: str,
template: bool = False,
trim: bool = False):
"""
Linearize the model specified via the config file
"""
if template:
generate_linearize_template()
return
with open(config, 'r') as infile:
data = load(infile)
aircraft = aircraft_property_from_dct(data['aircraft'])
ctrl = ControlInput.from_dict(data['init_control'])
state = State.from_dict(data['init_state'])
iu = [2, 3]
config_dict = {
"outputs": [
"x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz",
"ang_rate_x", "ang_rate_y", "ang_rate_z"
]
}
sys = build_nonlin_sys(aircraft, no_wind(), config_dict['outputs'], None)
idx = [2, 6, 7, 8, 9, 10, 11]
y0 = state.state
iy = [0, 1, 2, 5, 9, 10, 11]
xeq = state.state
ueq = ctrl.control_input
if trim:
print("Finding equillibrium point...")
xeq, ueq = find_eqpt(sys,
state.state,
u0=ctrl.control_input,
idx=idx,
y0=y0,
iy=iy,
iu=iu)
print("Equillibrium point found")
print()
print("Equilibrium state vector")
print(f"x: {xeq[0]: .2e} m, y: {xeq[1]: .2e} m, z: {xeq[2]: .2e} m")
print(
f"roll: {rad2deg(xeq[3]): .1f} deg, pitch: {rad2deg(xeq[4]): .1f} deg"
f", yaw: {rad2deg(xeq[5]): .1f} deg")
print(
f"vx: {xeq[6]: .2e} m/s, vy: {xeq[7]: .2e} m/s, vz: {xeq[8]: .2e} m/s"
)
print(
f"Ang.rates: x: {rad2deg(xeq[9]): .1f} deg/s, y: {rad2deg(xeq[10]): .1f} deg/s"
f", z: {rad2deg(xeq[11]): .1f} deg/s")
print()
print("Equilibrium input control vector")
print(
f"elevator: {rad2deg(ueq[0]): .1f} deg, aileron: {rad2deg(ueq[1]): .1f} deg"
f", rudder: {rad2deg(ueq[2]): .1f} deg, throttle: {ueq[3]: .1f}")
print()
actuator = actuator_from_dct(data['actuator'])
if isinstance(actuator, InputOutputSystem):
aircraft_with_actuator = add_actuator(actuator, sys)
states_lin = np.concatenate((ueq, xeq))
linearized = aircraft_with_actuator.linearize(states_lin, ueq)
else:
linearized = sys.linearize(xeq, ueq)
print("Linearization finished")
A, B, C, D = ssdata(linearized)
print("Found linear state space model on the form")
print()
print(" dx ")
print(" ---- = Ax + Bu")
print(" dt ")
print()
print("With observarion equation")
print()
print("y = Cx + Du")
print()
print("Eigenvalues of A:")
eig = np.linalg.eigvals(A)
print('\n'.join(f'{x:.2e}' for x in eig))
linsys = {
'A': A.tolist(),
'B': B.tolist(),
'C': C.tolist(),
'D': D.tolist(),
'xeq': xeq.tolist(),
'ueq': ueq.tolist()
}
if out is not None:
with open(out, 'w') as outfile:
json.dump(linsys, outfile, indent=2, sort_keys=True)
print(f"Linear model written to {out}")
def test_kinematics():
control_input = ControlInput()
prop = FrictionlessBall(control_input)
outputs = [
"x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz", "ang_rate_x",
"ang_rate_y", "ang_rate_z"
]
system = build_nonlin_sys(prop, no_wind(), outputs)
t = np.linspace(0.0, 10, 500, endpoint=True)
state = State()
state.vx = 20.0
state.vy = 1
for i in range(3):
state.roll = 0
state.pitch = 0
state.yaw = 0
if i == 0:
state.ang_rate_x = 0.157079633
state.ang_rate_y = 0
state.ang_rate_z = 0
elif i == 1:
state.ang_rate_x = 0
state.ang_rate_y = 0.157079633
state.ang_rate_z = 0
elif i == 2:
state.ang_rate_x = 0
state.ang_rate_y = 0
state.ang_rate_z = 0.157079633
T, yout = input_output_response(system, t, U=0, X0=state.state)
pos = np.array(yout[:3])
eul_ang = np.array(yout[3:6])
vel_inertial = np.array(
[np.zeros(len(t)),
np.zeros(len(t)),
np.zeros(len(t))])
for j in range(len(vel_inertial[0])):
state.roll = yout[3, j]
state.pitch = yout[4, j]
state.yaw = yout[5, j]
vel = np.array([yout[6, j], yout[7, j], yout[8, j]])
vel_inertial_elem = body2inertial(vel, state)
vel_inertial[0, j] = vel_inertial_elem[0]
vel_inertial[1, j] = vel_inertial_elem[1]
vel_inertial[2, j] = vel_inertial_elem[2]
vx_inertial_expect = 20 * np.ones(len(t))
vy_inertial_expect = 1 * np.ones(len(t))
vz_inertial_expect = GRAVITY_CONST * t
x_expect = 20 * t
y_expect = 1 * t
z_expect = 0.5 * GRAVITY_CONST * t**2
roll_expect = state.ang_rate_x * t
pitch_expect = state.ang_rate_y * t
yaw_expect = state.ang_rate_z * t
assert np.allclose(vx_inertial_expect, vel_inertial[0], atol=7e-3)
assert np.allclose(vy_inertial_expect, vel_inertial[1], atol=5e-3)
assert np.allclose(vz_inertial_expect, vel_inertial[2], atol=8e-3)
assert np.allclose(x_expect, pos[0], atol=8e-2)
assert np.allclose(y_expect, pos[1], atol=5e-2)
assert np.allclose(z_expect, pos[2], atol=7e-2)
assert np.allclose(roll_expect, eul_ang[0], atol=1e-3)
assert np.allclose(pitch_expect, eul_ang[1], atol=1e-3)
assert np.allclose(yaw_expect, eul_ang[2], atol=1e-3)
