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 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 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}")