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_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