粒子滤波四种基本重采样代码

粒子滤波中的重采样是非常重要的一步，可以使得粒子在一定程度上避免漂移。以下是四种基本的重采样代码：

1. Systematic Resampling（系统重采样）

def systematic_resampling(particles, weights):
    N = len(weights)
    positions = (np.arange(N) + np.random.uniform()) / N
    indexes = np.zeros(N, 'i')
    cumulative_sum = np.cumsum(weights)
    i, j = 0, 0
    while i < N and j < N:
        if positions[i] < cumulative_sum[j]:
            indexes[i] = j
            i += 1
        else:
            j += 1
    return particles[indexes]

2. Multinomial Resampling（多项式重采样）

def multinomial_resampling(particles, weights):
    indexes = np.zeros(len(weights), 'i')
    cumulative_sum = np.cumsum(weights)
    cumulative_sum[-1] = 1.  # avoid round-off errors
    for i in range(len(weights)):
        indexes[i] = np.searchsorted(cumulative_sum, np.random.uniform())
    return particles[indexes]

3. Residual Resampling（残余重采样）

def residual_resampling(particles, weights):
    N = len(weights)
    indexes = np.zeros(N, 'i')
    # determine integer parts for each weight
    num_copies = (N * weights).astype(int)
    k = 0
    for i in range(N):
        for _ in range(num_copies[i]):
            indexes[k] = i
            k += 1
    # use multinomial resample on the residual to fill up the rest.
    residual = weights - num_copies     # get fractional part
    residual /= sum(residual)           # normalize
    cumulative_sum = np.cumsum(residual)
    cumulative_sum[-1] = 1.  # avoid round-off errors
    for i in range(k, N):
        indexes[i] = np.searchsorted(cumulative_sum, np.random.uniform())
    return particles[indexes]

4. Stratified Resampling（分层重采样）

def stratified_resampling(particles, weights):
    N = len(weights)
    # make N subdivisions, and chose a random position within each one
    positions = (np.random.uniform(0, 1, N) + range(N)) / N
    indexes = np.zeros(N, 'i')
    cumulative_sum = np.cumsum(weights)
    i, j = 0, 0
    while i < N and j < N:
        if positions[i] < cumulative_sum[j]:
            indexes[i] = j
            i += 1
        else:
            j += 1
    return particles[indexes]