EKF Radar Tracker — Cruise

def _pack_P(P: np.ndarray) -> np.ndarray:
    """Pack 7×7 symmetric matrix into 28-element upper triangle."""
    out = np.empty(_N_COV)
    k = 0
    for i in range(_N_STATE):
        for j in range(i, _N_STATE):
            out[k] = 0.5 * (P[i, j] + P[j, i])
            k += 1
    return out

def _unpack_P(y_cov: np.ndarray) -> np.ndarray:
    """Reconstruct 7×7 symmetric matrix from 28-element upper triangle."""
    P = np.zeros((_N_STATE, _N_STATE))
    k = 0
    for i in range(_N_STATE):
        for j in range(i, _N_STATE):
            P[i, j] = y_cov[k]
            P[j, i] = y_cov[k]
            k += 1
    return P

def rhs(t, y):
    vx, vy, vz = y[3], y[4], y[5]
    omega = y[6]
    P = _unpack_P(y[_N_STATE:])

    d = np.empty(_DIM_T1)

    # Target kinematics (coordinated turn)
    d[0] = vx
    d[1] = vy
    d[2] = vz
    d[3] = -omega * vy
    d[4] = omega * vx
    d[5] = 0.0
    d[6] = 0.0

    # Jacobian F = df/dy
    F = np.zeros((_N_STATE, _N_STATE))
    F[0, 3] = 1.0
    F[1, 4] = 1.0
    F[2, 5] = 1.0
    F[3, 4] = -omega
    F[3, 6] = -vy
    F[4, 3] = omega
    F[4, 6] = vx

    # Continuous Riccati: dP/dt = F P + P Fᵀ + Q
    dP = F @ P + P @ F.T + Q

    d[_N_STATE:] = _pack_P(dP)
    return d