传感器融合小结(4):粒子滤波器

内容基于 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 构造一个离散概率分布,其中权重因子   可看作是每个粒子  出现的概率(粒子数 N 无穷大时,每个粒子出现的概率为 0 )。在 k 时刻时, 出现的概率与包含  的  出现的概率相同,即:

                                                             

尝试证明如下:

 的累积概率分布如下:

                   

上式最右端对满足  项求和。

上式亦可写成:

                    

                                               

                                                              

                                              

所以:

                                                  

上式对任意 a 成立,因此可以从小到大依次选择 a 代入以去掉求和符号,从而有:

                                                       

可能会有人提出,如果存在    的情况怎么办?个人认为,虽然实际运用时确实会出现这种情况,但是考虑到对于连续概率分布  抽取某一特定样本的概率为0(  ,虽然事件发生概率为 0 ,但仍有可能发生),因此不必纳入考虑范围。另一方面,也可以认为我们所认为的二者取值相同只是因为我们的分辨率有限,实际上不存在取值相同的情况。

计算:

                                                

对分子 

                      

                                             

                                             

对分母 ,因为每个 k 时刻抽样是独立的,因此:

                                                      

因此:

                      

                                         

即对每个粒子:

                                        

根据:

                                                                 

归一化后求得  的具体值。

 

4 重采样

理想情况下应该根据  采样,即   。此时:

                                                    

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

                                              

当粒子权重方差越大,退化程度越严重。为了避免退化,应该尽可能使   接近   ,因此在选择   最好考虑测量  和之前的状态  ,即把  作为参量纳入  的表达式中,这里记做  。

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

                                           

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

 

5 粒子滤波算法

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

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

(2) 执行循环:

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

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

     3) 重采样:对粒子集  重采样得  

    4) 计算状态估计值: 。

在工程上,通常选取  ,此时:

如果 k-1 时有重采样,则:

具体应用案例可参考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. 提前。此处仍把  看做噪声,计算出其概率分布后,可求出对应任意  和  的  。

估计为:

                   

                                               

                                               

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

                                            

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

                                           

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

 

已标记关键词 清除标记
©️2020 CSDN 皮肤主题: 1024 设计师:上身试试 返回首页