粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

文档中心

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

原创不易，路过的各位大佬请点个赞

机动目标跟踪/非线性滤波/传感器融合/导航等探讨代码联系WX: ZB823618313

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）
- 1、贝叶斯滤波
- 2、蒙特卡洛方法MC
- 3、序贯重要性采样SIS
- 4、粒子退化
- - 4.1、粒子退化问题
  - 4.2、粒子退化度量
  - 4.3、选择合适的重要性密度函数
- 5、粒子重采样
- 6、基于系统重采样的SIRPF的在目标跟踪应用：
- - 6.1、仿真参数
  - 6.2、跟踪轨迹
  - 6.3、跟踪误差（RMSE）

在非线性条件下，贝叶斯滤波面临一个重要问题是状态分布的表达和积分式的求解，由前面章节中的分析可知,对于一般的非线性/非高斯系统,解析求解的途径是行不通的。在数值近似方法中，蒙特卡罗仿真是一种最为通用、有效的手段，粒子滤波就是建立在蒙特卡罗仿真基础之上的，它通过利用一组带权值的系统状态采样来近似状态的统计分布。由于蒙特卡罗仿真方法具有广泛的适用性，由此得到的粒子滤波算法也能适用于一般的非线性/非高斯系统。但是，这种滤波方法也面临几个重要问题，如有效采样(粒子)如何产生、粒子如何传递以及系统状态的序贯估计如何得到等。

简单的理解，粒子滤波就是使用了大量的随机样本，采用蒙特卡洛(MonteCarlo，MC)仿真技术完成贝叶斯递推滤波(Recursive Bayesian Filter)过程。因此本博客从贝叶斯滤波出发，简单介绍粒子滤波PF的出生、即应用

核心思想：是使用一组具有相应权值的随机样本(粒子)来表示状态的后验分布。该方法的基本思路是选取一个重要性概率密度并从中进行随机抽样，得到一些带有相应权值的随机样本后，在状态观测的基础上调节权值的大小。和粒子的位置，再使用这些样本来逼近状态后验分布，最后将这组样本的加权求和作为状态的估计值。粒子滤波不受系统模型的线性和高斯假设约束，采用样本形式而不是函数形式对状态概率密度进行描述，使其不需要对状态变量的概率分布进行过多的约束，因而在非线性非高斯动态系统中广泛应用。尽管如此，粒子滤波目前仍存在计算量过大、粒子退化等关键问题亟待突破。

1、贝叶斯滤波

**贝叶斯滤波细节见Part-I**

考虑离散时间非线性系统动态模型，
$(1)x_k=f(x_{k-1},w_{k-1}) \\ z_k=h(x_k,v_k ) \tag{1}$
其中 $x_k$ 为 $k$ 时刻的目标状态向量， $z_k$ 为 $k$ 时刻量测向量（传感器数据）。这里不考虑控制器 $u_k$ 。 ${w_k}$ 和 ${v_k}$ 分别是过程噪声序列和量测噪声序列。 $w_k$ 和 $v_k$ 为零均值高斯白噪声。

定义 $1$ ~ $k$ 时刻对状态 $x_k$ 的所有测量数据为
$z^k=[z_1^T,z_2^T,\cdots,z_k^T]^T$

根据Part-I， $k$ 时刻状态 $x_k$ 的后验概率密度函数:
$)p(x_k |z^{k})=\frac{p(z_k |x_k)p(x_k |z^{k-1})}{p(z_k |z^{k-1})}$

通过后验分布 $p(x_k |z^{k})$ 可以得到 $x_k$ 的最小均方误差(MMSE)估计为
$(2)\hat{x}_k=E[x_k|z_k]=\int x_kp(x_k |z^{k}) dx_k \tag{2}$

2、蒙特卡洛方法MC

**蒙特卡洛近似方法细节见Part-I**

