Trajectory Classifier (FST.GNT)

def rhs(t, y):
    d = np.empty(27)

    # Smooth interpolation of noise (avoids piecewise-constant discontinuities)
    n0 = np.interp(t, _noise_ts, cmd_noise[0])
    n1 = np.interp(t, _noise_ts, cmd_noise[1])
    n2 = np.interp(t, _noise_ts, cmd_noise[2])

    # Pseudo-target command (shared input, each model responds differently)
    ax_cmd = maneuver_amp * np.sin(_TWO_PI * maneuver_freq * t) + 0.5 * n0
    ay_cmd = maneuver_amp * np.cos(_TWO_PI * maneuver_freq * t * 0.7) + 0.5 * n1
    az_cmd = 0.5 * n2

    # Model A: Multi-rotor (agile, can hover)
    s = 0
    vx_a, vy_a, vz_a = y[s + 3], y[s + 4], y[s + 5]
    ax_a, ay_a, az_a = y[s + 6], y[s + 7], y[s + 8]
    d[s + 0] = vx_a
    d[s + 1] = vy_a
    d[s + 2] = vz_a
    d[s + 3] = ax_a
    d[s + 4] = ay_a
    d[s + 5] = az_a - _G  # gravity in NED
    d[s + 6] = (ax_cmd - ax_a) / tau_mr
    d[s + 7] = (ay_cmd - ay_a) / tau_mr
    d[s + 8] = (az_cmd - az_a) / tau_mr

    # Model B: Fixed-wing (constrained by stall speed, wider turns)
    s = 9
    vx_b, vy_b, vz_b = y[s + 3], y[s + 4], y[s + 5]
    ax_b, ay_b, az_b = y[s + 6], y[s + 7], y[s + 8]
    V_b = np.sqrt(vx_b**2 + vy_b**2 + vz_b**2 + 0.01)
    stall_factor = np.tanh(max(V_b - V_stall, 0.0) / 5.0)
    d[s + 0] = vx_b
    d[s + 1] = vy_b
    d[s + 2] = vz_b
    d[s + 3] = ax_b
    d[s + 4] = ay_b
    d[s + 5] = az_b - _G
    d[s + 6] = (ax_cmd * stall_factor - ax_b) / tau_fw
    d[s + 7] = (ay_cmd * stall_factor * 0.5 - ay_b) / tau_fw
    d[s + 8] = (az_cmd * 0.3 - az_b) / tau_fw

    # Model C: VTOL hybrid (intermediate response)
    s = 18
    vx_c, vy_c, vz_c = y[s + 3], y[s + 4], y[s + 5]
    ax_c, ay_c, az_c = y[s + 6], y[s + 7], y[s + 8]
    V_c = np.sqrt(vx_c**2 + vy_c**2 + vz_c**2 + 0.01)
    transition = np.clip(V_c / V_cruise, 0.0, 1.0)  # 0=hover, 1=wing-borne
    tau_eff = tau_mr * (1.0 - transition) + tau_fw * transition
    d[s + 0] = vx_c
    d[s + 1] = vy_c
    d[s + 2] = vz_c
    d[s + 3] = ax_c
    d[s + 4] = ay_c
    d[s + 5] = az_c - _G
    d[s + 6] = (ax_cmd * (1.0 - 0.3 * transition) - ax_c) / tau_eff
    d[s + 7] = (ay_cmd * (1.0 - 0.5 * transition) - ay_c) / tau_eff
    d[s + 8] = (az_cmd - az_c) / tau_eff

    return d