粒子滤波

粒子滤波壹贰叁肆伍粒子滤波的发展基于粒子滤波实现的目标被动跟踪仿真粒子滤波的发展粒子滤波(PF:ParticleFilter)的思想基于蒙特卡洛方法(MonteCarlomethods)，它是利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(SequentialImportanceSampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。MCMC改进策略Unscented粒子滤波器(UPF)Rao－Blackwellised粒子滤波器(RBPF)马尔可夫链蒙特卡洛(MCMC)方法通过构造Markov链，产生来自目标分布的样本，并且具有很好的收敛性。在SIS的每次迭代中，结合MCMC使粒子能够移动到不同地方，从而可以避免退化现象，而且Markov链能将粒子推向更接近状态概率密度函数(probabilitydensityfunction(PDF))的地方，使样本分布更合理。基于MCMC改进策略的方法有许多，常用的有Gibbs采样器MetropolisHasting方法。MCMC改进策略UnscentedKalman滤波器(UKF)是Julier等人提出的。EKF(ExtendedKalmanFilter)使用一阶Taylor展开式逼近非线性项，用高斯分布近似状态分布。UKF类似于EKF，用高斯分布逼近状态分布，但不需要线性化只使用少数几个称为Sigma点的样本。这些点通过非线性模型后，所得均值和方差能够精确到非线性项Taylor展开式的二阶项，从而对非线性滤波精度更高。Merwe等人提出使用UKF产生PF的重要性分布，称为Unscented粒子滤波器(UPF)，由UKF产生的重要性分布与真实状态PDF的支集重叠部分更大，估计精度更高。Unscented粒子滤波器(UPF)在高维状态空间中采样时,PF的效率很低。对某些状态空间模型，状态向量的一部分在其余部分的条件下的后验分布可以用解析方法求得，例如某些状态是条件线性高斯模型，可用Kalman滤波器得到条件后验分布，对另外部分状态用PF，从而得到一种混合滤波器，降低了PF采样空间的维数，RBPF样本的重要性权的方差远远低于SIR方法的权的方差，为使用粒子滤波器解决SLAM问题提供了理论基础。而Montemerlo等人在2002年首次将Rao-Blackwellised粒子滤波器应用到机器人SLAM中，并取名为FastSLAM算法。该算法将SLAM问题分解成机器人定位问题和基于位姿估计的环境特征位置估计问题，用粒子滤波算法做整个路径的位置估计，用EKF估计环境特征的位置，每一个EKF对应一个环境特征。该方法融合EKF和概率方法的优点，既降低了计算的复杂度，又具有较好的鲁棒性。Rao－Blackwellised粒子滤波器(RBPF)基于粒子滤波实现的目标被动跟踪被动定位系统是一个仅有角测量的系统，通常对于目标距离是不可测的。由于实时处理和计算存储量的需求，通常选用递推滤波算法来实现。由于系统本身的弱观测性，状态空间模型非线性强，导致滤波算法的收敛精度和收敛时间满足不了要求。处理这种非线性、非高斯问题，粒子滤波算法有很好的表现。yx观测站目标轨迹xkyk-1xk-1ykzk-1zk被动定位系统中二维纯方位目标跟踪模型如上图所示。以观测站为原点，建立二维直角坐标系，图中标出了目标在k时刻、k-1时刻的位置，目标到观测站的距离R不可测。系统状态模型为：（1.1）系统观测模型为：（1.2）11,1,2,...,kkkxxukn1tan(/)kkkkzyxv其中为系统的k时刻状态值（目标在坐标系x，y方向上的位置和速度），为k-1时刻x,y方向上的系统噪声，为k时刻的观测噪声。为k时刻的观测角度。[,,,]kkTkkxkyxxVyV11k-1u[,]kkTxyuukvkz粒子滤波主要有三步基本操作：采样（从不含观察值的状态空间产生新的粒子）、权值计算（根据观察值计算各个粒子的权值）、重采样（抛弃权值小的粒子，使用权值大的粒子代替），这三步构成了粒子滤波的基本算法。序贯重要性重采样SIRF(SequentialImportanceResamplingFilter)算法是粒子滤波的一种基本算法。粒子滤波算法目标跟踪问题可以描述为：在给定一组观测条件下，对系统的状态进行估计。根据Bayesian滤波框架,假设跟踪目标的运动状态{xk,k=0,1,2,...}，观测状态{zk,k=0,1,2,...}，系统的状态空间可以由以下两个方程来描述p(xk|xk−1),p(x0)k≥1p(zk|xk)k≥1式中，先验概率p(xk|xk−1)表明了状态的转移是一个马尔可夫过程（未来的演变（将来）不依赖于它以往的演变（过去））；似然函数p(zk|xk)表明观测过程只依赖于当前的系统状态。为了实现对目标的跟踪，必须及时地递归估计系统的后验概率密度p(xk|z1:k)，其中z1:k={z1,z2,...zk}。状态模型与观测模型通过建立状态模型和观测模型，把运动目标的跟踪抽象为状态向量的估计。可知，一个动态系统至少需要两个参考模型，即描述随时间变化的状态模型及与状态有关并带有噪声的观测模型。状态模型：(1)其中:表示目标状态非线性转移函数，xk表示所跟踪的目标在时刻k的状态向量；vk为状态转移噪声。利用目标转移函数，可以递归的求出在各个离散时间点上的状态向量。观测模型：(2)其中，:表示目标状态非线性观测函数；nk为观测噪声。粒子滤波器是一种通过蒙特卡罗采样实现递归贝叶斯滤波的方法。用一个N点的带有权值的粒子集(其中表示粒子集，表示对应的权值)表示k时刻运动状态的后验概率密度，则p(xk:|z1:k)来近似表示状态的后验概率密度。算法共分4步来实现：粒子预测、权值计算、状态估计、重采样。粒子预测权值计算状态估计重采样粒子滤波器经过几次迭代之后，很多粒子只有很小甚至接近于零的权值，因而粒子退化是粒子滤波器中不可避免的现象。重采样是解决粒子退化的重要方法。累积分布是经常使用一种重采样算法。算法的一般描述：1.初始化：取k＝0，按抽取N个样本点，i＝1,…,N。2.重要性采样：，令，其中i＝1,…,N。()()0:11:(|,)iikkkkxqxxz()()()0:0:1(,)iiikkkxxx11111111()()()1()()()()()1()()0.50.5kkkkkkkkkkiiiikkxxiiixxxiiiikkyyiiiyyyxxVVVyyVVV3.计算权值：若采用一步转移后验状态分布，该式可简化为4.归一化权值：5.重采样：根据各自归一化权值的大小复制/舍弃样本，得到N个近似服从分布的样本。令＝1/N，i＝1,…,N。()()()()()11()()0:11:(|)(|)(|,)iiiiikkkkkkiikkkpzxpxxqxxz()jjiikkNk()ik()0:ikx()ik()()()1(|)iiikkkkpzx6.输出结果：算法的输出是粒子集，用它可以近似表示后验概率和函数的期望7.K=K+1,重复2步至6步。()0:{:1...}ikxiN0:()kkgx0:0:1:0:11(|)()ikNkkkxipxzdxN0:0:11(())()NikkkkiEgxgxN()()1()()1()()1()()1ˆˆˆˆˆkkkkNiikkkiNiixkxiNiikkkiNiiykyixxVVyyVV仿真—流程图权值计算采样权值归一化重采样状态值输出角度输入zkxkwk*wk