根据Part-II蒙特卡洛方法， $x_k^{(i)}, i=1,2,\cdots,N$ 表示从后验概率分布函数 $p(x_k |z^{k})$ 采样得到的 $N$ 个独立同分布的样本，则状态的后验概率密度可以通过如下经验公式近似得到:
$p(x_k |z^{k})=\frac{1}{N}\sum_{i=1}^N\delta(x_k-x_k^{(i)})$
同时后验条件期望可近似表示为
$(3)\hat{x}_k=E[x_k|z^{k}]\approx\hat{E}[x_k|z^{k}]\approx\frac{1}{N}\sum_{i=1}^Nx_k^{(i)}, \tag{3}\\ E[g(x_k)|z^{k}]\approx\hat{E}[g(x_k)|z^{k}]\approx\frac{1}{N}\sum_{i=1}^Ng(x_k^{(i)})$
蒙特卡洛方法是实现的贝叶斯滤波，得到粒子滤波的桥梁。

3、序贯重要性采样SIS

重要性采样不能直接用来进行递推估计，主要因为估计 $p(x_{0:k} |z_{1:k})$ 的过程需要用到所有的量测信息 $k$ ,然而每次在 $k + 1$ 时刻更新量测信息 $z_{k+1}$ 时，则需要重新计算整个状态序列的重要性权值，所以其计算量将随时间的推移而大量增加。为了解决这一问题，序贯重要性采样( Sequential Importance Sampling，SIS) 方法得以提出。

序贯重要性采样算法根据每一步接收到新的量测信息，逐次进行采样粒子和重要性权值的递推，算法步骤如下：

算法：序贯重要性采样SIS
For $i = 1 : N$
Step 3: 采样粒子 $x_k^{(i)} \sim q(x_{k} |x_{k-1},z_{1:k})$
Step 4: 根据 $\tilde{w}_k^{(i)}=\tilde{w}_{k-1}^{(i)}\frac{p(x_k^{(i)} |x_{k-1}^{(i)})p(z_{k}|x_k^{(i)})}{q(x_k^{(i)} |x_{k-1}^{(i)}, z_{k})}$
计算立在的权值 $)\tilde{w}_k^{(i)}$
End For
Step 5: 粒子重要性权重归一化
$\tilde{w}_k^{(i)}=\frac{w_k^{(i)}}{\sum_{j=1}^Nw_k^{(j)}}$

4、粒子退化

4.1、粒子退化问题

SIS 算法在经历次多次迭代后，粒子重要性权重的方差可能将变得很大，从而引发粒子退化问题（Particle Degeneracy Problem）。所谓粒子退化，指的是大量粒子中只有少数粒子具有较高权重，而绝大多数粒子的权重都很小甚至接近于0，导致计算加权均值时大量的运算资源被浪费在了小权重粒子上。进而是的估计性能下降，如下图所示：
在这里插入图片描述

粒子退化问题发生的根本原因是建议分布与真实分布的不匹配。

4.2、粒子退化度量

序贯重要性采样的一个常见问题就是粒子退化现象，即经过若干次迭代之后，除了少数几个粒子，大部分其他粒子的权值将小到可以忽略不计。粒子退化现象的原因在于，重要性权值的方差将随时间的推移而增加。因此，粒子退化问题的存在意味着大量的计算工作将浪费在更新那些对 $p(x_k |z^{k})$ 的估计作用几乎为零的粒子上。下面给出了一种衡量算法的粒子退化程度的方法，定义有效样本数(effective sample size)为
$)N_{eff}=\frac{N}{1+\text{var}(w_k^{*(i)})}$
式中， $w_k^{*(i)}$ 是真权值。有效样本无法通过计算准确得到，但可以用一下估计获得
$^e f f =N 1 + ∑ i = 1 N(w ~ k ( i ) ) 2 \hat{N}_{eff}=\frac{N}{1+\sum_{i=1}^N(\tilde{w}_k^{(i)})^2}$
式中， $\tilde{w}_k^{(i)}$ 是粒子重要性权重归一化。易知 $N^ eff ≤ Neff \hat{N}_{eff}\leq N_{eff}$ ，而很小的 $N^ eff \hat{N}_{eff}$ 意味着粒子严重退化。显然粒子退化问题是在粒子滤波过程中所不希望看到的，一种强制措施是采用大量粒子，增大粒子数N,这种方法通常情况下是不现实的。因此，可考虑采用两种解决方案：

