3D Closed-Loop with Actuator Dynamics

def _clamp(x: float, lo: float, hi: float) -> float:
    if x < lo:
        return lo
    if x > hi:
        return hi
    return x

def _los_rates_3d(rx, ry, rz, vrx, vry, vrz):
    """LOS rates (azimuth & elevation) in 3-D."""
    R2 = rx * rx + ry * ry + rz * rz + 0.01
    R_horiz2 = rx * rx + ry * ry + 0.01
    lam_az = (rx * vry - ry * vrx) / R2
    lam_el = (R_horiz2 * vrz - rz * (rx * vrx + ry * vry)) / (R2 * np.sqrt(R_horiz2))
    return lam_az, lam_el

def _sigmoid_window(t: float, t_center: float, tau: float) -> float:
    """Unit pulse centred at *t_center*, width ~4*tau, Lipschitz-continuous."""
    arg = (t - t_center) / max(tau, 1e-12)
    s = 1.0 / (1.0 + np.exp(-arg))
    return 4.0 * s * (1.0 - s)

def rhs(t, y):
    # unpack target
    xt, yt, zt = y[0], y[1], y[2]
    vxt, vyt, vzt = y[3], y[4], y[5]
    # interceptor
    xi, yi, zi = y[6], y[7], y[8]
    vxi, vyi, vzi = y[9], y[10], y[11]
    # EKF
    xh, yh, zh = y[12], y[13], y[14]
    vxh, vyh, vzh = y[15], y[16], y[17]
    # actuator
    a_lag_y, a_lag_z = y[18], y[19]

    d = np.empty(dim)

    # --- target 3-D maneuver ---
    d[0] = vxt
    d[1] = vyt
    d[2] = vzt
    d[3] = a_t * np.sin(omega_t_1 * t)
    d[4] = a_t * np.cos(omega_t_1 * t)
    d[5] = a_t * 0.5 * np.sin(omega_t_2 * t)

    # --- PN guidance (3-D, using EKF estimates) ---
    rx = xh - xi
    ry = yh - yi
    rz = zh - zi
    R = np.sqrt(rx * rx + ry * ry + rz * rz + 0.01)

    vrx = vxh - vxi
    vry = vyh - vyi
    vrz = vzh - vzi
    V_c = -(rx * vrx + ry * vry + rz * vrz) / R

    lam_az, lam_el = _los_rates_3d(rx, ry, rz, vrx, vry, vrz)
    lam_az = _clamp(lam_az, -0.5, 0.5)
    lam_el = _clamp(lam_el, -0.5, 0.5)

    Vc_eff = max(V_c, 10.0)
    a_cmd_y = N_pn * Vc_eff * lam_az
    a_cmd_z = N_pn * Vc_eff * lam_el

    if R < 0.1:
        a_cmd_y = 0.0
        a_cmd_z = 0.0

    # Actuator lag
    d[18] = (a_cmd_y - a_lag_y) / tau_act
    d[19] = (a_cmd_z - a_lag_z) / tau_act

    # Interceptor: acceleration along LOS direction + lateral from actuator
    lam_h = np.arctan2(ry, rx)
    lam_v = np.arctan2(rz, np.sqrt(rx * rx + ry * ry + 0.01))

    # PN accel is normal to LOS; decompose into body frame
    ax_i = 0.0  # no axial accel (constant speed assumption relaxed for ODE)
    ay_lat = -a_lag_y * np.sin(lam_h) + a_lag_z * 0.0
    az_lat = a_lag_z * np.cos(lam_v)

    # Simplified decomposition: accel perpendicular to LOS, in two planes
    cos_h, sin_h = np.cos(lam_h), np.sin(lam_h)
    cos_v, sin_v = np.cos(lam_v), np.sin(lam_v)
    d[6] = vxi
    d[7] = vyi
    d[8] = vzi
    d[9] = -a_lag_y * sin_h
    d[10] = a_lag_y * cos_h
    d[11] = a_lag_z

    # --- EKF propagation ---
    d[12] = vxh
    d[13] = vyh
    d[14] = vzh
    d[15] = 0.0
    d[16] = 0.0
    d[17] = 0.0

    # --- smoothed measurement updates ---
    k = int(t / T_update + 0.5)
    k = min(k, n_updates - 1)
    t_k = update_times[k]
    w = _sigmoid_window(t, t_k, tau_update)

    if w > 1e-6:
        drx = xt - xi
        dry = yt - yi
        drz = zt - zi
        R_true = np.sqrt(drx**2 + dry**2 + drz**2 + 0.01)
        az_true = np.arctan2(dry, drx)
        el_true = np.arctan2(drz, np.sqrt(drx**2 + dry**2 + 0.01))

        R_meas = R_true + noise_r_arr[k]
        az_meas = az_true + noise_az_arr[k]
        el_meas = el_true + noise_el_arr[k]

        x_meas = xi + R_meas * np.cos(el_meas) * np.cos(az_meas)
        y_meas = yi + R_meas * np.cos(el_meas) * np.sin(az_meas)
        z_meas = zi + R_meas * np.sin(el_meas)

        innov_x = x_meas - xh
        innov_y = y_meas - yh
        innov_z = z_meas - zh

        rate = w / max(tau_update, 1e-6)
        d[12] += K_pos * innov_x * rate
        d[13] += K_pos * innov_y * rate
        d[14] += K_pos * innov_z * rate
        d[15] += K_vel * innov_x * rate
        d[16] += K_vel * innov_y * rate
        d[17] += K_vel * innov_z * rate

    return d