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_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_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 constrain(x, s):
u = f16.Control(thtl=s[0], elv_deg=s[1], ail_deg=s[2], rdr_deg=s[3])
alpha = s[4]
beta = s[5]
x.alpha = alpha
x.beta = beta
cos = ca.cos
sin = ca.sin
tan = ca.tan
atan = ca.arctan
sqrt = ca.sqrt
VT = x.VT
g = p.g
G = psi_dot * VT / g
a = 1 - G * tan(alpha) * sin(beta)
b = sin(gam) / cos(beta)
c = 1 + G**2 * cos(beta)**2
# coordinated turn constraint pg. 188
phi = atan(G * cos(beta) / cos(alpha) *
((a - b**2) +
b * tan(alpha) * sqrt(c *
(1 - b**2) + G**2 * sin(beta)**2)) /
(a**2 - b**2 * (1 + c * tan(alpha)**2)))
x.phi = phi
# rate of climb constraint pg. 187
a = cos(alpha) * cos(beta)
b = sin(phi) * sin(beta) + cos(phi) * sin(alpha) * cos(beta)
theta = (a*b + sin(gam)*sqrt(a**2 - sin(gam)**2 + b**2)) \
/ (a**2 - sin(gam)**2)
x.theta = theta
# kinematics pg. 20
x.P = phi_dot - sin(theta) * psi_dot
x.Q = cos(phi) * phi_dot + sin(phi) * cos(theta) * psi_dot
x.R = -sin(phi) * theta_dot + cos(phi) * cos(theta) * psi_dot
# engine power constraint
x.power = f16.tables['tgear'](u.thtl)
return x, u
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
