粒子滤波

欢迎关注《汽车软件技术》公众号，回复关键字获取资料。

1. 简介

粒子滤波是一种滤波算法。

基本原理：随机选取预测域的 $N$N 个点，称为粒子。以此计算出预测值，并算出在测量域的概率，即权重，加权平均就是最优估计。之后按权重比例，重采样，进行下次迭代。

此算法存在的问题是：粒子数量越多，计算越精确，但计算量也会增加，且随着迭代，有些粒子权重变得很小，导致粒子枯竭。

滤波算法	适用范围
卡尔曼滤波	线性高斯分布
粒子滤波	非线性，非高斯分布

2. 概念

生成一系列 $x_t$xt​ 的假设，保留概率最大的，传播到 $x_{t+1}$xt+1​。保留最大概率 $x_{t+1}$xt+1​，传播到 $x_{t+2}$xt+2​，依此类推。

3. 算法

3.1 初始化

在预测值空间内随机选取 $N$N 个粒子 $x_1^i$x1i​，并分配权重 $w_i$wi​： $x_1^i\sim{p}(x_1)$x1i​∼p(x1​)， $w_i=\frac1{N}$wi​=N1​，式中 $i=1,...,N$i=1,...,N。

3.2 迭代

按照以下顺序，循环执行，$t=1,...,T$t=1,...,T。

1.估算每个粒子的权重 $w_t^i=g(y_t|x_t^i,u_t)$wti​=g(yt​∣xti​,ut​)。下图黑色圆点面积越大，权重越大。

2.计算估计值

加权求平均。

3.从 $\{x_t^i,w_t^i\}_{i=1}^N${xti​,wti​}i=1N​ 重采样 $\{x_t^i\}_{i=1}^N${xti​}i=1N​。下图红色圆点是新粒子。

4.通过采样 $f(\cdot|x_{t-1}^i,u_{t-1})$f(⋅∣xt−1i​,ut−1​) 传播 $x_t^i$xti​。

3.2.1 代码

用Matlab代码描述如下

X(:,1) = random(i_dist,N,1);
w(:.1) = ones(N,1)/N;
for t =1:T
   w(:,t) = pdf(m_dist,y(t)-g(x(:,t));
   w(:,t) = w(:,t)/sum(w(:,t));
   Resample x(:,t)
   x(:,t+1) = f(x(:,t),u(t))+random(t_dist,N,1);
end

3.2.2 结果

在 $t=5$t=5 时刻：

最终输出：

3.3 重采样(resampling)

• 使用 $N$N 个红色相同大小的点表示 $N$N 个不同大小的黑色点。黑色表示上次计算的权重，红色表示下次采样的点。从图中可以看出，当权重越高时，下次被选中的概率更高。

Matlab代码如下：

v = rand(N,1);
wc = cumsum(w(:,t);
[ ,ind1] = sort([v:wc]);
ind = find(ind1<=N)-(0:N-1)';
x(:,t)=x(ind,t);
w(:,t)=ones(N,1)./N;

3.4 权重

通过前期的测量，可以得到一个pdf（probability density function，概率密度函数），用于权重计算。权重计算：

$w_t^i\varpropto{w}_{t-1}^i\frac{p(y_t|x_t^i)p(x_k^i|x_{t-1}^i)}{q(x_t^i|x_{t-1}^i,y_i)}$wti​∝wt−1i​q(xti​∣xt−1i​,yi​)p(yt​∣xti​)p(xki​∣xt−1i​)​

4. 计算复杂度

理论上，粒子滤波计算复杂度是 $\mathcal{O}(NTn_x^2)$O(NTnx2​)，其中 $N$N 是粒子数量，$T$T 是迭代次数， $n_x$nx​ 是状态数量。而卡尔曼滤波是 $\mathcal{O}(Tn_x^3)$O(Tnx3​)。

5. 例子

状态方程如下：

$x_k=\frac{x_{k-1}}{2}+\frac{25x_{k-1}}{1+x_{k-1}^2}+8\cos(1.2(k-1))+v_k$xk​=2xk−1​​+1+xk−12​25xk−1​​+8cos(1.2(k−1))+vk​

$y_k=\frac{x_k^2}{20}+n_k$yk​=20xk2​​+nk​

使用python实现:

import numpy as np
import matplotlib.pyplot as plt


def estimate(particles, weights):
    """returns mean and variance of the weighted particles"""
    mean = np.average(particles, weights=weights)
    var = np.average((particles - mean) ** 2, weights=weights)
    return mean, var


def simple_resample(particles, weights):
    N = len(particles)
    cumulative_sum = np.cumsum(weights)
    cumulative_sum[-1] = 1.  # avoid round-off error
    rn = np.random.rand(N)
    indexes = np.searchsorted(cumulative_sum, rn)
    # resample according to indexes
    particles[:] = particles[indexes]
    weights.fill(1.0 / N)
    return particles, weights


x = 0.1  # 初始真实状态
x_N = 1  # 系统过程噪声的协方差（由于是一维的，这里就是方差）
x_R = 1  # 测量的协方差
T = 75  # 共进行75次
N = 100  # 粒子数，越大效果越好，计算量也越大

V = 2  # 初始分布的方差
x_P = x + np.random.randn(N) * np.sqrt(V)
x_P_out = [x_P]
# plt.hist(x_P,N, normed=True)

z_out = [x ** 2 / 20 + np.random.randn(1) * np.sqrt(x_R)]  # 实际测量值
x_out = [x]  # 测量值的输出向量
x_est = x  # 估计值
x_est_out = [x_est]
# print(x_out)

for t in range(1, T):
    x = 0.5 * x + 25 * x / (1 + x ** 2) + 8 * np.cos(1.2 * (t - 1)) + np.random.randn() * np.sqrt(x_N)
    z = x ** 2 / 20 + np.random.randn() * np.sqrt(x_R)
    # 更新粒子
    # 从先验p(x(k) | x(k - 1))中采样
    x_P_update = 0.5 * x_P + 25 * x_P / (1 + x_P ** 2) + 8 * np.cos(1.2 * (t - 1)) + np.random.randn(N) * np.sqrt(x_N)
    z_update = x_P_update ** 2 / 20
    # 计算权重
    P_w = (1 / np.sqrt(2 * np.pi * x_R)) * np.exp(-(z - z_update) ** 2 / (2 * x_R))
    P_w /= np.sum(P_w)
    # 估计
    x_est, var = estimate(x_P_update, P_w)
    # 重采样
    x_P, P_w = simple_resample(x_P_update, P_w)
    # 保存数据
    x_out.append(x)
    z_out.append(z)
    x_est_out.append(x_est)
    x_P_out.append(x_P)

# 显示粒子轨迹、真实值、估计值
t = np.arange(0, T)
x_P_out = np.asarray(x_P_out)
for i in range(0, N):
    plt.plot(t, x_P_out[:, i], color='gray')
plt.plot(t, x_out, color='lime', linewidth=2, label='true value')
plt.plot(t, x_est_out, color='red', linewidth=2, label='estimate value')
plt.legend()
plt.show()

显示真实值、估计值和粒子轨迹：

6. 附录

6.1 中英文对照表

英文	中文
Particle Filter	例子滤波
Probability Density Function	概率密度函数
Resample	重采样
State Space	状态空间

6.2 扩展阅读

• 《Particle Filter Tutorial 粒子滤波：从推导到应用》

• 《An introduction to particle filters》。

• Kalman-and-Bayesian-Filters-in-Python