粒子滤波及matlab实现.docx
《粒子滤波及matlab实现.docx》由会员分享，可在线阅读，更多相关《粒子滤波及matlab实现.docx（20页珍藏版）》请在冰豆网上搜索。
粒子滤波及matlab实现
粒子滤波及matlab实现
粒子滤波就是指：
通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象的称为“粒子”，故而叫粒子滤波。
粒子滤波通过非参数化的蒙特卡洛（MonteCarlo）模拟方法来实现递推贝叶斯滤波，适用于任何能用状态空间模型描述的非线性系统，精度可以逼近最优估计。
粒子滤波器具有简单、易于实现等特点，它为分析非线性动态系统提供了一种有效的解决方法，从而引起目标跟踪、信号处理以及自动控制等领域的广泛关注。
贝叶斯滤波
动态系统的目标跟踪问题可以通过下图所示的状态空间模型来描述。
在目标跟踪问题中，动态系统的状态空间模型可描述为
xkf（xk1）uk1ykh（xk）vk
其中f（）,h（）分别为状态转移方程与观测方程，xk为系统状态，yk为观测值，uk为
过程噪声，vk为观测噪声。
为了描述方便，用Xkx0:
k{x0,x1,,xk}与
Yky1:
k{y1,,yk}分别表示0到k时刻所有的状态与观测值。
在处理目标跟踪问题时，通常假设目标的状态转移过程服从一阶马尔可夫模型，即当前时刻的状态xk只与上一时刻的状态xk-1有关。
另外一个假设为观测值相互独立，即观测值yk只与k时刻的状态xk有关。
贝叶斯滤波为非线性系统的状态估计问题提供了一种基于概率分布形式的解决方案。
贝叶斯滤波将状态估计视为一个概率推理过程，即将目标状态的估计问题转换为利用贝叶斯公式求解后验概率密度p（Xk|Yk）或滤波概率密度p（xk|Yk），进而获得目标状态的最优估计。
贝叶斯滤波包含预测和更新两个阶段，预测过程利用系统模型预测状态的先验概率密度，新过程则利用最新的测量值对先验概率密度进行修正，得到后验概率密度。
假设已知k1时刻的概率密度函数为p（xk1|Yk1），贝叶斯滤波的具体过程如下：
（1）预测过程，由p（xk1|Yk1）得到p（xk|Yk1）：
p（xk,xk1|Yk1）p（xk|xk1,Yk1）p（xk1|Yk1）
当给定xk1时，状态xk与Yk1相互独立，因此
p（xk,xk1|Yk1）p（xk|xk1）p（xk1|Yk1）
上式两端对xk1积分，可得Chapman-Komolgorov方程
p（xk|Yk1）p（xk|xk1）p（xk1|Yk1）dxk1
（2）更新过程，由p（xk|Yk1）得到p（xk|Yk）：
获取k时刻的测量yk后，利用贝叶斯公式对先验概率密度进行更新，得到后验概率
p（xk|Yk）
p（yk|xk,Yk1）p（xk|Yk1）
p（yk|Yk1）
假设yk只由xk决定，即
p（yk|xk,Yk1）p（yk|xk）
因此
p（xk|Yk）
p（yk|xk）p（xk|Yk1）
p（yk|Yk1）
其中，p（yk|Yk1）为归一化常数
p（yk|Yk1）p（yk|xk）p（xk|Yk1）dxk
贝叶斯滤波以递推的形式给出后验（或滤波）概率密度函数的最优解。
目标状态的最优估计值可由后验（或滤波）概率密度函数进行计算。
通常根据极大后验（MAP）准则或最小均方误差（MMSE）准则，将具有极大后验概率密度的状态或条件均值作为系统状态的估计值，即x?
kMAP=argminp（xk|Yk）x?
kMMSE=E[f（xk）|Yk]f（xk）p（xk|Yk）dxk
贝叶斯滤波需要进行积分运算，除了一些特殊的系统模型（如线性高斯系统，有限状态的离散系统）之外，对于一般的非线性、非高斯系统，贝叶斯滤波很难得到后验概率的封闭解析式。
因此，现有的非线性滤波器多采用近似的计算方法解决积分问题，以此来获取估计
的次优解。
在系统的非线性模型可由在当前状态展开的线性模型有限近似的前提下，基于一阶或二阶Taylor级数展开的扩展Kalman滤波得到广泛应用。
在一般情况下，逼近概率密度函数比逼近非线性函数容易实现。
据此，Julier与Uhlmann提出一种UnscentedKalman滤波器，通过选定的sigma点来精确估计随机变量经非线性变换后的均值和方差，从而更好的近似状态的概率密度函数，其理论估计精度优于扩展Kalman滤波。
获取次优解的另外一中方案便是基于蒙特卡洛模拟的粒子滤波器。
粒子滤波
早在20世纪50年代，Hammersley便采用基于序贯重要性采样（Sequentialimportancesampling,SIS）的蒙特卡洛方法解决统计学问题[121]。
20世纪60年代后期，Handschin与Mayne使用序贯蒙特卡洛方法解决自动控制领域的相关问题。
20世纪70年代，Handschin、Akashi
以及Zaritskii等学者的一系列研究工作使得序贯蒙特卡洛方法得到进一步发展。
限于当时的计算能力以及算法本身存在的权值退化问题，序贯重要性采样算法没有受到足够重视，在随后较长一段时间内进展较为缓慢。
直到20世纪80年代末，计算机处理能力的巨大进展使得序贯蒙特卡洛方法重新受到关注。
Tanizaki、Geweke等采用基于重要性采样的蒙特卡洛方法成功解决了一系列高维积分问题。
Smith与Gelfand提出的采样-重采样思想为Bayesian推理提供了一种易于实现的计算策略。
随后，Smith与Gordon等人合作，于20世纪90年代初将重采样（Resampling）步骤引入到粒子滤波中，在一定程度上解决了序贯重要性采样的权值退化问题，并由此产生了第一个可实现的SIR（Samplingimportanceresampling）粒子滤波算法（Bootstrap滤波），从而掀起粒子滤波的研究热潮。
美国海军集成水下监控系统中的Nodestar
便是粒子滤波应用的一个实例。
进入21世纪，粒子滤波器成为一个非常活跃的研究领域，Doucet、Liu、Arulampalam等对粒子滤波的研究作了精彩的总结，IEEE出版的论文集
“SequentialMonteCarloMethodsinPractice对粒子滤波器”进行了详细介绍。
贝叶斯重要性采样
蒙特卡洛模拟是一种利用随机数求解物理和数学问题的计算方法，又称为计算机随机模拟方法。
该方法源于第一次世界大战期间美国研制原子弹的曼哈顿计划，著名数学家冯诺伊曼作为该计划的主持人之一，用驰名世界的赌城，摩纳哥的蒙特卡洛来命名这种方法。
蒙特卡洛模拟方法利用所求状态空间中大量的样本点来近似逼近待估计变量的后验概率分布，如图2.2所示，从而将积分问题转换为有限样本点的求和问题。
粒子滤波算法的核心思想便是利用一系列随机样本的加权和表示后验概率密度，通过求和来近似积分操作。
假设可以从后验概率密度p（xk|Yk）中抽取N个独立同分布的随机样本xk（i），i1,,N，则有
1N（i）
p（xk|Yk）N1i1（xkxk（i））（0.1）
这里xk为连续变量，（x-xk）为单位冲激函数（狄拉克函数），即（x-xk）0,xxk，
且（x）dx1。
当xk为离散变量时，后验概率分布P（xk|Yk）可近似逼近为
1N（i）P（xk|Yk）（xkxk（i））（0.2）
Ni1
其中，（xkxk（i））1,xkxk（i）;（xkxk（i））0,xkxk（i）。
图2.1经验概率分布函数
Fig.2.1Empiricalprobabilitydistributionfunction
设xk（i）为从后验概率密度函数p（xk|Yk）中获取的采样粒子，则任意函数f（xk）的期望
估计可以用求和方式逼近，即
1N（i）
E[f（xk）|Yk]f（xk）p（xk|Yk）dxk1f（xk（i））（0.3）
Ni1蒙特卡洛方法一般可以归纳为以下三个步骤：
（1）构造概率模型。
对于本身具有随机性质的问题，主要工作是正确地描述和模拟这个概率过程。
对于确定性问题，比如计算定积分、求解线性方程组、偏微分方程等问题，采用蒙特卡洛方法求解需要事先构造一个人为的概率过程，将它的某些参量视为问题的解。
（2）从指定概率分布中采样。
产生服从己知概率分布的随机变量是实现蒙特卡洛方法模拟试验的关键步骤。
（3）建立各种估计量的估计。
一般说来，构造出概率模型并能从中抽样后，便可进行现模拟试验。
随后，就要确定一个随机变量，将其作为待求解问题的解进行估计。
在实际计算中，通常无法直接从后验概率分布中采样，如何得到服从后验概率分布的随机样本是蒙特卡洛方法中基本的问题之一。
重要性采样法引入一个已知的、容易采样的重要性概率密度函数q（xk|Yk），从中生成采样粒子，利用这些随机样本的加权和来逼近后验滤波概率密度p（xk|Yk），如图2.3所示。
令｛xk（i）,wk（i）,i1,.N｝表示一支撑点集，其中xk（i）为是k时刻第i个粒子的状态，其相应的权值为w（ki），则后验滤波概率密度可以表示为
N
p（xk|Yk）wk（i）（xkxk（i））（0.4）
i1
其中，
p（xk（i）|Yk）q（xk（i）|Yk）
1N
N1i1f（xk（i））wk（i）（0.6）
序贯重要性采样算法
在基于重要性采样的蒙特卡洛模拟方法中，估计后验滤波概率需要利用所有的观测数据，每次新的观测数据来到都需要重新计算整个状态序列的重要性权值。
序贯重要性采样作为粒子滤波的基础，它将统计学中的序贯分析方法应用到的蒙特卡洛方法中，从而实现后验滤波概率密度的递推估计。
假设重要性概率密度函数q（x0:
k|y1:
k）可以分解为
q（x0:
k|y1:
k）q（x0:
k1|y1:
k1）q（xk|x0:
k1,y1:
k）（0.7）
设系统状态是一个马尔可夫过程，且给定系统状态下各次观测独立，则有
k
p（x0:
k）p（x0）p（xi|xi1）（0.8）
k
p（y1:
k|x1:
k）i1p（yi|xi）（0.9）
粒子权值wk（i）的递归形式可以表示为
通常，需要对粒子权值进行归一化处理，即
序贯重要性采样算法从重要性概率密度函数中生成采样粒子，并随着测量值的依次到来递推求得相应的权值，最终以粒子加权和的形式来描述后验滤波概率密度，进而得到状态估计。
序贯重要性采样算法的流程可以用如下伪代码描述：
[{xk（i）,wk（i）}iN1]SIS（{xk（i）1,wk（i）1}iN1,Yk）
Fori=1:
N
（1）时间更新，根据重要性参考函数q（xk（i）|x0（:
ik）1,Yk）生成采样粒子xk（i）；
（2）量测更新，根据最新观测值计算粒子权值wk（i）；
EndFor
粒子权值归一化，并计算目标状态。
为了得到正确的状态估计，通常希望粒子权值的方差尽可能趋近于零。
然而，序贯蒙特
卡洛模拟方法一般都存在权值退化问题。
在实际计算中，经过数次迭代，只有少数粒子的权值较大，其余粒子的权值可忽略不计。
粒子权值的方差随着时间增大，状态空间中的有效粒子数较少。
随着无效采样粒子数目的增加，使得大量的计算浪费在对估计后验滤波概率分布几乎不起作用的粒子更新上，使得估计性能下降。
通常采用有效粒子数Neff来衡量粒子权
值的退化程度，即
NeffN/（1var（w*k（i）））（0.13）
w*k（i）q（xp（（i）x|kx（|i）y,1:
ky））（0.14）q（xk|xk1,y1:
k）
有效粒子数越小，表明权值退化越严重。
在实际计算中，有效粒子数Neff可以近似为
N?
effN1（0.15）
（wk（i））2
i1
在进行序贯重要性采样时，若N?
eff小于事先设定的某一阈值，则应当采取一些措施加以控制。
克服序贯重要性采样算法权值退化现象最直接的方法是增加粒子数，而这会造成计算量的相应增加，影响计算的实时性。
因此，一般采用以下两种途径：
（1）选择合适的重要
性概率密度函数；
（2）在序贯重要性采样之后，采用重采样方法。
重要密度函数的选择
重要性概率密度函数的选择对粒子滤波的性能有很大影响，在设计与实现粒子滤波器的过程中十分重要。
在工程应用中，通常选取状态变量的转移概率密度函数p（xk|xk1）作为重要性概率密度函数。
此时，粒子的权值为
wk（i）wk（i）1p（yk|xk（i））（0.16）转移概率的形式简单且易于实现，在观测精度不高的场合，将其作为重要性概率密度函数可以取得较好的滤波效果。
然而，采用转移概率密度函数作为重要性概率密度函数没有考虑最新观测数据所提供的信息，从中抽取的样本与真实后验分布产生的样本存在一定的偏差，特别是当观测模型具有较高的精度或预测先验与似然函数之间重叠部分较少时，这种偏差尤为明显。
选择重要性概率密度函数的一个标准是使得粒子权值{wk（i）}iN1的方差最小。
Doucet等给
出的最优重要性概率密度函数为
q（xk（i）|xk（i）1,yk）p（xk（i）|xk（i）1,yk）
p（yk|xk（i）,xk（i）1）p（xk（i）|xk（i）1）
p（yk|xk（i）1）
p（yk|xk（i））p（x（ki）|xk（i）1）（0.17）（i）（0.17）p（yk|xk（i）1）
此时，粒子的权值为
wk（i）wk（i）1p（yk|x（ki）1）（0.18）
以p（xk（i）|x（ki）1,yk）作为重要性概率密度函数需要对其直接采样。
此外，只有在xk为有
限离散状态或p（xk（i）|xk（i）1,yk）为高斯函数时，p（yk|xk（i）1）才存在解析解。
在实际情况中，构造最优重要性概率密度函数的困难程度与直接从后验概率分布中抽取样本的困难程度等同。
从最优重要性概率密度函数的表达形式来看，产生下一个预测粒子依赖于已有的粒子和最新的观测数据，这对于设计重要性概率密度函数具有重要的指导作用，即应该有效利用最新的观测信息，在易于采样实现的基础上，将更多的粒子移动到似然函数值较高的区域，如图2.4所示。
图2.3移动粒子至高似然区域
Fig.2.3Movethesamplesinthepriortoregionsofhighlikelihood
辅助粒子滤波算法利用k时刻的信息，将k1时刻最有前途（预测似然度大）的粒子扩展到k时刻[137]，从而生成采样粒子。
与SIR滤波器相比，当粒子的似然函数位于先验分布的尾部或似然函数形状比较狭窄时，辅助粒子滤波能够得到更精确的估计结果。
辅助粒子滤波引入辅助变量m来表示k1时刻的粒子列表，应用贝叶斯定理，联合概率密度函数p（xk,m|y1:
k）可以描述为
p（xk,m|y1:
k）p（yk|xk）p（xk,m|y1:
k1）
p（yk|xk）p（xt|m,y1:
k1）p（m|y1:
k1）
p（yk|xkm）p（xk|xkm1）wkm1（0.19）
生成{xk（i）,m（i）}iN1的重要性概率密度函数q（xk,m|x0:
k1,y1:
k）为
q（xk,m|x0:
k1,y1:
k）p（yk|km）p（xk|xkm1）wkm1（0.20）
其中km为由{xk（i）1}iN1预测出的与xk相关的特征，可以是采样值kmp（xk|xkm1）或预测均值kmE{xk|xkm1}。
定义q（xk|m,y1:
k）p（xk|xkm1），由于
q（xk,m|y1:
k）q（xk|m,y1:
k）q（m|y1:
k）（0.21）
则有
q（m|y1:
k）p（yk|km）wkm1（0.22）此时，粒子权值wk（i）为
w（i）wm（i）p（yk|xk（i））p（xik|xkm）p（yk|xk（i））（0.23）wkwkmm（i）（0.23）
q（xk,m|x0m:
k1,yk）p（yk|km）
采用局部线性化的方法来逼近p（xk|xk1,yk）是另一种提高粒子采样效率的有效方法。
扩展Kalman粒子滤波与Uncented粒子滤波算法在滤波的每一步迭代过程中，首先利用最新观测值，采用UKF或者EKF对各个粒子进行更新，得到随机变量经非线性变换后的均值和方差，并将它作为重要性概率密度函数[138]。
另外，利用似然函数的梯度信息，采用牛顿迭代[139]或均值漂移[140]等方法移动粒子至高似然区域，也是一种可行的方案，如图2.5所示。
以上这
些方法的共同特点是将最新的观测数据融入到系统状态的转移过程中，引导粒子到高似然区域，由此产生的预测粒子可较好地服从状态的后验概率分布，从而有效地减少描述后验概率密度函数所需的粒子数。
{xk（i）1,N1}{xk（i）,N1}{x（ki）,w（ki）}{xk（i）,wk（i）}
{x（ki）,N1}
图2.4结合均值漂移的粒子滤波算法
Fig.2.4Particlefiltercombinedwithmeanshift
重采样方法
针对序贯重要性采样算法存在的权值退化现象，Gordon等提出了一种名为Bootstrap的粒子滤波算法。
该算法在每步迭代过程中，根据粒子权值对离散粒子进行重采样，在一定程度上克服了这个问题。
重采样方法舍弃权值较小的粒子，代之以权值较大的粒子。
重采样过程在满足p（xk（i）xk（i））wk（i）条件下，将粒子集合{xk（i）,wk（i）}1N更新为{x（ki）,1/N}1N。
重采样策略包括固定时间间隔重采样与根据粒子权值进行的动态重采样。
动态重采样通常根据当前的有效粒子数或最大与最小权值比来判断是否需要进行重采样。
常用的重采样方法包括多项
式（Multinomialresampling）重采样、残差重采样（Residualresampling）、分层重采样（Stratifiedresampling）与系统重采样（Systematicresampling）等。
残余重采样法具有效率高、实现方便的特点。
设NiNwk（i），其中为取整操作。
残余重采样采用新的权值
N
w*k（i）Nk1Nwk（i）Ni选择余下的NkNNi个粒子，如图2.6所示。
残余重采样
i1
的主要过程为
（1）计算剩余粒子的权值累计量j,j1,,Nk。
（2）生成Nk在个[0，1]区间均匀分布的随机数{l}lN?
k1；
（3）对于每个i，寻找归一化权值累计量大于或等于i的最小标号m，即
m1lm。
当i落在区间[m1,m]时，xkm被复制一次。
这样，每个粒子xk（i）经重采样后的个数为步骤（3）中被选择的若干粒子数目与Ni之和。
图2.5残差重采样
Fig.2.5ResidualResampling
重采样并没有从根本上解决权值退化问题。
重采样后的粒子之间不再是统计独立关系，给估计结果带来额外的方差。
重采样破坏了序贯重要性采样算法的并行性，不利于VLSI硬件实现。
另外，频繁的重采样会降低对测量数据中野值的鲁棒性。
由于重采样后的粒子集中包含了多个重复的粒子，重采样过程可能导致粒子多样性的丧失，此类问题在噪声较小的环境下更加严重。
因此，一个好的重采样算法应该在增加粒子多样性和减少权值较小的粒子数目之间进行有效折衷。
图2.7为粒子滤波算法的示意图，该图描述了粒子滤波算法包含的时间更新、观测更新和重采样三个步骤。
k1时刻的先验概率由N个权值为1/N的粒子xk（i）1近似表示。
在时间更新过程中，通过系统状态转移方程预测每个粒子在k时刻的状态xk（i）。
经过观测值后，更新粒子权值wk（i）。
重采样过程舍弃权值较小的粒子，代之以权值较大的粒子，粒子的权值被重新设置为1/N。
图2.6SIR算法示意图
Fig.2.6SIRalgorithm
标准的粒子滤波算法流程为：
（1）粒子集初始化，k0：
对于i1,2,,N，由先验p（x0）生成采样粒子{x0（i）}iN1
（2）对于k1,2,，循环执行以下步骤：
①重要性采样：
对于i1,2,,N，从重要性概率密度中生成采样粒子{xk（i）}iN1，计算粒子权值wk（i），并进行归一化；
②重采样：
对粒子集{x（ki）,wk（i）}进行重采样，重采样后的粒子集为{x（ki）,1/N}；
N
③输出：
计算k时刻的状态估计值：
x?
kxk（i）w（ki）。
i1
粒子滤波中的权值退化问题是不可避免的。
虽然重采样方法可以在一定程度上缓解权值退化现象，但重采样方法也会带来一些其它的问题。
重采样需要综合所有的粒子才能实现，限制了粒子滤波的并行计算。
另外，根据重采样的原则，粒子权值较大的粒子必然会更多的被选中复制，经过若干步迭代后，必然导致相同的粒子越来越多，粒子将缺乏多样性，可能出现粒子退化现象，从而使状态估计产生较大偏差。
针对粒子退化问题，一个有效的解决方法是增加马尔可夫蒙特卡洛（Markovchainmontecarlo,MCMC）移动步骤[141]。
马尔可夫链蒙特卡洛方法（如Gibbs采样、Metropolis-Hastings采样等）利用不可约马尔可夫过程可逆平稳分布的性质，将马尔可夫过程的平稳分布视为目标分布，通过构造马尔可夫链产生来自目标分布的粒子。
粒子退化问题是由于重采样使得粒子过分集中在某些状态上而导致的，对重采样后的粒子进行马尔可夫跳转可以提高粒子群的多样性，同时保证跳转后的粒子同样能够准确的描述既定的后验分布。
设粒子分布服从后验概率p（x0:
k|y1:
k），实施核为k（x0:
k|x0:
k）的马尔科夫链变换之后，若满足
k（x0:
k|x0:
k）p（x0:
k|y1:
k）dx0:
kp（x0:
k|y1:
k）（0.24）
便可以得到一组满足既定后验概率分布的粒子集合，且这组新的粒子可能移动到状态空间中更为有利的位置。
qq（（xx（k*ki）））p
升级会员 