Terminal Homing with Time-to-Go Singularity

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):
    xt, yt, zt = y[0], y[1], y[2]
    vxt, vyt, vzt = y[3], y[4], y[5]
    xi, yi, zi = y[6], y[7], y[8]
    vxi, vyi, vzi = y[9], y[10], y[11]
    xh, yh, zh = y[12], y[13], y[14]
    vxh, vyh, vzh = y[15], y[16], y[17]
    a_lag_y, a_lag_z = y[18], y[19]

    d = np.empty(dim)

    # --- target: 5g lateral maneuver with reversals ---
    a_y_tgt = a_t_maneuver * np.sign(np.sin(omega_man * t))
    a_z_tgt = a_t_maneuver * 0.5 * np.sign(np.cos(omega_man * t))
    d[0] = vxt
    d[1] = vyt
    d[2] = vzt
    d[3] = 0.0  # roughly constant axial speed
    d[4] = a_y_tgt
    d[5] = a_z_tgt

    # --- augmented PN with t_go ---
    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
    V_c = max(V_c, 10.0)

    t_go = max(R / V_c, 0.1)

    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)

    # Augmented PN: standard PN + bias term for estimated target accel
    # Estimate target maneuver from EKF acceleration (finite difference proxy)
    a_t_est_y = (vyh - vyt) * 0.0  # simplified: use a fraction of EKF innovation
    a_t_est_z = (vzh - vzt) * 0.0

    a_cmd_y = N_pn * V_c * lam_az + N_pn * V_c / (2.0 * t_go) * a_t_est_y
    a_cmd_z = N_pn * V_c * lam_el + N_pn * V_c / (2.0 * t_go) * a_t_est_z

    # The dominant stiffness source: N'*V_c/t_go guidance gain
    # At t_go=0.1, V_c~500 → gain ~N'*500/0.1 = 15000 for N'=3

    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
    lam_h = np.arctan2(ry, rx)
    cos_h, sin_h = np.cos(lam_h), np.sin(lam_h)
    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 (200 Hz) ---
    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