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

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

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

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

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

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

1、贝叶斯滤波

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

考虑离散时间非线性系统动态模型，
$$\begin{gathered} x_k=f(x_{k-1},w_{k-1}) \\ {z}_k=h(x_k,v_k)\text{\hspace{1em}(1)} \end{gathered}$$
其中$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)估计为
$${\hat{x}}_k=E[x_k|z_k]=\int x_{k}p(x_k|z^k)d{x}_k\text{\hspace{1em}(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)})$$
同时后验条件期望可近似表示为
$$\begin{gathered} {\hat{x}}_k=E[x_k|z^k]\approx \hat{E}[x_k|z^k]\approx \frac{1}{N}\sum _{i=1}^N{x}_k^{(i)}, \\ E[g(x_k)|z^k]\approx \hat{E}[g(x_k)|z^k]\approx \frac{1}{N}\sum _{i=1}^{N}g(x_k^{(i)})\text{\hspace{1em}(3)} \end{gathered}$$
蒙特卡洛方法是实现的贝叶斯滤波，得到粒子滤波的桥梁。

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}^N{w}_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^{\ast (i)})}$$
式中，$w_k^{\ast (i)}$是真权值。有效样本无法通过计算准确得到，但可以用一下估计获得
$${\hat{N}}_{eff}=\frac{N}{1+{\sum }_{i=1}^N({\tilde{w}}_k^{(i)}{)}^2}$$
式中，${\tilde{w}}_k^{(i)}$是粒子重要性权重归一化。易知${\hat{N}}_{eff}\le 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\tau$作为有效样本数N的阈值，当${\hat{N}}_{eff}$低于$N\tau$时进行重采样，其目的在于抑制权值较小的粒子，而只关心权值较大的粒子。重采样的步骤是，对于给定的后验概率密度函数$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$。

这一过程可以通过下图加以解释，同样途中圆圈的大小代表粒子所占的权重大小。

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

其中基于系统重采样的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;
    end
end
index=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}=\left[\begin{array}{cccc}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{array}\right]X_k+\left[\begin{array}{cc}{T}^2/2& 0\\ T& 0\\ 0& T^2/2\\ 0& T\end{array}\right]W_k$$

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

二、测量模型：2D主动雷达
在二维情况下，雷达量测为距离和角度
$$\begin{gathered} r_k^m=r_k+{\tilde{r}}_k \\ {b}_k^m=b_k+{\tilde{b}}_k \end{gathered}$$
其中
$$\begin{gathered} 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} \end{gathered}$$
$[x_0,y_0]$为雷达坐标，一般情况为0。雷达量测为$z_k=[r_k,b_k{]}^{\prime }$。雷达量测方差为
$$R_k=\text{cov}(v_k)=\left[\begin{array}{cc}{\sigma }_r^2& 0\\ 0& {\sigma }_b^2\end{array}\right]$$

6.2、 跟踪轨迹

6.3、 跟踪误差（RMSE）

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

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