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 trim(s0, x: f16.State, p: f16.Parameters, phi_dot: float, theta_dot: float,
psi_dot: float, gam: float):
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
s = ca.MX.sym('s', 6)
x, u = constrain(x, s)
f = f16.trim_cost(f16.dynamics(x, u, p))
nlp = {'x': s, 'f': f}
S = ca.nlpsol('S', 'ipopt', nlp, {'ipopt': {
'print_level': 0,
}})
r = S(x0=s0, lbg=0, ubg=0)
s_opt = r['x']
x, u = constrain(x, s_opt)
return x, u
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_jacobian():
x_sym = ca.MX.sym('x', 16)
u_sym = ca.MX.sym('u', 4)
x = f16.State.from_casadi(x_sym)
u = f16.Control.from_casadi(u_sym)
p = f16.Parameters()
dx = f16.dynamics(x, u, p)
A = ca.jacobian(dx.to_casadi(), x_sym)
B = ca.jacobian(dx.to_casadi(), u_sym)
f_A = ca.Function('A', [x_sym, u_sym], [A])
f_B = ca.Function('B', [x_sym, u_sym], [B])
print('A', f_A(np.ones(16), np.ones(4)))
print('B', f_B(np.ones(16), np.ones(4)))
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 simulate(x0, u0, t0, tf, dt):
xs = ca.MX.sym('x', 13)
x = f16.State.from_casadi(xs)
us = ca.MX.sym('u', 4)
u = f16.Control.from_casadi(us)
dae = {'x': xs, 'p': us, 'ode': f16.dynamics(x, u, p).to_casadi()}
F = ca.integrator('F', 'idas', dae, {'t0': 0, 'tf': dt, 'jit': True})
x = np.array(x0.to_casadi()).reshape(-1)
u = np.array(u0.to_casadi()).reshape(-1)
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
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