选择合适的重要性密度函数
粒子重采样法

4.3、选择合适的重要性密度函数

针对“选择合适的重要性密度函数”问题，常用的选取方案是次优的重要性密度函数，即.
$q(x_k |x_{k-1}^{(i)}, z_{k})=p(x_k |x_{k-1}^{(i)})$
进而得到
$w_k^{(i)}=w_{k-1}^{(i)}p(z_k |x_{k}^{(i)})$
同样 $w_k^{(i)}$ 需要归一化得到 $\tilde{w}_k^{(i)}$ 。

这种方案虽然未能利用最新的量测信息，使得采样粒子的方差较大，但其优点在于较为直观且易于实现，所以得到了广泛使用。

5、粒子重采样

重采样的思路是：既然那些权重小的不起作用了，那就不要了。要保持粒子数目不变，得用一些新的粒子来取代它们。找新粒子最简单的方法就是将权重大的粒子多复制几个出来，至于复制几个？那就在权重大的粒子里面让它们根据自己权重所占的比例去分配，也就是老大分身分得最多，老二分得次多，以此类推。下面以数学的形式来进行说明。

下面先给出常用的重采样方法：
系统重采样
多项式重采样
残差重采样
随机重采样

重采样(resampling)也是抑制粒子退化现象的一种有效方法。重采样法的主要思
想是，预先设定一个 $N τ$ 作为有效样本数N的阈值，当 $N^ eff \hat{N}_{eff}$ 低于 $N τ$ 时进行重采样，其目的在于抑制权值较小的粒子，而只关心权值较大的粒子。重采样的步骤是，对于给定的后验概率密度函数 $p(x_k |z^{k})$ 的离散近似:
$)p(x_k |z^{k})\approx\frac{1}{N}\sum_{i=1}^N\delta(x_k-x_k^{(i)})w_k^{(i)}$

采样 $N$ 次重新生成一组新的粒子 $x_k^{(i)}$ ，使得 $P(x_k^{(i)}=x_k^{j}) =w_k^{(j)}$ 。而根据重采样粒子的独立同分布特性，其权值将重置为 $w_k^{(i)}=1/N$ 。

这一过程可以通过下图加以解释，同样途中圆圈的大小代表粒子所占的权重大小。
粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

需要指出的是，虽然重采样方法在某种程度上可以抑制粒子退化问题，但会降低粒子的多样性，使得原本权值较小的粒子缺乏子代粒子，而少数权值较大的粒子具有多个相同的子代粒子。常用的重采样方法包括系统重采样、多项式重采样、残差重采样、随机重采样等。

其中基于系统重采样的SIS粒子滤波也被称之为：标准的粒子滤波

本人更喜欢随机重采样，它的讲解比较清楚在Dan Simon的《最优状态估计卡尔曼滤波， $H_\infty滤波和非线性滤波$ 》

下面给出系统重采样方法及代码：

算法：系统重采样（systematic resampling）
For $i = 1 : N$
Step 1: 初始化累积概率密度函数CDF： $c_1=0$
For $i = 2 : N$
Step 2: 构造CDF: $c_i=c_{i-1}+w_k^{(i)}$
Step 3: 从CDF的底部开始： $i = 1$
Step 4: 采样起始点： $u_1=\mathcal{U}[0,1/N]$
End For
For $j = 1 : N$
Step 5: 沿CDF移动: $u_j=u_{1}+(j-1)/N$
Step 6: While $u_j>c_i$
$i = i + 1$
End While
Step 7: 赋值粒子： $x_k^{(j)}=x_k^{(i)}$
Step 8: 赋值权值： $w_k^{(j)}=1/N$
Step 9: 赋值父代： $i^{(j)}=i$
End For

