在python中实现粒子滤波器的反向平滑

Question

我已经实现了一个用于跟踪目的的粒子过滤器。在前向过滤之后，我了解到有一种叫做反向平滑的东西，它使用所有观察结果并向后工作以更新每个时间戳的粒子状态。

我将前向过滤的结果存储在两个 3d 数组中，它们是形状为 [T, N, 3] 的 forward_states 和形状为 [T, N, 1] 的forward_weights。 T 表示总时间戳，N 是粒子数。 forward_states 存储 3 个状态变量，前向权重存储 1 个权重变量。

我已经编写了一些代码，使用 t 时间戳的权重来递归地更新 t-1 的状态，直到第一个时间戳。

def backward_pass(forward_states, forward_weights):
    num_timestamps, num_particles, num_state_variables = forward_states.shape
    backward_state_array = np.zeros((num_timestamps, num_particles, num_state_variables))
    backward_weight_array = np.zeros((num_timestamps, num_particles, 1))

    backward_state_array[-1] = forward_states[-1]
    backward_weight_array[-1] = forward_weights[-1]

    # starting from the second last
    for t in range(num_timestamps - 2, -1, -1):
        # resample the particles based on the normalised importance weights
        resampled_indices = np.random.choice(num_particles, size=num_particles, replace=True, p=forward_weights[t + 1].flatten())

        # backward simulate the state variables of the particles
        backward_state_array[t] = forward_states[t][resampled_indices]
        backward_weight_array[t] = forward_weights[t]

    return backward_state_array, backward_weight_array

然后使用 backward_state 和 backward_weight 计算最终的平滑值。然而，结果比仅仅前向过滤的估计更糟糕。

我试图学习它背后的数学，问题基本上是在给定所有观察的情况下找到任何状态的概率（权重）：

最后一项就是从前向过滤计算出来的forward_states。但是，我很难尝试将我的代码链接到。我不认为我的代码做同样的事情，我不确定如何正确地实现它。

感谢数学/编程方面的任何帮助。