（交通信息工程及控制专业论文）改进粒子滤波算法在FPGA中的研究与实现.pdf

中文摘要 摘要：粒子滤波是上世纪9 0 年代发展起来的一种基于蒙特卡罗方法和递推贝叶斯 估计的新滤波方法，在处理非线性、非高斯系统的参数估计和状态滤波方面具有 独到的优势。但是其复杂的算法结构、庞大的计算量和缓慢的运算速度限制了其 在实时系统中的应用。本文研究的目的是降低粒子滤波算法的复杂度，提高算法 运算速度，设计一种运算速度快、性能可靠、占用硬件资源少的粒子滤波器，使 其能应用于实时系统中。 现场可编程门阵歹d ( f i e l dp r o g r a m m a b l eg a t ea r r a y ，f p g a ) 是一种硬件逻辑器 件，可以执行真j 下意义上的并行运算。s p a r t a n 3 系列是x i l i n x 公司生产的高性价 f p g a ，该系列根据用户的实际需求，分为具有不同侧重点的f p g a ，女i s p a r t a n 3 e 、 s p a r t a n 3 a 等。本文选取s p a r t a n 3 a 作为粒子滤波硬件实现的目标器件。 本文首先介绍粒子滤波的课题背景和国内外的研究现状。然后详细介绍了粒子 滤波算法的相关理论及基本原理。接着以二维被动目标跟踪为系统模型，给出了 粒子滤波算法的基本流程。通过分析算法的复杂度和并行性，对权值归一化步骤 和重采样算法两方面进行了改进，降低了粒子滤波算法的复杂度，提高了运算速 度，以被动定位系统中目标跟踪为例进行仿真，验证改进后算法的正确性。基于 改进后的粒子滤波算法，利用f p g a 开发软件i s e 进行算法功能模块的设计，验证 了设计的有效性。最后，连接各个模块，生成可下载的流文件，在s p a r t a n 3 a 开发 板上进行算法的验证。 本文在f p g a 中设计并实现了改进的粒子滤波算法。波形图仿真和f p g a 资源使 用情况报告表明这种算法器具有速度快、占用资源少的特点，能广泛适用于目标 跟踪、图像处理、参数估计等实时系统中。 关键词：粒子滤波；f p g a , 目标跟踪；s p a r t a n 3 a 分类号：u 2 8 3 1 a bs t r a c t a b s t r a c t ：d e v e l o p i n gi n9 0 sl a s tc e n t u r y , p a r t i c l ef i l t e r , an e wf i l t e rm e t h o db a s e d o nm o n t ec a r l oa n dr e c u r s i v eb a y e s i a ne s t i m a t i o n ，h a ss p e c i a la d v a n t a g e si nd e a l i n g w i t ht h es t a t ea n dt h ep a r a m e t e re s t i m a t i o ni nt h en o n l i n e a ra n dn o n g a u s s i a ns y s t e m h o w e v e r , t h ed i s a d v a n t a g e so fc o m p l e xa l g o r i t h ma r c h i t e c t u r e , e n o r m o u sc o m p u t a t i o n s a n dl o ws p e e dh a v er e s t r i c t e di t si m p l e m e n t a t i o ni nr e a l t i m es y s t e m t h i st h e s i sa i m s t or e d u c et h ec o m p l e x i t yo ft h ep a r t i c l ef i l t e ra l g o r i t h ma n di n c r e a s et h ec o m p u t a t i o n s p e e d ，t h e n ，ap a r t i c l ef i l t e rw h i c hh a sh i g hc o m p u t es p e e d ，r e l i a b i l i t ya n du s e sl e s s h a r d w a r er e s o u r c ei sd e s i g n e df o r u s i n gi nt h er e a lt i m es y s t e m f i e l dp r o g r a m m a b l eg a t ea r r a yi sah a r d w a r el o g i cd e v i c e , w h i c hi m p l e m e n t st h e r e a lp a r a l l e lc o m p u t a t i o n t h es p a r t a n - 3s e r i e so fx i l i n xf p g a i sh i g l lc o s t e f f e c t i v e a c c o r d i n gt ot h ea c t u a ln e e d so fu s e r sa n dd i f f e r e n te m p h a s i si nt h ea p p l i c a t i o n ，x i l i n x h a sd e s i g n e dm a n yd i f f e r e n ts e r i e sb a s e do nt h es a m e t e c h n o l o g yo fs p a r t a n 3 ，s u c ha s s p a r t a n - 3e ，s p a r t a n - 3 aa n ds oo n i nt h i st h e s i s ，t h es p a r t a n 一3 ap l a t f o r mi ss e l e c t e df o r t h eh a r d w a r er e a l i z a t i o no fi m p r o v e dp a r t i c l ef i l t e ra l g o r i t h m t h i st h e s i s ，a tf i r s t ，i n t r o d u c e st h ep a r t i c l ef i l t e rr e s e a r c ha th o m ea n da b r o a d s e c o n d l y , i td e s c r i b e st h ec o r r e l a t i v ec o n t e n t sa n dt h eb a s i ct h e o r yo fp a r t i c l ef i l t e ri n d e t a i l t h e nt a k i n gt h eb e a r i n g - o n l ys y s t e ma st h em o d e l ，t h eb a s i cp r o c e s so f p a r t i c l e f i l t e ri sg i v e n 。t h r o u g ha n a l y z i n gt h ec o m p l e x i t ya n d p a r a l l e lo fa l g o r i t h m ，t h es t e p so f n o r m a l i z e dw e i g h ta n dr e s a m p l i n ga r ei m p r o v e d u s i n gb e a t i n g - o n l ys y s t e mo b j e c t t r a c k i n ga se x a m p l e , t h ei m p r o v e da l g o r i t h mi ss i m u l a t e dt ov e r i f yt h ec o r r e c t n e s s f i n a l l y , t h ef p g as o f t w a r ei s ei su s e dt od e s i g nt h em o d u l eo fi m p r o v e dp a r t i c l ef i l t e r a l g o r i t h ma n dt h ee f f e c t i v e n e s so ft h ed e s i g ni sv e r i f i e d t h e ne a c hm o d u l ei sc o n n e c t e d t og e n e r a t et h ef i l ef o rd o w n l o a da n dt h ei m p r o v e dp a r t i c l ef i l t e ra l g o r i t h mi sv e r i f i e di n s p a r t a n - 3 ad e v e l o p m e n tb o a r d i nt h i st h e s i s ，t h e i m p r o v e dp a r t i c l ef i l t e ra l g o r i t h mh a sb e e nd e s i g n e da n d i m p l e m e n t e do nf p g ap l a t f o r m t h ew a v e f o r ms i m u l a t i o na n dt h er e p o r to fr e s o u r c e u s i n gi n d i c a t et h a tt h i si m p r o v e da l g o r i t h m ，w i t hi t sc h a r a c t e r i s t i c so fh i g hs p e e d ，u s i n g s m a l lr e s o u r c e ，c a nb eu s e di n r e a l t i m es y s t e m ，s u c ha so b j e c t t r a c k i n g , i m a g e p r o c e s s i n g ，p a r a m e t e re s t i m a t i o ne t c k e y w o r d s ：p a r t i c l e f i l t e r ；f p g a ；o b j e c tt r a c k i n g ；s p a r t a n 3 a c l a s s n 0 ：i j 2 8 3 1 独创性声明 本人声明所呈交的学位论文是本人在导师指导下进行的研究工作和取得的研 究成果，除了文中特别加以标注和致谢之处外，论文中不包含其他人已经发表或 撰写过的研究成果，也不包含为获得北京交通大学或其他教育机构的学位或证书 而使用过的材料。与我一同工作的同志对本研究所做的任何贡献均已在论文中作 了明确的说明并表示了谢意。 学位论文作者签名：闾穿绚t 签字日期：弘刀年月，7 日 学位论文版权使用授权书 本学位论文作者完全了解北京交通大学有关保留、使用学位论文的规定。特 授权北京交通大学可以将学位论文的全部或部分内容编入有关数据库进行检索， 并采用影印、缩印或扫描等复制手段保存、汇编以供查阅和借阅。同意学校向国 家有关部门或机构送交论文的复印件和磁盘。 ( 保密的学位论文在解密后适用本授权说明) 学位敝作者虢1 7 司宁偷 签字日期：w 矽年彦月，7 日 。导师签名：，) 锄a 签字日期：_ 年舌月f ，7 日 致谢 本论文的工作是在我的导师张三同教授的悉心指导下完成的，张三同教授严 谨的治学态度和科学的工作方法给了我极大的帮助和影响。在此衷心感谢两年来 张三同老师对我的关心和指导。 张三同教授悉心指导我们完成了实验室的科研工作，在学习上和生活上都给 予了我很大的关心和帮助，在此向张三同老师表示衷心的谢意。 张三同教授对于我的科研工作和论文都提出了许多的宝贵意见，在此表示衷 心的感谢。 在实验室工作及撰写论文期间，薛志勇、陈通、朱韬等同学对我论文中的研 究工作给予了热情帮助，在此向他们表达我的感激之情。 另外也感谢家人和我的朋友们，他们的理解和支持使我能够在学校专心完成 我的学业。 1 引言 1 1 课题背景 运动目标的定位在军事和民用系统中都有着很重要的应用，尤其是在军事中， 全球定位系统( g l o b a lp o s i t i o n i n gs y s t e m ，g p s ) 由于能够快速准确定位，在战争 中产生了决定性的影响，例如在美国发动的几次海湾战争中都很好的体现了这一 点。但是g p s 信号很弱，易受干扰。一个定位系统就是惯性导航系统，目前较为 精确的定位则是两者的结合。随着控制论的发展，已经研究出来很多的应用方法， 例如卡尔曼滤波以及几年刚刚发展起来的粒子滤波，这几种方法在多传感器系统 中都有应用。研究中很多学者采用的研究条件是针对线性系统，因此卡尔曼滤波 得到了很大的应用。但是在实际处理数据时，需要使用状态空间表示法对过程建 模，使用的多为离散系统，而且跟踪系统中，目标位置的测量值是在与传感器位 置相关的极坐标系下得到的，因此，目标跟踪是一个非线性问题。二十世纪七十 年代以来，人们提出许多方法来解决非线性滤波问题，如扩展卡尔曼滤波( t h e e x t e n d e dk a l m a nf i l t e r ) ，无偏一致可修正测量值卡尔曼滤波( t h ed e b i a s e d c o n s i s t e n tc o n v e r t e dm e a s u r e m e n t sk a l m a nf i l t e r ) 等等，但是所有这些方法都依赖 于高斯假设，且状态方程和观测方程都为线性方程。而在实际中，由于环境的干 扰、测量设备精确度低等原因，得到的测量数据往往包含各种噪声，其中大部分 是非高斯分布的。在观测噪声和过程噪声为非高斯分布的情况下，尤其是当观测 噪声分布呈厚尾( h e a v y - t a i l e d ) 形状时，上面所提及的方法性能严重下降，甚至 得到错误的结果，以至无法应用。而粒子滤波的特点就是能够同时在非线性和非 高斯分布的模型上应用。因此，粒子滤波引起了相关领域研究者的广泛注意，并 被迅速应用到实际的运动目标跟踪等相关应用中。 粒子滤波( p a r t i c l ef i l t e r , p f ) 是上世纪9 0 年代发展起来的一种基于蒙特卡罗 方法( m o n t ec a r l om e t h o d s ，m c m ) 和递推贝叶斯估计的用于非线性、非高斯系 统的滤波方法。粒子滤波的基本思想是：首先依据系统状态向量的经验条件分布， 在状态空间抽样产生一组随机样本集合，这些样本集合称为“粒子 ；然后根据 观测值不断地调整粒子的权重大小和样本位置；最后，通过调整后的粒子的信息 修正最初的经验条件分布，估计出系统状态和参数。该算法是一种递推滤波算法， 原则上可以用来估计任意非线性、非高斯随机系统的状态和参数，有效克服了k f 、 e k f 等滤波算法的缺点。一般来说，粒子滤波以蒙特卡罗模拟方法来实现递推贝 叶斯滤波的计算量大于卡尔曼滤波等数学方程求解形式。但是，粒子滤波可以采 用并行结构计算实现，随着计算机速度的提高以及并行计算机的出现，粒子滤波 比传统贝叶斯滤波器( 卡尔曼滤波器、网格滤波器等) 更具有实用价值。 1 2 粒子滤波在国内外研究现状与趋势 1 2 1 国内外文献综述 国外对粒子滤波的研究起步较早，粒子滤波技术的思想最早可以追溯到二十世 纪五十年代，当时h a m e r s l e y 等人就提出一种被称为s e q u e n t i a li m p o r t a n ts a m p l i n g ( s i s ) 】的方法，它通过离散的随机测度逼近概率分布，应用在物理和工程领域。 七十年代粒子滤波算法得到了更进一步的发展，各个领域的学者继续沿着s i s 的 思路研究，提出了一系列改进的s i s 算法【2 】【3 1 。但是，s i s 算法容易导致粒子退化 现象( p a r t i c l ed e g e n e r a c y ) ，影响了它在实际中的推广应用。直到1 9 9 3 年，g o r d o n 等人提出了采样重要性重采样算法( s a m p l i n gi m p o r t a n c er e s a m p l i n g ，s i r ) 【4 】，才 基本解决了粒子退化问题，从而奠定了粒子滤波算法的基础。从此，粒子滤波被 广泛关注，并取得了重大进展，提出了一些重要的s i r 算法。如，多项式重采样 算法( m u l t i n o m i a lr e s a m p l i n g ) 1 5 1 ，分层重采样算法( s t r a t i f i e dr e s a m p l i n g ) 【6 】， 残差重采样算法( r e s i d u a lr e s a m p l i n g ) 【7 】，系统重采样算法( s y s t e m a t i cr e s a m p l i n g ) 【8 】等。如今，现代计算机技术得到了长足的发展，同时由于粒子滤波方法本身具有 的巨大潜力而使得其成为当前一个非常活跃的研究领域【9 】。 国际上，粒子滤波被广泛应用于信道通信、目标跟踪、导航与制导、图像处理、 故障诊断、参数估计、模式识别等方面。但是在国内，关于粒子滤波的研究起步 较晚，相关的介绍也很少。目前国内对粒子滤波的研究水平与国外相比尚有较大 差距，但各相关领域的学者都在孜孜不倦的研究探讨，相信会逐渐接近并超过国 外的研究水平。 1 2 2 主要研究方向 粒子滤波算法摆脱了解决非线性滤波问题中随机量必须满足高斯分布的制约 条件，并在一定程度上解决了粒子数匮乏问题，因此粒子滤波在处理非线性非高 斯问题上与卡尔曼滤波相比有着无可比拟的优势。它原则上可适用于任意的非线 性非高斯随机系统的状态估计，有效地克服了k a m a n 滤波的缺点。但是粒子滤波 自身也存在一些缺点，如算法复杂，计算量大，算法的稳定性，粒子数的选择等 问题。近年来，随着科研人员不断地投入，计算机处理能加的不断加强，粒子滤 2 波的性能得到了不断的改善。 粒子滤波的重要性函数的选择是粒子滤波研究方面一个重要的问题。一些研究 者提出拒绝采样法和引入辅助变量的方法来逼近最优重要性函数，但是这些方法 计算量都非常大。在目前，用高斯分布作为次最优的重要性函数基本可以解决这 一问题。在采样阶段。重要性函数取为状态转移先验概率密度函数，由于没有利 用新的观测，条件概率密度传播算法通常需要大量的粒子才能准确地表达状态后 验分布。尤其是当新的观测出现在先验密度的尾部或似然函数呈尖峰形状时，需 要更多的采样。由此提出从次最优的高斯分布中采样得到新粒子的方法，而不是 像条件概率密度传播算法那样从状态转移先验密度中采样。它的优点是能够将粒 子引导到似然函数值比较大的区域，因此，可以大大减少算法需要的采样数。因 此，对粒子滤波算法的重要性函数的选择是粒子滤波算法研究的主要方向之一。 粒子滤波s i s 算法中普遍存在的问题是采样粒子的退化现象( d e g e n e r a c y p h e n o m e n o n ) 。即经过若干次迭代后，除一个粒子外，其余粒子的权值都变的很小 可以忽略不计。因为权重的方差随时间增大，所以退化现象无法避免。这就意味 着大量的计算浪费在那些权值极小的粒子上，而这些粒子对估计的贡献几乎为零。 退化问题主要使用重采样算法来解决，目前减小粒子权重方差的方法大约有以下 几类【1 0 】： ( 1 ) 对粒子点进行重采样( s a m p l i n gi m p o r t a n c er e s a m p i n g ，s i r ) ( 2 ) 马尔可夫m o n t ec a r l o 方法( m c m c ) ( 3 ) r a o b l a c k w e l l i s e d 方法 尽管重采样在一定程度上解决了退化问题，但是由于重采样是根据权值来进行 的，这就导致采样后的粒子由大量重复的点组成，因而失去了多样性。而且如果 系统噪声较小，那么重采样后的粒子会集中在少数几个点上，因此采样粒子就缺 乏多样性。怎样在解决退化问题的同时避免粒子集中在少数几个点上缺乏多样性 是粒子滤波研究的主要方向之一。 经典的重采样算法虽然在克服粒子数匮乏方面有很好的效果，但计算量随着粒 子数的增加而成级数增加，在有实时性要求的应用中，粒子滤波的可实现性成为 主要问题。将成熟的多种不同寻优方法引入到重采样过程，以便更快地提取到反 映系统概率特征的典型“粒子”，逐渐成为重采样算法研究的重点。而为了提高粒 子滤波算法的预算速度和鲁棒性，研究粒子滤波的硬件实现方法尤为关键。粒子 滤波硬件实现的基本思想是：将粒子滤波划分为初始采样、重采样、状态更新等 不同过程，利用流水线实现分时并行处理。但实用化粒子滤波算法器尚未研制成 功。 由于粒子滤波算法出现较晚，相关理论还不够成熟，并且碍于学科间的差异， 3 粒子滤波仅能应用于有限的几个领域。但是，通过分析算法思想我们可以看出， 粒子滤波算法对于非线性状态估计系统建立的相关模型有巨大的优势，在采用 k a l m a n 滤波效果不理想的情况下，便可尝试采用粒子滤波算法。因此，如何将粒 子滤波算法应用与更广泛的领域也是粒子滤波算法研究的主要方向之一。 1 2 3 主要应用领域 粒子滤波技术在非线性、非高斯系统表现出来的优越性，决定了它的应用范围 非常广泛。另外，粒子滤波器的多模态处理能力，也是它应用广泛的原因之一。 目前，粒子滤波的应用主要集中在以下3 个领域。 ( 1 ) 粒子滤波用于目标跟踪、导航与定位 目标跟踪【l l 】问题是基于自身位置来衡量其它目标的方位和距离。在进行导航、 定位和目标跟踪时，通过选择坐标系可以保证模型的状态方程是线性的，而观测 方程在该坐标系下一般是非线性的。汽车和航空应用都表明粒子滤波相对卡尔曼 滤波算法有更多优点，精度有很大提高，同时计算量也偏大。测试已经表明其精 度可与g p s 相比，且有更高的完整性。此外，还可实现以下应用：飞行器的完整 导航及飞行器和汽车的目标跟踪；通过地图匹配进行汽车定位；通过无线电频率 实现汽车定位；通过地图匹配或地形匹配来实现飞行器定位；目标跟踪；实现导 航并进行目标跟踪；预测自己车和其它车的相对位置，实现汽车防撞。 ( 2 ) 粒子滤波用于故障诊断 粒子滤波算法具有同时估计连续状态和离散状态的特点，k a d i r k a m a n a t h a n 等 人提出了一种可用于混合系统状态监测与诊断的方法【1 2 1 。仿真结果也证实该方法 是可行的，此方法对状态估计效果有较好的改善。故障检测方案中的许多进展依 赖于系统是线性系统和噪声服从高斯分布这一前提，这种条件下的最优状态滤波 器是卡尔曼滤波器。因此非线性系统非高斯分布的状态估计是一个更困难的问题， 最优解决方案不能解析地表示。次优解决方案使用一些近似形式( 如e k f 中的模 型线性化) 作为残差产生器。然而，次优解决方案中近似的线性化技术可能有低 的检测率或高的虚警率。因此，将粒子滤波用于故障检测【 】【1 4 】技术将可以处理任 何非线性函数和任何噪声分布的系统，这是粒子滤波算法相比于其它算法最大的 优势。如今，高性能计算机的出现使该算法变得流行。 ( 3 ) 粒子滤波用于参数估计与系统辨识 粒子滤波自身提供了估计复杂非线性模型中未知参数的普遍方法，其基本秘诀 是将系统参数视为随机变量进行建模。在这些系统中，首先将系统分为线性部分 和非线性部分进行处理，这将使粒子滤波可在更复杂系统中应用，同时也减少了 4 那些用线性滤波可以处理的变量。当粒子数目趋近于无穷大时，其估计式收敛于 真正的后验分布。唯一问题在于粒子实际数目的限制，它取决于状态变量和系统 未知参数。随着研究的深入，粒子滤波如何用于系统辨识已经被通过相关计算公 式定性的表示出来【1 5 】【1 6 1 ，并且仿真结果证明粒子滤波可以在相关应用中代替经典 系统辨识方法。 1 3 本文研究的主要内容及目的 随着粒子滤波方法在许多领域中的成功应用，许多研究人员认为在解决所 有状态估计的滤波问题时，获得滤波性能最好的方法之一是粒子滤波算法。但是 为了得到很好的滤波性能，估计出一个状态值需要成百上千个粒子，当状态值维 数增加时，计算量更是成倍增加，这使得其运算速度及其缓慢，因此，计算负担 大、运算时间长、效率低是粒子滤波的几大缺点。现场可编程门阵列( f i e l d p r o g r a m m a b l eg a t ea r r a y ，f p g a ) 具有运算速度快、可靠性高、编程灵活等优点， 可以实现真正意义上的并行运算。理论上一个时钟周期，f p g a 输出一个结果。通 过使用f p g a 器件设计一个粒子滤波器可以有效的提高算法运算速度，可以将其 应用于很多实时性系统中。粒子滤波在目标跟踪、故障诊断、参数估计与系统辨 识等非线性、非高斯系统中具有广阔的发展前景，但计算量大、实时性差成为了 其在实际中应用的瓶颈。降低粒子滤波算法的复杂度，提高运算速度，设计一种 运算速度快、性能可靠、占用硬件资源少的粒子滤波算法器将具有重要的理论和 现实意义。 本文通过对粒子滤波的基本理论进行分析，给出了粒子滤波算法的基本流程， 并对粒子滤波算法进行分析、研究、改进，采用了一种新的重采样算法来实现重 采样，以二维被动目标跟踪系统为模型进行仿真。最后，将改进的粒子滤波算法 在f p g a 器件上实现。 主要章节安排如下： 第一章：引言。首先介绍粒子滤波的发展历程、粒子滤波国内外的主要研究 方向及应用领域，然后描述了论文的主要内容及研究目的。 第二章：粒子滤波算法原理。首先介绍粒子滤波的相关理论贝叶斯原理和蒙 特卡罗方法，然后介绍了粒子滤波算法的基本理论，最后给出了粒子滤波算法的 基本流程。 第三章：首先介绍二维被动目标系统的基本模型。以二维被动目标系统的基 本模型为例分析粒子滤波算法的各个步骤，并改进了归一化步骤，针对重采样算 法模块介绍了几种基本重采样算法，比较了几种重采样算法的优缺点，最后采用 5 了一种新的重采样算法实现重采样，最后对改进的粒子滤波算法进行了仿真实现。 第四章：首先介绍可编程逻辑器件f p g a 。介绍了可编程器件的发展情况，然 后主要介绍了x i l i n x 公司的高性价比s p a r t a n 3 系列芯片中的s p a r t a n - 3 a 系列器件 的特性、内部结构、开发流程和配置方式。其次以被动定位系统中目标跟踪模型 为实现对象设计改进粒子滤波算法的相关f p g a 模块，并最终在s p a r t a n 3 a 开发 板上验证其有效性。 第五章：总结和展望。对本文工作的总结和对粒子滤波硬件实现将来发展的 展望。 6 2 粒子滤波算法原理 粒子滤波( p a r t i c l ef i l t e r ) 是一种基于蒙特卡罗原理的贝叶斯概率滤波算法，又 称为c o n d e n s a t i o n ( c o n d i t i o n a ld e n s i t yp r o p a g a t i o n ，条件概率密度传播) 算法； b o o t s t r a pf i l t e r i n g ：i n t e r a c t i n gp a r t i c l ea p p r o x i m a t i o n s ；s e q u e n t i a lm o n t ec a r l om e t h o d s ( 序列蒙特卡罗方法) ；s i s ；s i r 等等f 1 7 1 。粒子滤波技术通过非参数化的蒙特卡罗 模拟方法来实现递推贝叶斯滤波，可以适用于任何用状态空问模型以及传统的卡 尔曼滤波表示的非线性系统，精度可以逼近最优估计【1 8 l 1 9 1 。 本章2 1 节介绍了用于求解目标状态后验概率的贝叶斯滤波理论。2 2 节简述 蒙特卡罗方法。2 - 3 节是本章的重点，详细叙述了粒子滤波基本算法、粒子的采样 和重采样算法及粒子估计运动状态的方法，并给出了粒子滤波算法的基本流程。 2 1 贝叶斯滤波原理 在统计估计理论的经典方法中，假定了所需估计的状态参数x 是确定的未知 数。但是当假定状态参数x 为随机变量，估计的是其特定的一个现实时，这被称 为贝叶斯( b a y e s i a n ) 方法【2 0 1 。贝叶斯滤波原理的实质是试图用所有已知信息来构 造系统状态变量的后验概率密度，即用系统状态转移模型预测状态的先验概率密 度，再使用最近的观测值进行修正，得到后验概率密度。 首先我们先给出一个离散状态空间模型，状态空间模型包括两个模型：一是 状态方程模型，反映动态系统在输入变量作用下在某时刻所转移到的状态；二是 输出或测量方程模型，它将系统在某时刻的输出和系统的状态及输入变量联系起 来。 状态方程模型： 、 x k = t ki x k _ 1 ，吼一lj ( 2 1 ) 测量方程模型： y k = h kl x k ，l l kj ( 2 2 ) 式中，k n 为离散时间，x k r 以为系统在k 时刻的状念向量，y k r 以为系 统在k 时刻的观测向量，它的维数一般小于状态向量。吒一。为与状态无关的零均 值白噪声，u 七为独立于系统噪声的量测噪声。 在贝叶斯框架下，公式( 2 1 ) 确定了预测当前状态的条件转移概率( 给定前 一时刻的状态和所有的观测值) ：p ( x 。l x k ，y 。：h ) 7 公式( 2 2 ) 确定了预测当前观测值的似然概率( 给定当前状态) ：p ( y 。l x 。) 假设k - l 时刻状态的后验分布已经得到，那么我们利用条件转移概率可以获得 k 时刻状态的先验分布： p ( x 七iy 一) = ，p ( x 七 x k - l , y ：。一，) p ( x 七一。iy 一。) 如七一 ( 2 3 ) 假设各个观测值是相互独立的，则有： p ( y k y 似一i ，x k ) = p ( y k k ) ( 2 4 ) 在k 时刻可以获得新的观测矢量y 七，基于贝叶斯准则可以利用似然模型来更 新先验概率分布，从而得到k 时刻状态的后验概率： p ( x kly 1 小血若掣产旺5 ， 在贝叶斯估计中，观测到数据后，后验概率密度p ( x 。iy l ：k ) 概括了我们对这个 参数的了解情况。对状态向量x 的所有现实和观测量y ，使均方根误差( m e a ns q u a r e e r r o r ，m s e ) 最小的估计量定义为最佳估计量，即所谓的贝叶斯m s e 。该估计量 是后验概率密度的均值，即x t = e ( x y i ：七) 。可以显式地表示为： e ( x 七ly l ：k ) = i g ( x 七) p ( x 七ly l ：k ) d x 七( 2 6 ) 在许多信号处理的应用问题中，接收的数据是对连续时间信号进行采样而得 到的。随着时间进展，数据源源不断，要么等所有数据采样完成再一次性全部处 理，称为批处理；要么按时间顺序进行处理，称为点处理。而贝叶斯滤波原理是 一种批处理的过程。 从式( 2 4 ) 中可以看出，在每一个新时刻当观测值到来时，后验概率密度就 被更新一次，直接计算很不方便。因此，贝叶斯滤波原理的流程为通过贝叶斯方 法来获得后验概率密度，然后通过迭代更新后验概率密度。现今，贝叶斯信号处 理已经成为解决信号检测问题的一个重要工具。 2 2 蒙特卡罗方法 近年来，基于蒙特卡罗方法【2 1 1 的滤波器也引起了人们的广泛关注，其基本思 想是产生服从后验概率密度分布的样本，并对其进行加权给出概率密度函数的近 似表示。这类方法大体可分为两类：一种是马尔可夫链的蒙特卡罗方法【2 2 1 ( m a r k o v c h a i nm o n t ec a r l o ，m c m c ) ，其中马尔可夫链给出服从感兴趣的后验概率密度分 8 布的样本，当有新的观测值到达时，m c m c 过程要重新开始，因而这是一种用于 解决批处理问题的方法；另一种就是序列蒙特卡罗方法( s e q u e n t i a lm o n t ec a r l o ， s m c ) ，是解决点处理问题的方法。从前文的介绍，我们看到状态向量x 的后验 p d f 计算为多维积分，很难直接计算得出，这可以通过蒙特卡罗方法解决1 2 3 1 。 蒙特卡洛方法依靠重复随机采样计算结果，经常应用于模拟物理或数学系统。 通过从估计概率分布中随机抽取一系列独立同分布的样本来接近真实概率分布。 当用确定性算法无法计算精确结果时，蒙特卡洛方法是最佳选择。蒙特卡罗积分 就是将积分值看成是某种随机变量的数学期望，并用抽样方法加以估计，如： l = i g ( x ) p ( x ) d x d ( 2 7 ) 其中，d 为n 维空间r 的一个区域，i 是在以d 为积分区域上的积分。p 满足：p ( x ) 0 ，i 。p ( x ) d x = l ，g ( x ) 是任意函数。通常将p 俐看成是状态变量的概 乏 率密度函数，只要积分有界，则i 可看成是颤x ) 的数学期望，即i = e g ( x ) 】。对于贝 叶斯估计问题，所关心的是状态在给定测量条件下的后验概率密度函数，即 p ( x ) = p ( x y i k ) ，令g ( x ) = x ，即可得到状态的最小方差估计。现从p ( x ) 抽取随机 变量x 的n 个样本f x k 则得到g ( ) 的平均值为： g j v = - - - 万e g ( x k ) ( 2 8 ) - k = l 可以证明g 是i 的无偏估计。根据大数定理，当 兹 中的样本相互独立，且 n 一时，样本的均值乳以概率l 收敛到i ，即： p = ( 1 i mg = ，) = l 工- - ) 0 0 若有不为零的有界方差存在，即： ( 2 9 ) ( 2 1 0 ) 存在，则在1 一口置信水平下，蒙特卡罗积分方法的误差由嚷和万决定。因 此要减少误差，要么增大样本数目n ，要么降低方差。 根据式( 2 8 ) ，由( 2 4 ) 和( 2 5 ) 两式求得状态变量x o ：k 的后验概率分布 p ( x o ：k l y l ：k ) 后，任意函数g ( x ) 的数学期望为： e ( g ( k ) ) = p ( h k ) p ( x o ：k l y 冰) d x o ：k 9 ( 2 1 1 ) 2 3 粒子滤波基本理论 粒子滤波是一种依据大数定理采用蒙特卡罗方法来求解贝叶斯估计中的积分 运算，利用重要性抽样在动态状态空间上得到一组不断更新的随机样本( 粒子) ， 来逼近估计状态的后验概率密度，这些粒子与权值一一对应。粒子滤波方法通过 非参数化的蒙特卡罗模拟方法来实现贝叶斯估计，用样本形式而不是以函数形式 对先验信息和后验信息进行描述。当样本点数增至无穷大，蒙特卡罗模拟特性与 后验概率密度的函数表示等价，从而滤波精度可以逼近最优估计。在数学处理的 方便性上，它不受线性和高斯分布的限制，原则上适用于能用状态空间模型表示 的任何非线性、非高斯系统，因此受到了高度重视。粒子滤波基本算法包括两个 基本部分：( 1 ) 顺序重要性采样算法( s e q u e n t i a li m p o r t a n c es a m p l i n g ，s i s ) ，( 2 ) 采样 重要性重采样算法( s a m p l i n gi m p o r t a n c er e s a m p l i n g , s i r ) 。 2 3 1 贝叶斯重要性采样 贝叶斯重要性采样冽( b a y e s i a ni m p o r t a n c es a m p l i n g ( b i s ) ) 是通过定义一个 参考分布( 重要性函数) g ( 黾七l y l k ) ，从重要性函数中采样，通过对采样粒子点 进行加权来近似p ( ：七l y l ：k ) 。由式( 2 1 1 ) 变形得： 地( ) = 胁七) 急器l y l k ) ( 2 1 2 ) p ( 黾肌k ) = 地掣 一翌! 当；墨! 圣q 盔! 竺! 逸盘2 p ( y 1 ：七) 将( 2 1 3 ) 式代入( 2 1 2 ) 式得： 瞰( 训= 胁七) 血皆黔七 定义权值比( 七) 为： 比( 虫) = 些掣 g 【| i y l ：k ) 1 0 ( 2 1 3 ) ( 2 1 4 ) ( 2 1 5 ) 将式( 2 1 5 ) 代入( 2 1 4 ) ，并f l - p ( y , ：七) 对于每个观测值y 为定值，得到： 瞰( ) ) = 志p ( ) 比( u g ( 阢蛾：七 p ( m ：七) = ip ( 乃：七i x o ：k ) p ( x o ：七) 氐：七 地伪2 嚣一 i p ( m ：。切( ：t ) 遨 氐 i g ( x o ：t ) 心( ：i ) q ( x o ：t l y l ：k ) i ： = = ! ：一 1w , ( x o ：i ) q ( x o ：il y l ：k ) 氐：t ：墨! 照坠。! ! 墨! 墨兰! 兰! 垒! 塑 乞( 讯| y i ) 【w , ( x o ：i ) 】 ( 2 1 6 ) ( 2 1 7 ) ( 2 1 8 ) 式中乞( 址i y 。上) 表示，样本是按照建议分布g ( i y i ：t ) 抽取的。当粒子采样点足够 多时，连续函数的期望近似于离散化。假设每一时刻采样了n 个粒子，将式( 2 1 8 ) 简化得： 寺g ( ：。) 比( 乇) e ( g ( ：女) ) 旦毒 f 一 丙l 善i v 心( 乇) ( 2 1 9 ) = g ( ：。) 愀( ：。) 式( 2 1 9 ) 中愀( ：女) 为k 时刻第i 个粒子的归一化权值( n o r m a l i z e di m p o r t a n c e w e i 曲t s ) ，所以函数g ( x ) 的数学期望近似为一个求和过程。 2 3 2 序贯重要性采样 序贯重要性采样算法s i s 是一种通过蒙特卡罗模拟实现递推贝叶斯滤波器的 技术。其核心思想是利用一系列随即样本的加权和表示所需的后验概率密度，得 到状态的估计值。当样本点数增至无穷大时，蒙特卡罗特性与后验概率密度的函 数表示等价，s i s 滤波器接近于最优贝叶斯估计。粒子滤波算法中，使用 ( 矗2 ，硝) 兰，表示系统后验概率密度函数p ( ：。iz o ：。) 的样本集合，其中 ( x 盈，i = 1 ，2 ，n ) 是粒子集合，n 为粒子数，各个粒子相应的权值为 ( 以，i = l ，2 ，) ，且满足皑= l ，假设当前状态不依赖于将来的观测，由此得 到重要性函数的另一种表达方式： q ( x o ：kly l ：k ) 2q ( x kix o ：k 1 ，y l ：k ) q ( x o ：k 1ly l ：k - i ) ( 2 2 0 ) 将( 2 2 0 ) 式代入( 2 1 5 ) 式得： 嘶j 2 掣 ：翌! 叠生匾! ! 翌! 苎；墨! q ( x o l y l ：k ) o c 剿 ( 2 2 1 ) q ( x o ：t l y l k ) 7 ，p ( y 。fx 。) p ( x 。ix ) p ( x 姚。ly 从一，) 瓯1 k 瓦j 万丽了瓦厂 ，p ( y 。ix 。) p ( x 。ix ) k - l q 瓦赢x o 五y l fix ki ：k 1 ，：kj 在给定重要性函数q ( x kix o ：k - iy 1 k ) 的条件下，式( 2 2 1 ) 提供了一个递归计 算权值的方法。重要性函数q ( x kix o ：k 1y 。：k ) 的设计是一个关键问题，它经常被近 似以简化采样。当重要性函数为后验概率密度的情况下，达到最优估计，但最优 估计一般很难实现，所以常采用次优算法，令重要性函数 g ( 吒i 吒小儿) = p ( 吒l 吒一。) ，代入式( 2 2 1 ) 得： 心= 岷一。p ( y k x k ) 2 3 2 重要性重采样 ( 2 2 2 ) s i s 算法存在的一个基本问题就是退化现象【2 5 1 ( d e g e n e r a c yp r o b l e m ) ，即经过 几步迭代递推后，许多粒子的权重变得非常小，只有少数几个粒子具有较大权值， 大量的计算则浪费在小权值粒子上【2 6 】。重采样算法s i r 是降低粒子匮乏现象的一 种方法，通过定义一个有效采样尺度m 为： = 百二一 ( 2 2 3 ) ( 以) 2 1 2 当已经下降到一个门限阂值r 时，对粒子进行重采样把那些权重很小 的粒子删除，然后集中到那些权重大的粒子上去。重采样过程就是对现有的样本 集合( ) 芒。重新采样n 次，产生新的样本集合( ) 芒l 来构成对p ( 吒i 乃：七) 的离散分 布的新的近似。 p ( 吒l 咒生) = 以万( - 4 ) ( 2 2 4 ) i = 1 图2 1 是重采样示意图，其中t - l 时刻的先验概率由若干个权值为。1 的粒子 近似表示( 图中仅画出