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

# Trim and Stability Analysis

From [Part 2](f16_02.md) we have a nonlinear state-space model of F-16 flight dynamics in the form

```{math}
\dot{x} = f(t, x, u)
```

In Part 3 of this tutorial series on F-16 flight dynamics modeling we will take the the next step in a typical control systems development workflow; finding and analyzing the stability of trim conditions.
The technical details in this part will be largely self-contained, but we will skip derivations wherever possible (and recommend referring to ["Aircraft Control and Simulation"](https://doi.org/10.1002/9781119174882) by Stevens, Lewis, and Johnson for details).
The source code is available on [GitHub](https://github.com/PineTreeLabs/archimedes/tree/main/docs/source/tutorials/f16/).

The interface of our flight dynamics model is roughly:

```python
@struct
class SubsonicF16:

    @struct
    class State(RigidBody.State):
        pos: np.ndarray  # North-East-Down position [ft]
        att: Attitude  # quaternion or RPY attitude
        v_B: np.ndarray  # Body-frame (FRD) velocity [ft/s]
        w_B: np.ndarray  # Body-frame angular velocity [rad/s]
        eng: F16Engine.State  # Engine state (may be empty)
        elevator: Actuator.State  # Actuator state (may be empty)
        aileron: Actuator.State  # Actuator state (may be empty)
        rudder: Actuator.State  # Actuator state (may be empty)

    @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 net_forces(self, t, x: State, u: Input) -> tuple[np.ndarray, np.ndarray]:
        """Net forces and moments in body frame B"""

    def dynamics(self, t, x: State, u: Input) -> State:
        """Compute time derivative of the state"""

```

As an example of how to interact with the model, take the following reference time derivative evaluation from Stevens, Lewis, and Johnson (3.5-2):

```{code-cell} python
:tags: [remove-cell]
# ruff: noqa: N806, N816
from pathlib import Path

from archimedes.docs.utils import extract_py_class, extract_py_function

plot_dir = Path.cwd() / "_plots"
plot_dir.mkdir(exist_ok=True)
```

```{code-cell} python
:tags: [hide-cell]
from pprint import pprint

import control
import matplotlib.pyplot as plt
import numpy as np
import yaml
from f16 import GRAV_FTS2, SubsonicF16
from stability import (
    LateralInput,
    LateralState,
    LongitudinalInput,
    LongitudinalState,
    StabilityState,
)

import archimedes as arc
from archimedes.spatial import Quaternion

# Truncate NumPy print options for readability
np.set_printoptions(precision=4, suppress=True)
```

```{code-cell} python
model = SubsonicF16(xcg=0.4)

u = model.Input(
    throttle=0.9,
    elevator=20.0,
    aileron=-15.0,
    rudder=-20.0,
)

# Textbook state used (Vt, alpha, beta) = (500.0, 0.5, -0.2)
# Our model uses equivalent (u, v, w) = (430.0447, -99.3347, 234.9345)
#   --> (du, dv, dw) = 100.8536, -218.3080, -437.0399
pos = np.array([1000.0, 900.0, -10000.0])  # NED-frame position
rpy = np.array([-1.0, 1.0, -1.0])  # Roll, pitch, yaw
v_B = np.array([430.0447, -99.3347, 234.9345])  # Velocity in body frame
w_B = np.array([0.7, -0.8, 0.9])  # Angular velocity in body frame

att = Quaternion.from_euler(rpy)
x_eng = model.engine.State(90.0)  # Set Engine power
x = model.State(pos, att, v_B, w_B, eng=x_eng)

x_t = model.dynamics(0.0, x, u)
pprint(x_t)
```

## Finding Trim Conditions

There are various ways to define "trim" conditions for an aircraft.
We will define it as a (state, controls) pair such that the **time derivative of body-frame velocity and angular velocity are zero**:

```{math}
\frac{dv^B}{dt} = 0, \qquad \frac{d\omega^B}{dt} = 0.
```

Importantly, this means that the angular velocity may be nonzero, and hence the time derivative of _inertial_ velocity may also be nonzero.
An example of this is a coordinated turn maneuver, which is not "steady" in any traditional dynamical systems sense.

The body-frame velocity and angular velocity are the "dynamics" subset of the full vehicle state $\mathbf{x}$, so we could write the condition above in shorthand as:

```{math}
0 = \begin{bmatrix}
\frac{dv^B}{dt} \\
\frac{d\omega^B}{dt}
\end{bmatrix}
\equiv f_\mathrm{dyn}(t, x, u).
```

The value of $f_\mathrm{dyn}(t, x, u)$ is our "trim residual" - we will need this to be identically zero to find a trim condition.

We will additionally specify several desired characteristics of the trim point:

- True airspeed $V_T$
- Altitude $-z$
- Flight path (climb) angle $\gamma$
- Roll, pitch, and yaw (turn) rates $\dot{phi}$, $\dot{\theta}$, $\dot{\psi}$

Mathematically, what we are doing is essentially a root-finding problem (though it could alternatively formulated as weighted least-squares or a more general constrained nonlinear program).
Given the trim condition above as input, we want to find state and input variables such that the "quasi-steady" condition above holds.

We will use the following as our "decision variables" for this optimization problem:

- Angle of attack $\alpha$
- Sideslip angle $\beta$
- Throttle level $\delta_T$
- Control surface deflections $\delta_e$, $\delta_a$, $\delta_r$

This gives us six optimization parameters for six dynamic residuals - exactly what we need for a Newton-type root-finding solver.

The logical flow for the optimization problem is to take the inputs and optimization parameters, derive a full vehicle state and inputs, and then evaluate the trim residual.
That is, if our input conditions are $c$ and our optimization parameters are $p$ we need to define functions $x = x(c, p)$ and $u = u(c, p)$.
The the full root-finding problem is

```{math}
\mathrm{given} ~ c, ~~ \mathrm{find} ~ p ~~ \textrm{such that} ~~ 0 = f_\mathrm{dyn}(t, x(c, p), u(c, p))
```

The inputs $u(c, p)$ are the easy part, since all of the flight control inputs are part of the optimization parameters already.
That leaves the function $x(c, p)$ which constructs a full vehicle state from the trim condition and optimization parameters.

This actually may be easier to understand in code than on paper:

```{code-cell} python
:tags: [remove-input]
extract_py_class("trim.py", "TrimCondition")
extract_py_class("trim.py", "TrimVariables")
extract_py_function("trim.py", "trim_residual")
```

The function `trim_state` is $x(c, p)$, which we have not defined yet

### Deriving the Trim State

There are three states we don't care about, as they play no role in the dynamics: North-East positions, and yaw angle.
The vertical position is given as part of the input conditions (negative altitude).
"Auxiliary" states like the engine and control surface actuators have their own `.trim()` methods built-in, so we will assume that the subsystem models can trim themselves independently of the vehicle dynamics.

This means that the main work in `trim_state` is deriving the body-frame velocity and angular velocity, and the pitch and roll angles.

The pitch and roll angles are the most complex - these have to be chosen carefully to ensure coordination between the velocity and angular velocity vectors.
This is mainly a kinematic problem and doesn't depend on the details of the flight vehicle itself.

The condition for the turn constraint that determines roll angle $\phi$ is:

```{math}
\begin{gather}
\tan \phi = \frac{\mathcal{G} \cos \beta \sqrt{(a - b^2) + b \tan \alpha (c(1-b^2) + \mathcal{G}^2 \sin^2(\beta))} }{\cos\alpha(a^2 - b^2(1 + c \tan^2 \alpha))} \\
\mathcal{G} = \dot{\psi} V_t / g_0 \\
a = 1 - \mathcal{G} \tan \alpha \sin \beta \\
b = \sin \gamma / \cos \beta \\
c = 1 + \mathcal{G}^2 \cos^2 \beta
\end{gather}
```

Once $\phi$ is known, the pitch angle $\theta$ can be determined with the rate-of-climb constraint:

```{math}
\begin{gather}
\tan \theta = \frac{a b + \sin \gamma \sqrt{a^2 + b^2 - \sin^2 \gamma}}{a^2 - 2 \sin^2 \gamma} \\
a = \cos \alpha \cos \beta \\
b = \sin \phi \sin \beta + \cos \phi \sin \alpha \cos \beta \\
\end{gather}
```

Now we have a full roll-pitch-yaw attitude specification and Euler angle rates (given as inputs), and we can use the [inverse Euler kinematic equation](#archimedes.spatial.euler_kinematics) to solve for the corresponding body-frame angular velocity:

```{math}
\omega^B = H^{-1}(\Phi) \dot{\Phi}
```

Finally, body-frame velocity is simply the rotation of the wind-frame velocity $v^W = \begin{bmatrix}V_T & 0 & 0\end{bmatrix}^T$ into the body frame using the aerodynamic angles $(\alpha, \beta)$:

```{math}
v^B = R_{BW}(\alpha, \beta) ~ v^W.
```

Here's the full implementation:

```{code-cell} python
:tags: [remove-input, hide-cell]
extract_py_function("trim.py", "trim_state")
```

This looks a little hairy, but it maps cleanly to the math - better yet, it doesn't depend on the details of the model, so the same code can be reused across a wide variety of flight vehicles.

### Solving the Trim Problem

At this point the hard work is done.
The `trim` function itself essentially just packs the inputs into a struct, calls [`root`](#archimedes.root) to set the optimization parameters, and returns the results for further analysis:

```{code-cell} python
:tags: [remove-input, hide-cell]
extract_py_class("trim.py", "TrimPoint")
extract_py_function("trim.py", "trim")
```

Now we can use this to solve for a variety of trim conditions, e.g. steady level flight at 10,000 ft:

```{code-cell} python
model = SubsonicF16(xcg=0.35)

result = model.trim(vt=500.0, alt=10000.0, gamma=0.0)
pprint(result.variables)
```

```{code-cell} python
:tags: [remove-cell]
assert np.linalg.norm(result.residuals) < 1e-8
```

or a constant turn of 0.1 rad/s:

```{code-cell} python
result = model.trim(vt=500.0, turn_rate=0.1, alt=10000.0, gamma=0.0)
pprint(result.variables)
```

```{code-cell} python
:tags: [remove-cell]
assert np.linalg.norm(result.residuals) < 1e-8
```

## Stability Analysis

Now that we can identify trimmed conditions, the next natural question is one of stability.
In principle, if we set the controls to their trimmed points and reach the right $(V_T, \alpha, \beta)$, the aircraft will keep going as intended.
But what happens if we perturb away from the trimmed condition?

For a general nonlinear state-space model of the form

```{math}
\begin{align}
\dot{x} &= f(x, u) \\
y &= h(x, u),
\end{align}
```

which is how our model is expressed, for some nominal condition $(x_0, u_0)$ and perturbations $(\delta x, \delta u)$, we can do a Taylor expansion:

```{math}
\begin{align}
\dot{\delta x} &= f(x_0, u_0) + A \delta x + B \delta u \\
\delta y &= h(x_0, u_0) + C \delta x + D \delta u,
\end{align}
```

where $(A, B, C, D)$ are the Jacobian matrices evaluated at the nominal condition.

The eigenvalues/vectors of $A$ and the poles and zeros of the transfer function $G(s) = C(sI - A)^{-1}B + D$ then give insight into what happens when we perturb away from the trim condition.

### Model Decomposition

In our case the full state has 14 elements (13 for a `RigidBody` with quaternion kinematics, plus one extra for the engine state).
However, a number of these are irrelevant to stability analysis - North-East position and yaw play no role in the dynamics, and altitude is only weakly involved (through the atmosphere).
Moreover, for small perturbations it is more convenient and intuitive to represent the dynamics with Euler angles than a quaternion (unless we're linearizing at extreme pitch angles).
Finally, we will assume the engine response is fast enough to have a negligible effect on the overall stability.

Removing these from consideration, we're left with 8 elements in the state vector: body-frame velocity and angular velocity, plus roll and pitch angles.
These can be represented in various ways; here we'll use $(V_T, \alpha, \beta)$ for velocity and Euler angles for attitude.
This truncated state can be split into two weakly-coupled blocks (almost completely decoupled, in steady level flight):

|        | Longitudinal             | Lateral-Directional |
|--------|--------------------------|---------------------|
| States | Airspeed $V_T$           | Sideslip $\beta$    |
|        | Angle of Attack $\alpha$ | Roll Angle $\phi$   |
|        | Pitch Angle $\theta$     | Roll Rate $p$       |
|        | Pitch Rate $q$           | Yaw Rate $r$        |
| Inputs | Throttle $\delta_T$      | Aileron $\delta_a$  |
|        | Elevator $\delta_e$      | Rudder $\delta_r$   |

To derive decoupled linearized models, the general procedure is to define a function that evaluates the derivatives of the reduced state.
For example, to evaluate the longitudinal derivatives, this function must:

1. Expand the longitudinal state into a full vehicle state, filling in lateral-directional values from the trim condition
2. Evaluate the time derivative of the full vehicle state
3. Extract the longitudinal components of the full time derivative

Then this function must be linearized about the longitudinal trim condition.

Typically, this process involves manually managing entries of flat vectors (e.g. `x_lon[0] = x_full[6]` for airspeed) and using careful finite differencing for the Jacobian.
In our case we can lean on `struct` representations of the state and flatten/unflatten as needed, so we can define structured types for the reduced states.

The general interface used for our tutorial is:

```python
@struct
class LongitudinalState:
    vt: float
    alpha: float
    theta: float
    q: float

    @classmethod
    def from_full_state(cls, x: SubsonicF16.State) -> LongitudinalState:
        """Extract longitudinal components of the full state"""
    
    def as_full_state(self) -> SubsonicF16.State:
        """Expand the reduced state into a full state"""


@struct
class LongitudinalInput:
    throttle: float
    elevator: float


@struct
class LateralState:
    beta: float
    phi: float
    p: float
    r: float

    # ... similar from_, as_ methods


@struct
class LateralInput:
    aileron: float
    rudder: float


@struct
class StabilityState:
    lon: LongitudinalState
    lat: LateralState

    @classmethod
    def from_full_state(cls, x: SubsonicF16.State) -> StabilityState:
        """Extract the truncated stability state variables"""

    def as_full_state(self) -> SubsonicF16.State:
        """Expand the truncated state to a full nonlinear vehicle state"""

    @classmethod
    def from_full_derivative(
        cls, x: SubsonicF16.State, x_dot: SubsonicF16.State
    ) -> StabilityState:
        """Compute the time derivatives of truncated stability state variables"""


@struct
class StabilityInput:
    lon: LongitudinalInput
    lat: LateralInput
```

Once again, this is much more verbose than the simpler manual array management approach, and may not be well suited for all applications.
That said, it does tend to be more scalable and maintainable to have the dependencies and data flow be explicit without things like hardcoded indexing.

### Longitudinal Stability

With this interface, we can define abstracted reduced dynamics and output functions:

```{code-cell} python
:tags: [hide-cell]
# Use simplified model (no actuator or engine dynamics)
with open("config.yaml", "r") as f:
    config = yaml.safe_load(f)

model = SubsonicF16.from_yaml("config.yaml", "stability")

# Trim for steady level flight
result = model.trim(vt=502.0, pitch_rate=0.0)

x0 = LongitudinalState.from_full_state(result.state)
x0_lat = LateralState.from_full_state(result.state)
u0 = LongitudinalInput.from_full_input(result.inputs)

x0_flat, unravel_x = arc.tree.ravel(x0)
u0_flat, unravel_u = arc.tree.ravel(u0)


def unravel_lon(x_flat, u_flat):
    """Unpack the flattened longitudinal arrays into full vehicle data"""
    x_stab = StabilityState(lon=unravel_x(x_flat), lat=x0_lat)
    u_stab = unravel_u(u_flat)
    x_full = x_stab.as_full_state()
    u_full = u_stab.as_full_input()
    return x_full, u_full


def lon_dynamics(t, x_flat, u_flat):
    """Evaluate the time derivative of the flattened longitudinal state"""
    x_full, u_full = unravel_lon(x_flat, u_flat)
    x_dot_full = model.dynamics(t, x_full, u_full)

    x_dot = StabilityState.from_full_derivative(x_full, x_dot_full)
    x_dot_flat, _ = arc.tree.ravel(x_dot.lon)

    return x_dot_flat


def lon_output(t, x_flat, u_flat):
    """Output normal acceleration, pitch rate, and angle of attack"""
    x_full, u_full = unravel_lon(x_flat, u_flat)

    x_stab = unravel_x(x_flat)
    q = x_stab.q
    alpha = np.rad2deg(x_stab.alpha)  # Angle of attack [deg]

    # Normal acceleration [g's]
    F_net_B, _ = model.net_forces(t, x_full, u_full)
    F_grav_B = model.calc_gravity(x_full)
    a_n = -(F_net_B[2] - F_grav_B[2]) / (model.m * GRAV_FTS2)

    return np.hstack([a_n, q, alpha])
```

Then the linearization is as simple as calling the [`jac`](#archimedes.jac) for automatic differentiation:

```{code-cell} python
A, B = arc.jac(lon_dynamics, argnums=(1, 2))(0.0, x0_flat, u0_flat)
C, D = arc.jac(lon_output, argnums=(1, 2))(0.0, x0_flat, u0_flat)

input_labels = [r"$\delta_t$", r"$\delta_e$"]
output_labels = [r"$a_n$", r"$q$", r"$\alpha$"]
sys = control.ss(
    A, B, C, D, inputs=input_labels, outputs=output_labels, name="longitudinal"
)
sys
```

Now we can assess the well-known modes of the system:

```{code-cell} python
evals = np.linalg.eigvals(A)

phugoid_idx = np.argmax(evals.real)
short_idx = np.argmax(evals.imag)

print(
    f"Phugoid mode:\t\t{evals[phugoid_idx].real:.4f} ± {evals[phugoid_idx].imag:.4f}j"
)
print(f"Short period mode:\t{evals[short_idx].real:.4f} ± {evals[short_idx].imag:.4f}j")
```

```{code-cell} python
:tags: [remove-cell]
# Stevens, Lewis, Johnson values:
slj_phugoid = -0.0087 + 0.0739j
slj_short = -1.2039 + 1.4922j

assert np.allclose(evals[phugoid_idx], slj_phugoid, rtol=1e-3)
assert np.allclose(evals[short_idx], slj_short, rtol=1e-3)
```

and its open-loop behavior:

```{code-cell} python
:tags: [hide-cell, remove-output]
fig, ax = plt.subplots(2, 2, figsize=(10, 4))
control.bode_plot(sys, overlay_outputs=True, ax=ax, dB=True, Hz=True)
plt.show()
```

```{code-cell} python
:tags: [remove-cell]

for theme in {"light", "dark"}:
    arc.theme.set_theme(theme)

    fig, ax = plt.subplots(2, 2, figsize=(10, 5))
    control.bode_plot(sys, overlay_outputs=True, ax=ax, dB=True, Hz=True)

    plt.savefig(plot_dir / f"f16_03_0_{theme}.png")
    plt.close()
```

```{image} _plots/f16_03_0_light.png
:class: only-light
```

```{image} _plots/f16_03_0_dark.png
:class: only-dark
```

### Lateral-directional Stability

The lateral-directional analysis proceeds almost identically:

```{code-cell} python
:tags: [hide-cell]
x0 = LateralState.from_full_state(result.state)
x0_lon = LongitudinalState.from_full_state(result.state)
u0 = LateralInput.from_full_input(result.inputs)

x0_flat, unravel_x = arc.tree.ravel(x0)
u0_flat, unravel_u = arc.tree.ravel(u0)


def unravel_lat(x_flat, u_flat):
    x_stab = StabilityState(lon=x0_lon, lat=unravel_x(x_flat))
    u_stab = unravel_u(u_flat)
    x_full = x_stab.as_full_state()
    u_full = u_stab.as_full_input()
    return x_full, u_full


def lat_dynamics(t, x_flat, u_flat):
    x_full, u_full = unravel_lat(x_flat, u_flat)
    x_dot_full = model.dynamics(t, x_full, u_full)

    x_dot = StabilityState.from_full_derivative(x_full, x_dot_full)

    x_dot_flat, _ = arc.tree.ravel(x_dot.lat)
    return x_dot_flat


A, B = arc.jac(lat_dynamics, argnums=(1, 2))(0.0, x0_flat, u0_flat)
evals, evecs = np.linalg.eig(A)

dutch_idx = np.where(evals.imag != 0)[0][0]
spiral_idx = np.argmax(evals.real)
roll_idx = np.argmin(evals.real)

print(f"Dutch roll:\t{evals[dutch_idx].real:.4f} ± {evals[dutch_idx].imag:.4f}j")
print(f"Spiral mode:\t\t{evals[spiral_idx].real:.4f}")
print(f"Roll subsidence:\t{evals[roll_idx].real:.4f}")
```

```{code-cell} python
:tags: [remove-cell]
# Stevens, Lewis, Johnson values:
slj_dutch = -0.4399 + 3.220j
slj_spiral = -0.0128
slj_roll = -3.601

assert np.allclose(evals[dutch_idx], slj_dutch, rtol=5e-3)
assert np.allclose(evals[spiral_idx], slj_spiral, rtol=5e-3)
assert np.allclose(evals[roll_idx], slj_roll, rtol=5e-3)
```

These linearized systems are the typical starting point for control systems design, since they provide relatively small, intuitive models for cascaded control and various CAS/SAS/autopilot loops.