代码：系统重采样（systematic resampling）

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 系统重采样子函数% 输入参数：weight为原始数据对应的权重大小% 输出参数：outIndex是根据weight筛选和复制结果function outIndex = systematicR(weight);N=length(weight);N_children=zeros(1,N);label=zeros(1,N);label=1:1:N;s=1/N;auxw=0;auxl=0;li=0;T=s*rand(1);j=1;Q=0;i=0;u=rand(1,N);while (T<1)    if (Q>T) T=T+s; N_children(1,li)=N_children(1,li)+1;    else i=fix((N-j+1)*u(1,j))+j; auxw=weight(1,i); li=label(1,i); Q=Q+auxw; weight(1,i)=weight(1,j); label(1,i)=label(1,j); j=j+1;    endendindex=1;for i=1:N    if (N_children(1,i)>0) for j=index:index+N_children(1,i)-1     outIndex(j) = i; end;    end;    index= index+N_children(1,i);end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

6、基于系统重采样的SIRPF的在目标跟踪应用：

6.1、仿真参数

**一、目标模型：CT（细节见另一个博客） **

$X_{k+1}=\begin{bmatrix}1&\frac{\sin(\omega T)}{\omega}&0&-\frac{1-\cos(\omega T)}{\omega}\\0&\cos(\omega T)&0&-\sin(\omega T)\\0&\frac{1-\cos(\omega T)}{\omega}&1&\frac{\sin(\omega T)}{\omega}\\0&\sin(\omega T)&0&\cos(\omega T)\end{bmatrix}X_{k} + \begin{bmatrix}T^2/2&0\\T&0\\0&T^2/2\\0&T\end{bmatrix}W_k$

CV CT 模型的具体方程形式见另一个博客

二、测量模型：2D主动雷达
在二维情况下，雷达量测为距离和角度
${r}_k^m=r_k+\tilde{r}_k\\ b^m_k=b_k+\tilde{b}_k$
其中
$r_k=\sqrt{(x_k-x_0)^+(y_k-y_0)^2)}\\ b_k=\tan^{-1}{\frac{y_k-y_0}{x_k-x_0}}\\$
$x_0,y_0]$ 为雷达坐标，一般情况为0。雷达量测为 $z_k=[r_k,b_k]'$ 。雷达量测方差为
$R_k=\text{cov}(v_k)=\begin{bmatrix}\sigma_r^2 & 0 \\0 & \sigma_b^2 \end{bmatrix}$

6.2、跟踪轨迹

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

6.3、跟踪误差（RMSE）

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

PF的妈妈贝叶斯滤波、基于标准的粒子滤波见Part-I和Part-V

原创不易，路过的各位大佬请点个赞

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

1、贝叶斯滤波

2、蒙特卡洛方法MC

3、序贯重要性采样SIS

4、粒子退化

4.1、粒子退化问题

4.2、粒子退化度量

4.3、选择合适的重要性密度函数

5、粒子重采样

6、基于系统重采样的SIRPF的在目标跟踪应用：

6.1、仿真参数

6.2、跟踪轨迹

6.3、跟踪误差（RMSE）

公告

免费可商用字体批量下载

标签

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

粒子滤波PF—从贝叶斯滤波到粒子滤波PF—Part-IV（粒子退化和重采样）

1、贝叶斯滤波

2、 蒙特卡洛方法MC

3、 序贯重要性采样SIS

4、 粒子退化

4.1、 粒子退化问题

4.2、 粒子退化度量

4.3、 选择合适的重要性密度函数

5、 粒子重采样

6、基于系统重采样的SIRPF的在目标跟踪应用：

6.1、 仿真参数

6.2、 跟踪轨迹

6.3、 跟踪误差（RMSE）

相关问题

公告

免费可商用字体批量下载

标签

2、蒙特卡洛方法MC

3、序贯重要性采样SIS

4、粒子退化

4.1、粒子退化问题

4.2、粒子退化度量

4.3、选择合适的重要性密度函数

5、粒子重采样

6.1、仿真参数

6.2、跟踪轨迹

6.3、跟踪误差（RMSE）