粒子滤波算法原理和仿真
1  引言
    粒子滤波(Particle Filter, PF)是一种基于蒙特卡洛(Monte Carlo, MC)方法的递推贝叶斯滤波算法。其核心思想是通过从状态空间寻的一系列随机样本来近似系统变量的概率密度函数，以样本均值代替积分运算，从而获得状态的最小方差估计。其中从状态空间中抽取的样本称为“粒子”。一般地，随着粒子数目的增加，粒子的概率密度函数就逐渐逼近状态的概率密度函数，从而达到最优贝叶斯估计的效果。
2  粒子滤波原理
2.1 系统的动态空间
    对于被观测对象的状态，可以通过以下非线性离散系统来描述：
                          (1)
                          (2)
    以上为系统的状态方程和观测方程。其中，f( )为状态函数，h( )为观测函数，xt是系统在时间t的状态变量，wt为对应的过程噪声，zt是系统在时间t的观测值，vt为对应的观测噪声。
    从贝叶斯估计角度来看，状态估计问题就是根据观测信息z0:t构造状态的概率密度函数p(x0:t|z0:t)，从而估计在系统在任何状态下的滤波值。设系统状态序列函数为gt，则有：
                  (3)
    根据蒙特卡洛方法，后验概率分布可以用有限的离散样本来近似，由大数定律，当系统粒子数N→∞时，期望E[gt(x0:t)]可近似为：
                      (4)
    式中{: i=1,2,...N}为状态空间中按p(x0:t|z0:t)得到的采样点。
2.2 重要性采样
    在粒子采集过程中，p正则化粒子滤波(x0:t|z0:t)往往是未知且多变的，因此可先从一个已知且容易采样的参考分布q(x0:t|z0:t)中抽样，再通过对抽样粒子集进行加权求和来估计系统的状态值，即：
                (5)
    令ωt(x0:t) = p(z0:t|x0:t)p(x0:t) ∕ q(x0:t|z0:t)，则式(5)可表示为：
                (6)
    按照式(4)，式(6)可近似为：
                  (7)
    其中为的归一化权值，是由q( x0:t z0:t )采样获得的粒子。
2.3 序贯重要性抽样
    贝叶斯估计是一个序列估计问题，通过序贯重要性抽样(Sequential Importance Sampling, SIS)建立采样粒子的序列关系。在t+1时刻采样时，不改变状态序列过去的样本集，而采用递归的形式计算重要性权值。因此参考分布可表示为：
                (8)
    同时，假设系统状态是一阶马尔可夫过程，即xt只与xt-1相关，zt只与xt相关，因此：
                        (9)
              (10)
    再将式(8) ~ (10)代入ωt，可推知：
                      (11)
    一般q( xt|x0:t-1 , z0:t ) =p(xt|xt-1, zt)为参考分布的最优分布，其概率密度仅依赖于xt-1和zt。但这种选择在实际中往往难以实现，因此多选用q( xt|x0:t-1 , z0:t ) = p(xt|xt-1)来近似。此时系统仅记录当前状态粒子和其权重，则由式(11)可知：
                          (12)
2.4 粒子滤波重采样
    粒子退化是序贯重要性采样算法的主要问题，即经过若干次递推后，权值方差会逐渐增大，这样大量权值都集中在少数粒子上，而多数粒子则对系统状态的估计影响甚微，由此严重影响了滤波器的性能。重采样是解决粒子退化的重要方法，其核心思想是减少或剔除权值较小的粒子，而复制权值较大的粒子，最后将所有粒子的权值设为1/N。粒子滤波重采样方法的如下图所示：
图1 粒子滤波重采样示意
    图中为采样得到的粒子，对应的权值为；为重采样所得的粒子，对应权值为1/N。目前常用的重采样算法包括随机重采样(random resampling)、多项式重采样(multinomial resampling)、系统重采样(systematic resampling)和残差重采样(residual resampling)，几种算法的计算步骤分别如下：
    1. 随机重采样
    设u~U(0,1]，{ui : i=1, 2, .., N }独立同分布。定义函数D( )，若：
                (13)
则D(ui) = m，且。
    2. 多项式重采样
    设~U(0,1]，{ : i=1, 2, .., N }独立同分布，定义：
                (14)
对函数D( )，由式(13)可知：D(ui) = m，且。
    3. 系统重采样
    4. 残差重采样
    综上所述，粒子滤波算法的一般流程为：
    （1）粒子集初始化，由先验概率密度p(x0)产生粒子集{}，并使所有粒子权值为1/N；
    （2）按照对t时刻的系统进行重要性采样，并通过当前观测值zt根据式(12)计算粒子权值；
    （3）计算归一化权值，并按式(15)计算有效粒子数Neff；
                          (15)
    （4）若Neff小于设定阈值Nth，则对粒子进行重采样，得到新的粒子集，其中=1/N，否则；
    （5）由式(7)推知系统的状态估计值为：
                            (16)
再转到步骤（2），对t+1时刻的系统进行重要性采样。
3  粒子滤波仿真
    首先构造系统的状态模型和观测模型：
                (17)
                            (18)

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。