6-DOF Interceptor (TC.ST.NW)

def rhs(t, y):
    x, yp, z = y[0], y[1], y[2]
    u, v, w = y[3], y[4], y[5]
    phi, theta, psi = y[6], y[7], y[8]
    p, q, r = y[9], y[10], y[11]
    de, da, dr, dc = y[12], y[13], y[14], y[15]

    d = np.empty(16)

    cphi, sphi = np.cos(phi), np.sin(phi)
    cth, sth = np.cos(theta), np.sin(theta)
    cpsi, spsi = np.cos(psi), np.sin(psi)

    V_body = np.sqrt(max(u**2 + v**2 + w**2, 1.0))
    alpha = np.arctan2(w - wind_z, max(abs(u - wind_x), 1.0))
    beta = np.arcsin(np.clip(v / V_body, -0.99, 0.99))
    qbar = 0.5 * _RHO * V_body**2

    CL = CL_alpha * alpha
    CD = CD0 + CD_alpha2 * alpha**2
    L = qbar * S_ref * CL
    D = qbar * S_ref * CD
    Y = qbar * S_ref * 0.5 * beta

    # Body-frame aero forces
    X_aero = -D * np.cos(alpha) + L * np.sin(alpha)
    Y_aero = -Y
    Z_aero = -D * np.sin(alpha) - L * np.cos(alpha)

    # Moments
    L_aero = qbar * S_ref * 0.3 * (Cl_delta_a * da + Cl_p * p * 0.3 / max(V_body, 1.0))
    M_aero = qbar * S_ref * 0.3 * (Cm_alpha * alpha + Cm_delta_e * de + Cm_q * q * 0.3 / max(V_body, 1.0))
    N_aero = qbar * S_ref * 0.3 * (Cn_beta * beta + Cn_delta_r * dr + Cn_r * r * 0.3 / max(V_body, 1.0))

    # Translational dynamics (body frame)
    d[3] = X_aero / m - (q * w - r * v) - _G * sth
    d[4] = Y_aero / m - (r * u - p * w) + _G * cth * sphi
    d[5] = Z_aero / m - (p * v - q * u) + _G * cth * cphi

    # Rotational dynamics
    d[9]  = (L_aero - (Izz - Iyy) * q * r) / Ixx
    d[10] = (M_aero - (Ixx - Izz) * p * r) / Iyy
    d[11] = (N_aero - (Iyy - Ixx) * p * q) / Izz

    # Euler angle kinematics
    sec_th = 1.0 / max(abs(cth), 0.01) * np.sign(cth) if abs(cth) < 0.01 else 1.0 / cth
    d[6] = p + (q * sphi + r * cphi) * sth * sec_th
    d[7] = q * cphi - r * sphi
    d[8] = (q * sphi + r * cphi) * sec_th

    # Position (NED)
    d[0] = cth * cpsi * u + (sphi * sth * cpsi - cphi * spsi) * v + (cphi * sth * cpsi + sphi * spsi) * w
    d[1] = cth * spsi * u + (sphi * sth * spsi + cphi * cpsi) * v + (cphi * sth * spsi - sphi * cpsi) * w
    d[2] = -sth * u + sphi * cth * v + cphi * cth * w

    # Guidance: PN towards target
    tgt_x = tgt_x0 + V_tgt * np.cos(omega_tgt * t) * t
    tgt_y = tgt_y0 + V_tgt * np.sin(omega_tgt * t) * t
    tgt_z = tgt_z0
    dx_t = tgt_x - x
    dy_t = tgt_y - yp
    dz_t = tgt_z - z
    R_los = np.sqrt(dx_t**2 + dy_t**2 + dz_t**2 + 1.0)
    los_el = np.arcsin(np.clip(-dz_t / R_los, -0.99, 0.99))
    los_az = np.arctan2(dy_t, dx_t + 1e-10)

    # LOS error with rate limiting to prevent singularity in crossing geometry
    t_go = max(R_los / max(V_body, 1.0), 0.1)
    los_rate_el = np.clip((los_el - theta), -0.5, 0.5) / t_go
    los_rate_az = np.clip((los_az - psi), -0.5, 0.5) / t_go

    # PN acceleration commands (rate-limited)
    a_cmd_z = N_pn * V_body * los_rate_el
    a_cmd_y = N_pn * V_body * los_rate_az
    de_cmd = np.clip(-a_cmd_z / max(abs(Cm_delta_e * qbar * S_ref * 0.3 / Iyy), 0.1), -0.5, 0.5)
    dr_cmd = np.clip(a_cmd_y / max(abs(Cn_delta_r * qbar * S_ref * 0.3 / Izz), 0.1), -0.5, 0.5)

    # Actuator dynamics (1st-order lag)
    d[12] = (de_cmd - de) / tau_act
    d[13] = (0.0 - da) / tau_act  # wings-level
    d[14] = (dr_cmd - dr) / tau_act
    d[15] = (0.0 - dc) / tau_act

    return d