2D Closed-Loop EKF + PN Engagement

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

def _los_rate_2d(rx, ry, vrx, vry):
    """LOS rate in 2-D (lambda_dot = (R x V_rel) / R^2)."""
    R2 = rx * rx + ry * ry + 0.01
    return (rx * vry - ry * vrx) / R2

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):
    xt, yt = y[0], y[1]
    vxt, vyt = y[2], y[3]
    xi, yi = y[4], y[5]
    vxi, vyi = y[6], y[7]
    xh, yh = y[8], y[9]
    vxh, vyh = y[10], y[11]

    d = np.empty(dim)

    # --- target kinematics (sinusoidal weave) ---
    d[0] = vxt
    d[1] = vyt
    d[2] = a_t * np.sin(omega_t * t)
    d[3] = a_t * np.cos(omega_t * t)

    # --- PN guidance from EKF estimates ---
    rx = xh - xi
    ry = yh - yi
    R = np.sqrt(rx * rx + ry * ry + 0.01)

    vrx = vxh - vxi
    vry = vyh - vyi
    V_c = -(rx * vrx + ry * vry) / R  # closing velocity

    lam_dot = _los_rate_2d(rx, ry, vrx, vry)
    lam_dot = _clamp(lam_dot, -0.5, 0.5)

    a_n = N_pn * max(V_c, 10.0) * lam_dot

    # LOS angle for decomposition
    lam = np.arctan2(ry, rx)
    ax_i = -a_n * np.sin(lam)
    ay_i = a_n * np.cos(lam)

    if R < 0.1:
        ax_i = 0.0
        ay_i = 0.0

    d[4] = vxi
    d[5] = vyi
    d[6] = ax_i
    d[7] = ay_i

    # --- EKF propagation (constant-velocity prediction) ---
    d[8] = vxh
    d[9] = vyh
    d[10] = 0.0
    d[11] = 0.0

    # --- smoothed measurement updates ---
    # Find the nearest update epoch
    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:
        # True range & bearing
        drx_true = xt - xi
        dry_true = yt - yi
        R_true = np.sqrt(drx_true**2 + dry_true**2 + 0.01)
        theta_true = np.arctan2(dry_true, drx_true)

        # Noisy measurement
        R_meas = R_true + noise_r[k]
        theta_meas = theta_true + noise_th[k]

        # Measurement in Cartesian
        x_meas = xi + R_meas * np.cos(theta_meas)
        y_meas = yi + R_meas * np.sin(theta_meas)

        # Innovation
        innov_x = x_meas - xh
        innov_y = y_meas - yh

        # Correction impulse (scaled by window)
        rate = w / max(tau_update, 1e-6)
        d[8] += K_pos * innov_x * rate
        d[9] += K_pos * innov_y * rate
        d[10] += K_vel * innov_x * rate
        d[11] += K_vel * innov_y * rate

    return d