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')])
