内容基于 edX 平台上Chalmers的micromaster项目：Emerging Automotive Technologies: Sensor Fusion and Non-linear Filtering for Automotive Systems，在学习过程主要参考了Particle Filter Tutorial 粒子滤波：从推导到应用。

这里主要按照自己的思路进行小结，内容主要包括蒙特卡洛采样，重要性采样权重递推推导，重采样和边缘粒子滤波器。

 

1 模型说明

系统模型包括动态模型（Motion Model）和观测模型（Measurement Model）：

                                                           

                                                           

其中  为状态变量， 为观测变量，和  为独立噪声。

根据贝叶斯滤波，我们的目标是在已知  时，求   。

 

2 蒙特卡洛近似和重要性采样

蒙特卡洛近似（Monte Carlo approximations）指的是根据采样样本近似目标概率分布的方法。直观理解就是我们可以对随机事件进行多次独立重复实验，最后根据每种情况出现的次数，绘制频数分布直方图，从而得到近似的概率分布。用数学语言来描述便是：

                                                        

其中  为代表 k 时刻的第 i 个样本。上式两边同时积分后，结果均为1。

上述方法存在的问题是：目标概率未知时，在计算机上不易获得样本，陷入了矛盾。因此引入重要性采样（Importance sampling）：

                                                        

其中  是人为选择的一个概率分布，称为重要性分布，更合理的选择是结合观测数据  确定  ，即  并据此生成样本，即：

 

                                                          

记权重因子：

                                                               

则：

                               

两边积分有：

                                                                       

此时：

                                  

因此问题转化为求  和选择  。至于如何生成一个已知概率分布的样本，可参考史上最全采样方法详细解读与代码实现。

 

3 权重递推

同样采用递推的思路求解  。k 时刻  的权重与 k 时刻  的权重相同，证明如下：

1）对重要性采样：

                                             

冲激函数的作用是为了使连续条件下积分结果为 1 。从直观上理解（例如一维情况）或者从信号采样的角度出发，对具体某一点  ，我们有：

                                                            。

2）对 k 时刻求 0:k 的  ，即：

                                    

因为：

                           

                                               

                                               

所以：

                       

                                            ；当且仅当 j=i 时, 积分不为0。

因此：

计算：

                                                    

对分子 ：

                      

                                             

                                             

对分母  ，不妨取：

                                    

因此：

                          

                                             

即对每个粒子：

                                             

根据：

                                                                 

归一化求得  的具体值。

上述过程中，每个 k 时刻，对应生成样本函数为  。生成样本的过程又可分解为先生成  然后再生成  ，其中  均可看作是参数。因此在 k 时刻我们只需生成  加入  即可。

 

4 重采样

理想情况下应该根据  采样，即  。此时：

                                                    

实际情况下，由于  与  不同，易出现采样点  对应的  接近 0，而  仍较大，此时 。根据递推公式，在 k+1 时刻，也有 。如此重复，不断有新的粒子权重约等于0，经过归一化后，最后只有一个粒子权重接近1，而其他粒子权重均接近0，这会导致估计出现较大误差，同时大量算力浪费在无贡献的粒子上。这种情况称为退化。 退化程度可用有效粒子数来衡量：

                                              

当粒子权重方差越大，退化程度越严重。当有效粒子数小于某一阈值时，需要采取措施干预。通常可采用重采样。例如，生成 [0,1] 之间的 N 个均匀随机数，统计落入区间  （如下图）的粒子数 n ，复制区间对应 的粒子  至 n 个，此时每个粒子的权重均为    。

                                           

上述方式相当于把按权重大小复制粒子，权重大复制得多，权重小的复制得少，甚至舍去。下一个时刻，基于  生成新粒子时，对多个取值相同的粒子  ，还是能分裂出不同取值的粒子  。

 

5 粒子滤波算法

标准粒子滤波算法流程为：

(1) 初始化：根据  生成粒子集

(2) 执行循环：

     1) 重要性采样：根据选择的  生成新粒子   ，计算权重并归一化得 。

     2) 判断粒子退化程度，满足有效粒子数大于某一阈值，，跳到4) 。

     3) 重采样：对粒子集  重采样得  。

    4) 计算状态估计值： 。

在工程上，通常选取  ，此时：

如果涉及重采样，则：

具体应用案例可参考Particle Filter Tutorial 粒子滤波：从推导到应用（四） 。

 

6 边缘粒子滤波器

随着模型状态变量维度的增大，粒子滤波器的所需样本数 N增大（1维时在线段上采样，2维时在平面上采样……），计算量急剧增大，称之为“维度灾难”。此时可以考虑采用边缘粒子滤波器（Marginalized Particle Filter, 也称作Rao-Blackwellized Particle Filter）。

边缘粒子滤波器将状态变量分解为线性变量和非线性变量，其中非线性变量用粒子滤波器求解，线性变量用其他滤波器（如KF）求解，以达到降维的目的。此时后验概率可以写为：

                          

                                                       

其中  为线性变量， 为非线性变量； 用卡尔曼滤波器求解，  用粒子滤波器求解。具体步骤如下：

               

              

对每个步骤的理解如下：

1) PF-pred.：PF 的采样过程。不妨取  。

因为：

其中  和  由 k-1 时刻求得，所以  在这里可看作是一种噪声，即总噪声为  。

2) KF-dyn. upd.：基于  更新  。

 可看作是  的观测方程，则：

                                    

3) KF-pred：预测  ，此时  。

因为  中各项均已知，此步骤为常规的卡尔曼滤波预测步。

4) KF-upd.：基于  更新 。个人认为此步应早于 PF-upd.。

式  中  已由采样确定，此时：

                                            

5) PF-upd.：PF 的权重更新过程   

                                                   

因为  且个人认为此处  应服从  ，因此把 KF-upd. 提前。此处仍把  看做噪声，计算出其概率分布后，可求出对应任意  和  的  。

由上述过程可看出，边缘粒子滤波器相当于采样后对每一个粒子做卡尔曼滤波（此时  相当于参量），最后更新权重。对每个粒子  进行滤波的结果示意如下：

                                            

每条曲线乘以权重  后即可得到近似实际概率分布。实际概率分布如下：

                                           

                                             （相当于选择合适的方向切片再组合来近似原形状）