def test_table_3_5_2(): # pg 187 p = f16.Parameters(xcg=0.4) x = f16.State(VT=500, alpha=0.5, beta=-0.2, phi=-1, theta=1, psi=-1, P=0.7, Q=-0.8, R=0.9, p_N=1000, p_E=900, alt=10000) u = f16.Control(thtl=0.9, elv_cmd_deg=20, ail_cmd_deg=-15, rdr_cmd_deg=-20) x = f16.trim_actuators(x, u) x.power = 90 dx = f16.dynamics(x, u, p) dx_compute = np.array(dx.to_casadi())[:, 0] dx_check = np.array([ -75.23724, -0.8813491, -0.4759990, 2.505734, 0.3250820, 2.145926, 12.62679, 0.9649671, 0.5809759, 342.4439, -266.7707, 248.1241, -58.68999, 0, 0, 0 ]) print('\nexpected:\n\t', dx_check) print('\nactual:\n\t', dx_compute) print('\nerror:\n\t', dx_check - dx_compute) assert np.allclose(dx_compute, dx_check, 1e-3)
def test_simulate(): f_control = lambda t, x: f16.Control() f16.simulate(x0=f16.State(VT=502), f_control=f_control, p=f16.Parameters(), t0=0, tf=10, dt=0.01)
def test_trim_computation(): # pg 195 p = f16.Parameters() x = f16.State(VT=502) x0, u0 = f16.trim(x=x, p=p, phi_dot=0, theta_dot=0, psi_dot=0, gam=0) dx = f16.dynamics(x0, u0, p) print(dx) assert f16.trim_cost(dx) < TRIM_TOL
def test_trim3(): # pg 197 p = f16.Parameters(xcg=0.38) x = f16.State(VT=502, alpha=0.03544, theta=0.03544) u = f16.Control(thtl=0.1325, elv_cmd_deg=-0.0559) x = f16.trim_actuators(x, u) dx = f16.dynamics(x, u, p) assert f16.trim_cost(dx) < TRIM_TOL
def test_trim3(): # pg 197 p = f16.Parameters(xcg=0.38) x = f16.State(VT=502, alpha=0.03544, theta=0.03544) u = f16.Control(thtl=0.1325, elv_deg=-0.0559) x.power = f16.tables['tgear'](u.thtl) dx = f16.dynamics(x, u, p) assert f16.trim_cost(dx) < TRIM_TOL
def test_trim1(): # pg 197 p = f16.Parameters() x = f16.State(VT=502, alpha=0.03691, theta=0.03691) u = f16.Control(thtl=0.1385, elv_cmd_deg=-0.7588) x = f16.trim_actuators(x, u) dx = f16.dynamics(x, u, p) print(dx) assert f16.trim_cost(dx) < TRIM_TOL
def test_trim2(): # pg 197 p = f16.Parameters(xcg=0.3) x = f16.State(VT=502, alpha=0.03936, theta=0.03936) u = f16.Control(thtl=0.1485, elv_deg=-1.931) x.power = f16.tables['tgear'](u.thtl) dx = f16.dynamics(x, u, p) print(dx) assert f16.trim_cost(dx) < TRIM_TOL
def test_trim1(): # pg 197 p = f16.Parameters() x = f16.State(VT=502, alpha=0.03691, theta=0.03691) u = f16.Control(thtl=0.1385, elv_deg=-0.7588) x.power = f16.tables['tgear'](u.thtl) dx = f16.dynamics(x, u, p) print(dx) assert f16.trim_cost(dx) < TRIM_TOL
def test_trim_and_linearize(): p = f16.Parameters() x = f16.State(VT=502) x0, u0 = f16.trim(x=x, p=p, phi_dot=0, theta_dot=0, psi_dot=0, gam=0) dx = f16.dynamics(x0, u0, p) assert f16.trim_cost(dx) < TRIM_TOL print(dx) sys = f16.linearize(x0, u0, p) sys.sub_system(['VT', 'elv_deg', 'alpha', 'Q'], ['elv_cmd_deg'], ['alpha', 'Q']) print(sys) ss = sys.to_control()
def test_trim5(): # pg 197 p = f16.Parameters(xcg=0.3) # listed as -0.3, must be typo # theta_dot = 0.3 x = f16.State(VT=502, alpha=0.3006, beta=4.1e-5, theta=0.3006, Q=0.3) u = f16.Control(thtl=1.023, elv_cmd_deg=-7.082, ail_cmd_deg=-6.2e-4, rdr_cmd_deg=0.01655) x = f16.trim_actuators(x, u) dx = f16.dynamics(x, u, p) print(dx) assert f16.trim_cost(dx) < 2e-2 # doesn't converge as close
def test_trim4(): # pg 197 p = f16.Parameters(xcg=0.3) # psi_dot = 0.3 x = f16.State(VT=502, alpha=0.2485, beta=4.8e-4, phi=1.367, theta=0.05185, P=-0.0155, Q=0.2934, R=0.06071) u = f16.Control(thtl=0.8499, elv_cmd_deg=-6.256, ail_cmd_deg=0.09891, rdr_cmd_deg=-0.4218) x = f16.trim_actuators(x, u) dx = f16.dynamics(x, u, p) print(dx) assert f16.trim_cost(dx) < TRIM_TOL
def test_trim6(): # pg 195 p = f16.Parameters() x = f16.State(VT=502, alpha=2.392628e-1, beta=5.061803e-4, phi=1.366289, theta=5.000808e-2, psi=2.340769e-1, P=-1.499617e-2, Q=2.933811e-1, R=6.084932e-2, p_N=0, p_E=0, alt=0, power=6.412363e1) u = f16.Control(thtl=8.349601e-1, elv_deg=-1.481766, ail_deg=9.553108e-2, rdr_deg=-4.118124e-1) dx = f16.dynamics(x, u, p) print(dx) assert f16.trim_cost(dx) < TRIM_TOL
data = {'t': [0], 'x': [x]} t_vect = np.arange(t0, tf, dt) for t in t_vect: x = np.array(F(x0=x, p=u)['xf']).reshape(-1) data['t'].append(t) data['x'].append(x) for k in data.keys(): data[k] = np.array(data[k]) return data # %% start = time.time() p = f16.Parameters() x0, u0 = trim(s0=[0, 0, 0, 0, 0, 0], x=f16.State(VT=500, alt=5000), p=p, phi_dot=0, theta_dot=0, psi_dot=0.2, gam=0) print('trim computation time', time.time() - start) # %% start = time.time() data = simulate(x0, u0, 0, 10, 0.01) print('sim computation time', time.time() - start) state_index = f16.State().name_to_index plt.figure() plt.plot(data['x'][:, state_index('p_E')], data['x'][:, state_index('p_N')])