---
jupytext:
  text_representation:
    extension: .md
    format_name: myst
kernelspec:
  display_name: Python 3
  language: python
  name: archimedes
---

# Porting Legacy Code

The classic reference code for the subsonic F-16 is ["Aircraft Control and Simulation"](https://doi.org/10.1002/9781119174882) by Stevens, Lewis, and Johnson.
This textbook includes detailed explanations of the conventions used in the model and interpretations of the results, along with Fortran source code for the entire model.

:::{note}
In keeping with the conventions used by the textbook (and because the units are not clearly documented in some places in the original code), here we depart from the typical Archimedes convention of SI units and use the imperial system.  Please raise an [issue](https://github.com/PineTreeLabs/archimedes/issues) or [discussion](https://github.com/PineTreeLabs/archimedes/discussions) on GitHub if there is anything unclear.
:::

Other similar implementations based on the same NASA data include [F16Model.jl](https://github.com/isrlab/F16Model.jl/tree/master) and [AeroBenchVV](https://github.com/pheidlauf/AeroBenchVV).

## The Monolithic Approach

Here's what a direct port of the original Fortran code to Python looks like.
Note that here the aerodynamic and propulsion lookup tables are imported from separate files to reduce clutter; see [GitHub](https://github.com/PineTreeLabs/archimedes/tree/main/docs/source/tutorials/f16/) for the full source code.

```{code-cell} python
:tags: [remove-cell]
# ruff: noqa: N803, N806
```

```{code-cell} python
import numpy as np
from aero import (
    calc_damping,
    cl_interpolant,
    cm_interpolant,
    cn_interpolant,
    cx_interpolant,
    cz_interpolant,
    dlda,
    dldr,
    dnda,
    dndr,
)
from engine import Tidl_interpolant, Tmax_interpolant, Tmil_interpolant


def adc(Vt, alt):
    """Air data computer: standard atmosphere model"""
    R0 = 2.377e-3  # Density scale [slug/ft^3]
    gamma = 1.4  # Adiabatic index for air [-]
    Rs = 1716.3  # Specific gas constant for air [ft·lbf/slug-R]
    Tfac = 1 - 0.703e-5 * alt  # Temperature factor
    T = np.where(alt > 35000, 390.0, 519.0 * Tfac)
    rho = R0 * Tfac**4.14
    amach = Vt / np.sqrt(gamma * Rs * T)
    qbar = 0.5 * rho * Vt**2
    return amach, qbar


def tgear(thtl):
    """Commmand power [lbf] as a function of throttle level [0-1]"""
    return np.where(thtl <= 0.77, 64.94 * thtl, 217.38 * thtl - 117.38)


def rtau(dp):
    """Inverse engine time constant"""
    return np.where(
        dp <= 25,
        1.0,
        np.where(dp >= 50, 0.1, 1.9 - 0.036 * dp),
    )


def pdot(p, cpow):
    """Time derivative of engine power pdot(pow, cpow)"""
    p2 = np.where(
        cpow >= 50.0,
        np.where(p >= 50.0, cpow, 60.0),
        np.where(p >= 50.0, 40.0, cpow),
    )

    t = np.where(p >= 50.0, 5.0, rtau(p2 - p))
    pd = t * (p2 - p)
    return pd


def thrust(pow, alt, rmach):
    T_mil = Tmil_interpolant(alt, rmach)
    T_idl = Tidl_interpolant(alt, rmach)
    T_max = Tmax_interpolant(alt, rmach)

    return np.where(
        pow < 50.0,
        T_idl + (T_mil - T_idl) * pow * 0.02,
        T_mil + (T_max - T_mil) * (pow - 50.0) * 0.02,
    )


def cx(alpha, el):
    """Body-frame x-force (forward) coefficient"""
    return cx_interpolant(alpha, el)


def cy(beta, ail, rdr):
    """Body-frame y-force (right) coefficient"""
    return -0.02 * beta + 0.021 * (ail / 20) + 0.086 * (rdr / 30)


def cz(alpha, beta, el):
    """Body-frame z-force (down) coefficient"""
    cz_lookup = cz_interpolant(alpha)
    return (-0.19 / 25) * el + cz_lookup * (1.0 - (beta / 57.3) ** 2)


def cl(alpha, beta):
    """Roll moment coefficient"""
    return np.where(
        beta > 0,
        cl_interpolant(alpha, beta),
        -cl_interpolant(alpha, -beta),
    )


def cm(alpha, el):
    """Pitch moment coefficient"""
    return cm_interpolant(alpha, el)


def cn(alpha, beta):
    """Yaw moment coefficient"""
    return np.where(
        beta > 0,
        cn_interpolant(alpha, beta),
        -cn_interpolant(alpha, -beta),
    )


def f16_dynamics(x, controls, xcg=0.35, weight=20490.4459):
    """Subsonic F-16 dynamics model

    State vector: [vt, alpha, beta, phi, theta, psi, p, q, r, pn, pe, alt, pow]
    Input vector: [thtl, el, ail, rdr]
    """
    thtl, el, ail, rdr = controls
    xd = np.zeros_like(x)

    Axx = 9496.0
    Ayy = 55814.0
    Azz = 63100.0
    Axz = 982.0
    Axzs = Axz**2
    xpq = Axz * (Axx - Ayy + Azz)
    gam = Axx * Azz - Axz**2
    xqr = Azz * (Azz - Ayy) + Axzs
    zpq = (Axx - Ayy) * Axx + Axzs
    ypr = Azz - Axx
    gd = 32.17
    mass = weight / gd

    s = 300.0
    b = 30.0
    cbar = 11.32
    xcgr = 0.35  # Reference CG location
    hx = 160.0
    rtod = 57.29578

    # Assign state & control variables
    vt = x[0]
    alpha = x[1]
    beta = x[2]

    cbta = np.cos(beta)
    u = vt * np.cos(alpha) * cbta
    v = vt * np.sin(beta)
    w = vt * np.sin(alpha) * cbta

    alpha_deg = alpha * rtod
    beta_deg = beta * rtod
    phi = x[3]
    theta = x[4]
    psi = x[5]
    p = x[6]
    q = x[7]
    r = x[8]
    alt = x[11]
    pow = x[12]

    # Air data computer and engine model
    amach, qbar = adc(vt, alt)
    cpow = tgear(thtl)
    xd[12] = pdot(pow, cpow)
    t = thrust(pow, alt, amach)

    # Lookup tables and component buildup
    cxt = cx(alpha_deg, el)
    cyt = cy(beta_deg, ail, rdr)
    czt = cz(alpha_deg, beta_deg, el)
    dail = ail / 20.0
    drdr = rdr / 30.0
    clt = (
        cl(alpha_deg, beta_deg)
        + dlda(alpha_deg, beta_deg) * dail
        + dldr(alpha_deg, beta_deg) * drdr
    )
    cmt = cm(alpha_deg, el)
    cnt = (
        cn(alpha_deg, beta_deg)
        + dnda(alpha_deg, beta_deg) * dail
        + dndr(alpha_deg, beta_deg) * drdr
    )

    # Add damping derivatives
    tvt = 0.5 / vt
    b2v = b * tvt
    cq = cbar * q * tvt
    d = calc_damping(alpha_deg)
    cxt = cxt + cq * d[0]
    cyt = cyt + b2v * (d[1] * r + d[2] * p)
    czt = czt + cq * d[3]

    clt = clt + b2v * (d[4] * r + d[5] * p)
    cmt = cmt + cq * d[6] + czt * (xcgr - xcg)
    cnt = cnt + b2v * (d[7] * r + d[8] * p) - cyt * (xcgr - xcg) * cbar / b

    # Prep for state equations
    sth = np.sin(theta)
    cth = np.cos(theta)
    sph = np.sin(phi)
    cph = np.cos(phi)
    spsi = np.sin(psi)
    cpsi = np.cos(psi)
    qs = qbar * s
    qsb = qs * b
    rmqs = qs / mass
    gcth = gd * cth
    qsph = q * sph
    ay = rmqs * cyt
    az = rmqs * czt

    # Force equations
    udot = r * v - q * w - gd * sth + (qs * cxt + t) / mass
    vdot = p * w - r * u + gcth * sph + ay
    wdot = q * u - p * v + gcth * cph + az

    dum = u * u + w * w
    xd[0] = (u * udot + v * vdot + w * wdot) / vt
    xd[1] = (u * wdot - w * udot) / dum
    xd[2] = (vt * vdot - v * xd[0]) * cbta / dum

    # kinematics
    xd[3] = p + (sth / cth) * (qsph + r * cph)
    xd[4] = q * cph - r * sph
    xd[5] = (qsph + r * cph) / cth

    # Moments
    roll = qsb * clt
    pitch = qs * cbar * cmt
    yaw = qsb * cnt
    pq = p * q
    qr = q * r
    qhx = q * hx
    xd[6] = (xpq * pq - xqr * qr + Azz * roll + Axz * (yaw + qhx)) / gam
    xd[7] = (ypr * p * r - Axz * (p**2 - r**2) + pitch - r * hx) / Ayy
    xd[8] = (zpq * pq - xpq * qr + Axz * roll + Axx * (yaw + qhx)) / gam

    # navigation
    t1 = sph * cpsi
    t2 = cph * sth
    t3 = sph * spsi
    s1 = cth * cpsi
    s2 = cth * spsi
    s3 = t1 * sth - cph * spsi
    s4 = t3 * sth + cph * cpsi
    s5 = sph * cth
    s6 = t2 * cpsi + t3
    s7 = t2 * spsi - t1
    s8 = cph * cth
    xd[9] = u * s1 + v * s3 + w * s6  # north speed
    xd[10] = u * s2 + v * s4 + w * s7  # east speed
    xd[11] = u * sth - v * s5 - w * s8  # vertical speed

    return xd
```

This implementation combines models for atmosphere, gravity, aerodynamics, propulsion, and rigid body dynamics into one "monolithic" ~150 line function (plus several "subroutines" for auxiliary computations).

A few details on porting the Fortran code:

* In keeping with the [control flow](../../control-flow.md) requirements of symbolically traced code, all `if`/`else` conditionals need to be converted to `np.where`
* The $C_L$ and $C_N$ lookup tables are symmetric in sideslip angle; a natural way to implement this would be `np.sign(beta) * interpolant(alpha, abs(beta))`.  This produces the correct answer, but is not differentiable at `beta=0`, causing problems with stability analysis.  Instead, `np.where` can safely handle the symmetry.

## Running the model

What we have implemented here in the `f16_dynamics` function is a standard nonlinear state-space model.
If we've defined a controller (or set the controls to be constant) we can simulate this with our usual ODE solvers:

```python
def controller(t, x):
    return u0  # Or feedback control, planned trajectory, etc.

def ode_rhs(t, x):
    u = controller(t, x)
    return f16_dynamics(x, u)

xs = arc.odeint(ode_rhs, (t0, tf), x0_flat, t_eval=ts)
```

Here `x` and `u` are flat vectors with 13 and 4 entries, respectively, as defined in the docstring of `f16_dynamics`.

## Monolithic vs Hierarchical

The main advantages to the "monolithic" approach are that it is self-contained, enabling quick iteration in very early stages of model development - there's no need to sit down and map out a comprehensive system architecture before the model structure has crystallized.
It is also easy to port legacy codes that have already been written this way.
For those reasons alone, this is a perfectly valid way to build dynamics models in Archimedes.

However, there are also some disadvantages to the "monolithic" implementation, namely:

1. It doesn't clearly delineate distinct physics (gravity vs aerodynamics, for instance)
2. The monolithic code is difficult to test, debug, and validate against subsystem test data
3. We can't take advantage of reusable components like rigid body dynamics, gravity, and atmosphere models.
4. It's difficult to replace subsystem models without modifying the main dynamics model
5. As the model grows in complexity, the flat state and input vectors become more difficult to manually manipulate

It's worth mentioning that there's no need to make a straw man out of the textbook Fortran ported directly to Python.
First, even the original code was never presented as a production flight dynamics implementation, but as a pedagogical textbook example.
The various "magic numbers" and hardcoded parameters, while generally not considered good practice today, are understandable in that context.
The modularity and reusability of this code (points 1-3 above) could largely be addressed through classic "single responsibility" scoping, breaking the monolithic dynamics function into a series of smaller functions responsible for computing gravity, engine thrust, aerodynamic coefficients, etc.

Second, the quirks of Archimedes with respect to [control flow](../../control-flow.md) mean that the legible `if/else` patterns from the original Fortran are replaced with the equivalent but less obvious `np.where` in `adc`, `tgear`, `rtau`, and `pdot`.
In short, this is because the truth evaluation in Python `if/else` statements happens at "compile time" and therefore cannot be used to build up symbolic computation graphs, while `np.where` dispatches to a symbolic operation (CasADi's `if_else` function) which can.
The upshot is that some of these subroutines are actually harder to read in this Python implementation than in the original Fortran (or vanilla MATLAB/Python).

What we will do in [Part 2](f16_02.md) is fully rewrite the code while keeping the dynamics model essentially identical.
The result is something along the lines of:

```python
@.struct
class SubsonicF16:
    rigid_body: RigidBody
    gravity: GravityModel
    atmos: AtmosphereModel
    engine: F16Engine
    aero: F16Aerodynamics

    m: float  # Vehicle mass [slug]
    J_B: np.ndarray # Vehicle inertia matrix [slug·ft²]

    @struct
    class State(RigidBody.State):
        engine: F16Engine.State

    @struct
    class Input:
        throttle: float  # Throttle command [0-1]
        elevator: float  # Elevator deflection [deg]
        aileron: float  # Aileron deflection [deg]
        rudder: float  # Rudder deflection [deg]

    def dynamics(self, t, x: State, u: Input) -> State:
        ...
```

This breaks up the generic rigid body dynamics, the vehicle-specific aerodynamics, reusable models for atmosphere and gravity, and a modular engine component.

This is generally easier to test and debug, more scalable to complex dynamics models, and allows you to swap subsystem models easily (e.g. `ConstantAtmosphere` for dynamic inversion, `USSA1976` for Monte Carlo tests).

The stateis a hierarchical `struct` object, so for instance the body-frame velocity vector can be accessed by `x.v_B` instead of indexing a flat vector.
In conjunction with the modular subsystem approach, this means that it's easy to swap component models with different state representations; the basic engine model supplied by NASA uses a single lag state, but you could use a simpler lookup table approximation (in which case `F16Engine.State` would be `None`), or a physics-based turbofan model (in which case `F16Engine.State` would be another nested `struct`).

With the hierarchical approach, replacing the `engine` component with either of these does not change the top-level vehicle dynamics code at all.
In any of these cases you can expect to pass a scalar throttle value, the dynamic engine state, and get back the thrust and time derivative of the engine state - regardless of how the engine state is defined.

As we'll see, there's a similar story with actuator models.
For some applications it is sufficient to assume ideal actuation (instantaneously reaching the command position), while in others it might be crucial to resolve lags and rate limits.
If we design the model with generic interfaces to these subsystems we can easily reconfigure for different kinds of analysis.

## Validation

To verify that this implementation is correct we can check against some of the textbook results from Stevens, Lewis, and Johnson.
These tests are basically a spot check; the hierarchical model is [unit tested more substantially](https://github.com/PineTreeLabs/archimedes/tree/main/docs/source/tutorials/f16/test_f16.py).

<!-- Unit tests -->
```{code-cell} python
:tags: [hide-cell]

# From Stevens, Lewis, Johnson Table 3.5-2
def test_352():
    u = np.array([0.9, 20.0, -15.0, -20.0])
    x = np.array(
        [
            500.0,
            0.5,
            -0.2,
            -1.0,
            1.0,
            -1.0,
            0.7,
            -0.8,
            0.9,
            1000.0,
            900.0,
            10000.0,
            90.0,
        ]
    )

    # NOTE: There is a typo in the chapter 3 code implementation of the DCM,
    # leading to a sign change for yaw rate xd[11].  Hence, Table 3.5-2 has
    # 248.1241 instead of -248.1241 (the latter is consistent with the SciPy
    # DCM implementation).
    xd_expected = 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,
        ]
    )

    xd = f16_dynamics(x, u, xcg=0.4)

    assert np.allclose(xd, xd_expected, atol=1e-2)
    print("Test 3.5.2:")
    print("  Reference:\t\t✓")


# From Stevens, Lewis, Johnson Table 3.6-2
def test_362():
    vt = 5.020000e2
    alpha = 2.392628e-1
    beta = 5.061803e-4

    thtl = 8.349601e-1
    el = -1.481766e0
    ail = 9.553108e-2
    rdr = -4.118124e-1

    x = np.array(
        [
            vt,
            alpha,
            beta,
            1.366289e0,
            5.000808e-2,
            2.340769e-1,
            -1.499617e-2,
            2.933811e-1,
            6.084932e-2,
            0.0,
            0.0,
            0.0,
            6.412363e1,
        ]
    )

    u = np.array([thtl, el, ail, rdr])

    xd = f16_dynamics(x, u, xcg=0.35)
    zero_idx = [0, 1, 2, 6, 7, 8]
    print("\nTest 3.6")
    assert np.allclose(xd[zero_idx], 0.0, atol=1e-4)
    print("  Trimmed:\t\t✓")
    assert np.allclose(xd[3], 0.0, atol=1e-4)
    print("  Roll rate:\t\t✓")
    assert np.allclose(xd[4], 0.0, atol=1e-4)
    print("  Pitch rate:\t\t✓")
    assert np.allclose(xd[5], 0.3, atol=1e-4)
    print("  Turn rate:\t\t✓")

    # Turn coordination when flight path angle is zero
    phi = x[3]
    gd = 32.17
    G = xd[5] * vt / gd
    tph1 = np.tan(phi)
    tph2 = G * np.cos(beta) / (np.cos(alpha) - G * np.sin(alpha) * np.sin(beta))
    assert np.allclose(tph1, tph2, atol=1e-4)
    print("  Turn coord:\t\t✓")


# Trim conditions (3.6-3)
def test_363():
    vt = 5.020000e2
    alpha = 0.03691
    beta = -4e-9

    thtl = 0.1385  # Throttle setting [0-1]
    el = -0.7588  # Elevator angle [deg]
    ail = -1.2e-7  # Aileron angle [deg]
    rdr = -6.2e-7  # Rudder angle [deg]

    x = np.array(
        [
            vt,
            alpha,
            beta,
            0.0,
            0.03691,  # pitch [rad]
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            0.0,
            tgear(thtl),
        ]
    )

    u = np.array([thtl, el, ail, rdr])

    xd = f16_dynamics(x, u, xcg=0.35)
    zero_idx = [0, 1, 2, 6, 7, 8]

    print(xd)

    print("\nTest 3.6-3:")
    assert np.allclose(xd[zero_idx], 0.0, atol=1e-3)
    print("  Trimmed:\t\t✓")


test_352()
test_362()
test_363()
```