6DOF Quadrotor + Dryden Gust Model (17-state)

def _body_forces(T, phi, theta, psi):
    """Thrust vector decomposition in inertial frame."""
    cp, sp = np.cos(phi), np.sin(phi)
    ct, st = np.cos(theta), np.sin(theta)
    cy, sy = np.cos(psi), np.sin(psi)
    Fx = T * (cy * st * cp + sy * sp)
    Fy = T * (sy * st * cp - cy * sp)
    Fz = T * ct * cp
    return Fx, Fy, Fz

def _pseudo_white(t, phases):
    """Sum of sinusoids approximating unit-variance white noise."""
    return _AMPLITUDE * np.sum(np.sin(_FREQ_BASE * t + phases))

def _dryden_filter_rhs(x_filter, t):
    """5-state Dryden forming filter dynamics."""
    dx = np.zeros(5)

    # Pseudo-white noise inputs
    w_u = _pseudo_white(t, _PHASE_U)
    w_v = _pseudo_white(t, _PHASE_V)
    w_w = _pseudo_white(t, _PHASE_W)

    # u_g filter (1st-order): x[0]
    a_u = _V_AIR / _L_U
    b_u = _SIGMA_U * np.sqrt(2.0 * a_u)
    dx[0] = -a_u * x_filter[0] + b_u * w_u

    # v_g filter (2nd-order): x[1], x[2]
    a_v = _V_AIR / _L_V
    b_v = _SIGMA_V * np.sqrt(3.0 * a_v) * a_v
    dx[1] = x_filter[2]
    dx[2] = -a_v**2 * x_filter[1] - 2.0 * a_v * x_filter[2] + b_v * w_v

    # w_g filter (2nd-order): x[3], x[4]
    a_w = _V_AIR / _L_W
    b_w = _SIGMA_W * np.sqrt(3.0 * a_w) * a_w
    dx[3] = x_filter[4]
    dx[4] = -a_w**2 * x_filter[3] - 2.0 * a_w * x_filter[4] + b_w * w_w

    return dx

def _euler_kinematics(phi, theta, p, q, r):
    """Euler angle rates from body angular rates."""
    cp, sp = np.cos(phi), np.sin(phi)
    theta_c = np.clip(theta, -1.39, 1.39)
    tan_th = np.tan(theta_c)
    cos_th = np.cos(theta_c)
    sec_th = 1.0 / cos_th if abs(cos_th) > 1e-12 else 1e12 * np.sign(cos_th)
    dphi = p + q * sp * tan_th + r * cp * tan_th
    dtheta = q * cp - r * sp
    dpsi = (q * sp + r * cp) * sec_th
    return dphi, dtheta, dpsi

def _pd_controller(y):
    """Cascaded PD hover controller."""
    sp = _HOVER_SP
    px, py, pz = y[0], y[1], y[2]
    vx, vy, vz = y[3], y[4], y[5]
    phi, theta, psi = y[6], y[7], y[8]
    p, q, r = y[9], y[10], y[11]

    ax_d = _KP_POS * (sp[0] - px) + _KD_POS * (sp[3] - vx)
    ay_d = _KP_POS * (sp[1] - py) + _KD_POS * (sp[4] - vy)
    az_d = _KP_POS * (sp[2] - pz) + _KD_POS * (sp[5] - vz)

    T_des = _MASS * (_G + az_d)
    phi_des = (1.0 / _G) * (ax_d * np.sin(psi) - ay_d * np.cos(psi))
    theta_des = (1.0 / _G) * (ax_d * np.cos(psi) + ay_d * np.sin(psi))

    tau_x = _KP_ATT * np.arctan2(np.sin(phi_des - phi), np.cos(phi_des - phi)) - _KD_ATT * p
    tau_y = _KP_ATT * np.arctan2(np.sin(theta_des - theta), np.cos(theta_des - theta)) - _KD_ATT * q
    tau_z = _KP_YAW * np.arctan2(np.sin(-psi), np.cos(-psi)) - _KD_YAW * r

    T_des = np.clip(T_des, 0.0, _THRUST_MAX)
    tau_x = np.clip(tau_x, -_TORQUE_CLIP, _TORQUE_CLIP)
    tau_y = np.clip(tau_y, -_TORQUE_CLIP, _TORQUE_CLIP)
    tau_z = np.clip(tau_z, -_TORQUE_CLIP * 0.25, _TORQUE_CLIP * 0.25)
    return T_des, tau_x, tau_y, tau_z

def rhs_dryden_gust(t, y):
    """6-DOF quadrotor with MIL-STD-1797A Dryden gust model (17 states)."""
    body = y[:12]
    x_filter = y[12:17]

    # Dryden filter outputs (gust velocities in NED)
    u_gust = x_filter[0]
    v_gust = x_filter[1]
    w_gust = x_filter[3]

    # Relative velocity (body velocity minus gust)
    vx_rel = body[3] - u_gust
    vy_rel = body[4] - v_gust
    vz_rel = body[5] - w_gust

    phi, theta, psi = body[6], body[7], body[8]
    p, q, r = body[9], body[10], body[11]

    T, tau_x, tau_y, tau_z = _pd_controller(body)
    Fx, Fy, Fz = _body_forces(T, phi, theta, psi)

    # Gust-induced aerodynamic forces
    q_dyn = 0.5 * 1.225 * _S_REF
    f_gust_x = -q_dyn * _CD_GUST * u_gust * abs(u_gust)
    f_gust_y = -q_dyn * _CD_GUST * v_gust * abs(v_gust)
    f_gust_z = q_dyn * _CL_GUST * w_gust

    # Gust-induced moments (differential pressure on airframe)
    arm_eff = 0.15  # effective moment arm (m)
    m_gust_x = q_dyn * _CL_GUST * v_gust * arm_eff   # roll from side gust
    m_gust_y = -q_dyn * _CL_GUST * u_gust * arm_eff   # pitch from head gust
    m_gust_z = q_dyn * _CD_GUST * (u_gust - v_gust) * arm_eff * 0.3  # yaw

    # Body dynamics
    d = np.zeros(17)

    # Position derivatives
    d[0] = body[3]
    d[1] = body[4]
    d[2] = body[5]

    # Translational acceleration (with drag on relative velocity)
    d[3] = (Fx - _CD * vx_rel + f_gust_x) / _MASS
    d[4] = (Fy - _CD * vy_rel + f_gust_y) / _MASS
    d[5] = (Fz - _CD * vz_rel + f_gust_z) / _MASS - _G

    # Euler angle rates
    d[6], d[7], d[8] = _euler_kinematics(phi, theta, p, q, r)

    # Angular acceleration with gust-induced moments
    d[9] = (tau_x + m_gust_x + (_IYY - _IZZ) * q * r) / _IXX
    d[10] = (tau_y + m_gust_y + (_IZZ - _IXX) * p * r) / _IYY
    d[11] = (tau_z + m_gust_z + (_IXX - _IYY) * p * q) / _IZZ

    # Dryden filter dynamics
    d[12:17] = _dryden_filter_rhs(x_filter, t)

    return d