Exemplo n.º 1
0
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
Exemplo n.º 2
0
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()
Exemplo n.º 3
0
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}")