（电路与系统专业论文）粒子滤波算法的硬件优化设计.pdf

浙江大学硕士学位论文 摘要 摘要 粒子滤波算法在非线性、非高斯的状态估计问题中具有广阔的应用前景，但庞大的计 算量限制了其在实时系统中的应用，而硬件实现为此提供了有效的解决手段。本文针对粒 子滤波集中式和分布式硬件设计中的速度和存储问题，提出了多种优化方案。 在集中式结构中，为降低重采样的存储开销，提出了状态回写和权重存储器复用技术， 避免了传统结构中粒子索引存储器和复制因子存储器的使用；为降低采样中粒子状态的存 储需求，本文将粒子状态分解为一初始状态与状态编码表示的两状态之差，实现了状态的 压缩；为提升处理的速度，在硬件中引入了有效粒子数的判断和间隔重采样方法，降低了 重采样的发生次数，并进一步提出了一种局部权重均值比较机制，成倍提升了滤波的效率 在分布式结构中，提出了权重排序机制，包括权重全排序和部分排序，用以平衡各处 理单元内的粒子权重和，避免了粒子的再分配过程；同时提出了分层重采样方法，包括粗 粒度分层和细粒度分层，将重采样以层次形式展开，并在统计上获得了与集中式重采样相 同的结果相对于传统的结构，基于上述两种方法的分布式结构在处理速度、存储开销和 滤波精度上均达到了更好的平衡。 关键词：粒子滤波状态压缩局部权重均值比较权重排序分层重采样 i l a b s t r a c t p a r t i c l ef i l t e r sh a v es h o w ng r e a tp r o m i s ei nd e a l i n gw i t hn o n 1 i n e a ra n dn o n g a u s s i a ns t a t e e s t i m a t i o np r o b l e m s h o w e v e r , t h e i ra p p l i c a t i o n st op r a c t i c a lr e a l - t i m es c e n a r i o sa r es e v e r e l y r e s t r i c t e dd u et ot h e i ri n h e r e n tc o m p u t a t i o n a lc o m p l e x i t i e s t h e r e f o r e ，i ti sn e c e s s a r y , u n d e rs u c h c i r c u m s t a n c e s ，t ot u r nt ot h e i rh a r d w a r ei m p l e m e n t a t i o n s t h i sp a p e rp u t sf o r w a r dm u l t i p l e o p t i m i z a t i o n sf o rb o t hc e n t r a l i z e da n dd i s t r i b u t e da r c h i t e c t u r e so fp a r t i c l ef i l t e r sf r o mt h ep o i n t o fs p e e da n dm e m o r y i nc e n t r a l i z e da r c h i t e c t u r e ，w er e d u c et h em e m o r yu t i l i z a t i o no f r e s a m p l i n gb ys t a t er e w r i t i n g a n dr e u s eo ft h ew e i g h tm e m o r y , w h i c hm a k ei tu n n e c e s s a r yt ou s ed e d i c a t e di n d e xm e m o r ya n d r e p l i c a t e df a c t o rm e m o r y ；m e a n w h i l e ，w er e d u c et h em e m o r yr e q u i r e m e n to fs t a t es t o r a g ei n s a m p l i n gb ys t a t ec o m p r e s s i o ni nw h i c hap a r t i c l es t a t ei sd e c o m p o s e di n t oa ni n i t i a ls t a t ea n da c o d ei n d i c a t i n gt h ed e v i m i o no ft h et w os t a t e s t os p e e du p f i l t e r i n gp r o c e s s i n g ，t h ej u d g m e n to f e f f e c t i v ep a r t i c l en u m b e r sa sw e l l 勰i n t e r v a lr e s a m p l i n gi si n t r o d u c e dt od e c r e a s et h et i m e so f r e s a m p l i n g ；f u r t h e r , al o c a lw e i g h tm e a nc o m p a r i s o ns c h e m ei sp r o p o s e d ，i m p r o v i n gt h es p e e d b yt i m e s i nd i s t r i b u t e da r c h i t e c t u r e ，aw e i g h ts o r t i n gs c h e m ei s p r o p o s e d ，i n c l u d i n gc o m p l e t es o r t i n g a n dp a r t i a ls o r t i n g ，t oa v o i dt h er e d i s t r i b u t i o no f p a r t i c l e sb yb a l a n c i n gt h ew e i g h ts u m m a t i o n s o fe a c hp r o c e s s i n ge l e m e n t a l s op r e s e n t e di sah i e r a r c h i c a lr e s a m p l i n gm e t h o d w h i l ed i v i d i n g r e s a m p l i n gi n t oh i e r a r c h i e s ，e i t h e ri nc o a r s eg r a i no ri nf i n eg r a i n , i tg e n e r a t e si d e n t i c a lr e s u l t s 雒 c e n t r a l i z e dr e s a m p l i n gi ns t a t i s t i c s c o m p a r e d 诵t l lt r a d i t i o n a ld i s t r i b u t e da r c h i t e c t u r e s b e t t e r t r a d e - o f f sa m o n gs p e e d ，s t o r a g ea n da c c u r a c yc a nb eo b t a i n e di ns t r u c t u r e sb a s e do nt h ea b o v e t w om e t h o d s k e y w o r d s ：p a r t i c l ef i l t e r , s t a t ec o m p r e s s i o n ，l o c a lw e i g h tm e a nc o m p a r i s o n , w e i g h ts o r t i n g ， h i e r a r c h i c a lr e s a m p l i n g i i i 浙江大学硕士学位论文 致谢 致谢 籍此论文完成之际，谨向在硕士研究生阶段指导和培养我的老师、关心和帮助我的同 学、鼓励和支持我的朋友致以最诚挚的谢意! 首先，我要衷心地感谢我的导师严晓浪教授。严老师渊博的专业学识、广阔的产业视 野、严谨的治学态度以及忘我的工作精神，一直深深地感染着我。在我的研究生阶段，严 老师不仅为我指明了正确的研究方向，还为我创造了充分的科研环境和实践条件，使我在 求学的道路上能够不断地前进。 其次我要特别感谢我的合作导师潘赞副教授。潘老师以丰富的研究经验和高度的工作 热忱，从论文的选题、设计到最终的完成，都给予了我悉心的指导和宝贵的建议，是我学 习和科研中的良师益友，和他的每一次交流与讨论都充实而愉快 感谢丁勇老师在学 - - - j 和生活中对我的关心和指导丁老师思维敏捷而缜密，谈吐幽默 而风趣，让我在研究生的两年半时问里受益匪浅。 感谢程爱莲师姐，感谢万民永、王翔和王一木师兄，感谢曹晓阳、贾梦楠、叶森、孙 纲德、宋文华、张渊和刘晓东同学和你们在一起的点点滴滴，都将淀积成最美好的回忆 感谢浙江大学对我的教育和栽培“求是、创新”的校训和“大不自多、海纳江河” 的校歌，将永远指引着我前进的方向。 最后，感谢我的父母。是你们一如既往的理解、支持和鼓励，才铸就了我求学生涯中 不竭的动力。 浙江大学硕士学位论文图目录 图目录 图2 1滤波问题6 图2 2粒子滤波思想示意图7 图2 3 s r 算法示意图一1 1 图2 4 似然分布位于先验分布的尾部1 2 图2 5粒子滤波算法的递推过程1 4 图2 6滤波算法的探索空间16 图3 1基于s r 的重采样单元1 8 图3 2 基于s r 的采样和权重计算单元1 8 图3 3基于r s r 的重采样单元1 9 图3 4 基于r s r 的采样和权重计算单元1 9 图3 5 基于i m h 的权重计算与重采样单元。芝1 图3 6 基于i m h 的采样单元2 l 图3 7r c r 算法示意图。2 3 图3 8 存储优化的重采样单元2 3 图3 9 状态和权重存储器在重采样后的变化示意图2 4 图3 1 0 重采样存储优化后对应的采样和权重计算单元2 4 图3 1 l 状态和权重存储器在采样和权重计算后的变化示意图。2 5 图3 1 2一维空间中粒子状态的压缩示意图2 5 图3 1 3 存储优化的采样单元2 6 图3 1 4 不同划分等级下状态压缩后估计目标位置的r m s e 均值与s i r 算法的比较2 8 图3 1 5 使用有效粒子数后的采样和权重计算单元2 9 图3 1 6 不同模型下间隔重采样对r m s e 的影响3 0 图3 1 7 基于局部权重均值比较机制的粒子权重索引存储结构3 l 图3 1 8 基于局部权重均值比较机制的滤波结构3 2 图3 1 9 基于不同重采样算法的粒子滤波硬件时序图3 2 图3 2 0 基于局部权重均值比较机制的粒子滤波与s i r 算法的性能比较3 3 图4 1基于r p a 的分布式结构3 5 i v 浙江大学硕士学位论文图目录 图4 2r n a 的三种机制3 6 图4 3 基于r n a 的通用分布式结构3 6 图4 4 不同分布式粒子滤波算法在一个递推周期中的操作序列3 9 图4 5 基于权重排序机制的分布式粒子滤波结构3 9 图4 6 权重排序的硬件结构4 0 图4 7 权重排序单元的硬件结构4 1 图4 8 权重部分排序的示例图4 1 图4 9 基于权重排序的分布式粒子滤波的硬件时序图4 2 图4 1 0 权重全排序中p e 问权重和的误差比例4 3 图4 1 1权重部分排序中p e 间权重和的误差比例4 4 图4 1 2 基于权重排序的分布式粒子滤波算法同集中式算法的r m s e 均值比较4 4 图4 13p e 重采样示意图4 6 图4 1 4 基于粗粒度分层重采样的分布式粒子滤波硬件结构图4 7 图4 1 5 基于细粒度分层重采样的分布式粒子滤波硬件结构图4 9 图4 1 6内问重采样的流水线结构5 0 图4 1 7 一级内间重采样的流水线结构5 0 图4 1 8 粗、细粒度分层重采样的区别。5 0 图4 1 9 基于粗、细粒度分层重采样的分布式粒子滤波的硬件时序图一5 1 图4 2 0 基于粗、细粒度分层重采样的分布式粒子滤波与集中式算法的性能比较5 2 v 浙江大学硕士学位论文表目录 表目录 表1 1b o t 模型下的软硬件速度对比2 表3 1基于采样存储优化的结构与基于r s r 的结构间的存储开销比较2 7 表3 2 不同有效粒子数阈值下重采样的发生概率及对应的加速比3 0 表3 3 不同集中式粒子滤波结构在存储及速度方面的比较3 3 表4 1 基于权重排序的分布式粒子滤波结构与一般分布式结构在存储及速度方面的比较 z i ：! 表4 2 全排序和部分排序对硬件消耗的比较4 3 表4 3 基于粗、细粒度分层重采样的分布式粒子滤波硬件结构的存储及速度开销5 2 浙江大学硕士学位论文绪论 l 绪论 状态估计是根据观测( 实验) 数据估计一个系统的变量，这些变量代表了该系统在某 一时刻的内部条件或状态。状态估计广泛应用于各种工程和科学领域，任何与系统的数学 建模相关的学科都有可能涉及状态估计在实际应用中，良好的状态估计不仅可以用于实 现基于状态反馈的精确控制，还有助于掌握系统的运行状况以便做出正确的决策 1 1 课题背景及意义 在状态估计中，系统常常被建模为这样一组随时刻演化的方程：受过程噪声干扰的状 态转移方程和受量测噪声干扰的观测方程。状态的估计过程就是当新的时刻到来时，根据 此时的观测值和系统之前的状态来计算当前的状态 卡尔曼滤波( k f ，k a l m a nf i l t e r ) 是状态估计的经典递推算法，在系统的状态转移方 程和观测方程均为线性并且具有高斯白噪声时可以得到最优估计当线性条件扩展为非线 性时，可以将上述两方程进行一阶泰勒级数展开，从而得到近似的线性化模型，即为扩展 卡尔曼滤波( e k f ，e x t e n d e dk a l m a nf i l t e r ) 在有限状态的离散系统中，当先验概率和后 验概率均可计算时，基于网格的滤波( g f ，g r i d b a s e df i l t e r ) 将成为最优算法【l 】虽然针 对不同的状态估计问题已有多种相应的滤波算法，但传统的方法对系统模型进行了过多的 约束，从而限制了算法的应用范围，在诸多现实的复杂应用中得不到好的估计效果。而粒 子滤波算法的出现，在相当程度上解决了这个问题。 粒子滤波是一种基于蒙特卡洛模拟的递推贝叶斯滤波方法【2 1 ，适用于任何能用状态空 问模型表示的非线性、非高斯系统它以一组加权的粒子( 样本) 表示系统的后验概率密 度分布，并通过状态转移方程和观测方程递推地估计系统的状态转换，而且精度可以接近 最优估计广泛的适用范围、优异的估计性能以及适于并行的算法结构，使得粒子滤波迅 速受到国内外相关学者的普遍关注，并在通信【3 】- 【5 1 、跟踪【6 1 、信号处理1 7 、导航与定位【8 】【9 l 、 计算机视觉1 1 0 】等诸多领域中崭露头角 尽管粒子滤波已成为解决非线性、非高斯动态系统的参数估计和状态滤波问题的主流 方法，但由复杂的模型运算及众多的粒子数目所带来的庞大计算量限制了其在实时系统中 的应用( 实验中，在采用c k c p u 对3 2 0 2 4 0 像素的图像进行最基本的颜色直方图跟踪 时，当处理器频率为2 0 0 m h z ，粒子数目为5 0 且粒子大小为1 3 1 3 像素时，视频处理的 浙江大学硕士学位论文绪论 速度只有1 0 帧秒) 因此，采用专用的处理器( 如d s p u 】) 、通用的加速处理单元( 如 g p u 1 2 】f 1 4 】) 、多核处理器【1 5 】。【1 7 1 ，甚至专用的硬件设计( 如f p g a 1 8 】- 【2 0 】 4 1 1 4 2 1 ) 便成为目前 粒子滤波摆脱模拟与仿真，走向实用的有效途径。其中，专用的硬件设计因其在速度上的 独特优势，已逐渐成为研究的一个焦点表1 1 显示了在纯方位跟踪( b o t ，b e a r i n g s o n l y t r a c k i n g ) 模型下当采用文献 2 1 】中的实现结构时，软硬件的速度对比( 软件运行环境为 m a t l a b ，c p u 为p e n t i u md u a l c o r e ，频率为3 0 0 g h z ；硬件中时钟频率为i o o m h z ) 。 表1 1b o t 模型下的软硬件速度对比 p a r t i c l en u m b e r s1 0 2 42 0 4 83 0 7 24 0 9 6 m a t l a b7 5 9 9 h z3 0 6 5 h z1 8 0 8 h z1 2 0 1 h z f p g a3 1 1 8 k h z1 6 2 1k h z1 0 5 3k h z7 9 lk h z s p e e d - u p 4 2 0 1 95 4 1 6 05 9 6 2 46 7 4 4 4 1 2 国内外研究现状 自第一个实用的粒子滤波算法，即采样重要性重采样1 2 1 ( s i r ，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 ) 算法，于1 9 9 3 年被g o r d o n 等人提出，经过十数年的发展，粒子滤波已在算法 理论完善、滤波性能改进、应用领域拓展和运算处理加速等方面取得了长足的进步鉴于 论文的研究内容和方向，本节将主要回顾国内外在粒子滤波硬件设计方面的研究现状。 通用的粒子滤波主要包括采样、权重计算和重采样三个步骤由于采样和权重计算可 以流水和并行实现，因此相关文献主要针对重采样步骤进行研究和讨论。按照具体的实现 方式，现有的粒子滤波硬件结构大致可以分为集中式和分布式两种 在集中式结构中，采样和权重计算步骤以流水线方式完成后，得到更新的粒子状态和 权重，然后对所有粒子统一进行序贯重采样文献 2 2 】根据粒子权重的最后三位对粒子进 行标记( “粒子标签”) ，进而提出了一种残差重采样( r r ，r e s i d u a lr e s a r n p l i n g ) 算法的 快速定点实现方式文献【2 1 】基于系统重采样( s r ，s y s t e m a t i cr e s a m p l i n g ) 算法和残差 系统重采样( r s r ，r e s i d u a ls y s t e m a t i cr e s a m p l i n g ) 算法提出了两种通用的集中式硬件结 构，并比较分析了相应的处理速度和硬件开销；相比 2 2 1 ，这两种结构通过使用双端口存 储器将粒子状态存储的需求降低了一半文献【2 3 】提出了基于压缩重采样算法的结构，在 【2 1 1 均基础上进一步降低了重采样硬件的开销，但关键的重采样步骤仍然需要独立串行完 成，限制了速度的进一步提升此外，一些基于局部选择( l o c a ls e l e c t i o n ) 和马尔科夫 链蒙特卡洛( m c m c ，m a r k o vc h a i nm o n t ec a r l o ) 方法的重采样结构业已被相继提出， 2 浙江大学硕士学位论文绪论 如局部交错选择【2 4 】( l i s ，l o c a li n t e r l e a v i n gs e l e c t i o n ) 机制和i m h ( i n d e p e n d e n tm e t r o p o l i s h a s t i n g s ) 【2 5 】方法。 在分布式结构中，采样和权重计算步骤在多个处理单元( p e ，p r o c e s s i n ge l e m e n t ) 上 并行完成后，通过特定的重采样和分配策略在各个p e 内进行重采样并完成p e 间粒子的 重新分配。文献【2 6 提出了基于比例分配重采样( r p a ，r e s a m p l i n gw i t hp r o p o r t i o n a l a l l o c a t i o n ) 算法和非比例分配重采样( r n a ，r e s a m p l i n gw i t hn o n - p r o p o r t i o n a la l l o c a t i o n ) 算法两种分布式结构：前者首先计算各个p e 需要重采样的粒子数目，然后在各p e 内进 行重采样并在p e 问进行粒子分配；后者( 原文献中r n a 包括重组、自适应重组和局部 交换三种方式，本文只讨论最易于硬件实现的局部交换方式) 直接在各p e 内重采样相同 数目的粒子，然后在相邻的p e 问交换一定数目的粒子以防止粒子局部性的产生r p a 设 计较为复杂，尤其是用于p e 间粒子分配的控制逻辑，而且由于粒子权重分布的随机性， 粒子分配所需的时间也不确定；r n a 结构较为简单，但牺牲了估计的性能，甚至会导致 估计的发散。文献【2 7 提出的分布式结构保证了处理的速度和精度，但逻辑较为复杂，尤 其是存储的开销会随着p e 数目的增长而线性增长针对一般的分布式结构中p e 与中央 单元( c u ，c e n t r a lu n i t ) 之间通信量过大、通信时间较长的问题，相关研究也开始从特 殊的重采样算法入手，而l i s 和i m h 则为此提供了新的思路【2 8 】【2 9 1 1 3 本文研究内容及创新点 围绕粒子滤波硬件设计中的存储及速度两大关键问题，本文针对集中式和分布式两种 粒子滤波硬件实现方式展开了论述，在分析现有硬件结构的基础上，提出了相应的优化机 制，并进行了性能的分析，具体工作包括以下几个方面： ( 1 ) 在集中式结构中，采用状态回写和权重存储器重用技术减少了重采样步骤对存 储的需求，采用状态压缩方法减少了采样步骤对存储的需求在重采样阶段，通过将需要 复制的粒子回写到状态存储器的顶部，同时将其复制因子保存到权重存储器顶部的对应位 置，避免了传统结构中专用的复制因子存储器及索引存储器的使用，从而大大节省了重采 样存储的开销。针对粒子数目较大或状态维数较高等因素带来的状态存储器开销过大的问 题，本文提出了一种状态压缩方法，在粒子采样后利用状态间的差值将粒子状态在每个维 度上压缩为数目较少的离散值，在下一时刻采样阶段到来时再利用各个离散值对状态进行 恢复。对近似匀速转弯模型的实验表明，在不影响滤波精度的前提下，该方法可以显著降 浙江大学硕士学位论文 绪论 低粒子状态的存储需求。 ( 2 ) 在集中式结构中，采用有效粒子数判断、间隔重采样等基于传统重采样的方法 对滤波进行加速，同时提出了一种基于局部权重均值的比较机制对速度进行优化在基于 s i r 算法的传统集中式结构中，重采样发生在每个递推周期中。本文引入标准粒子滤波算 法中的有效粒子数判断以及间隔重采样两种方法减少重采样的次数，而对单变量不稳定增 长( u n o ，u n i - v a r i a t en o n s t a t i o n a r yg r o w t h ) 和b o t 的实验也证明了这两种方法的有效 性。在基于局部权重均值的比较机制中，当一个粒子的权重计算完毕后，将其同当前的权 重均值( 局部权重均值) 进行比较；若该权重较大，则将该权重置于权重存储器的顶部， 同时将该粒子的索引置于索引存储器顶部的对应位置；否则，将权重和索引置于相应存储 器的底部权重和索引存储器各有一对，以便在处理过程中进行乒乓操作在下一时刻， 从索引存储器顶部读取粒子索引并获得相应粒子，同时从权重存储器中读取相应权重，该 粒子的复制次数可直接由权重与所有粒子平均权重倒数的乘积得到。采用该机制的集中式 结构，将本时刻的重采样步骤同下一时刻的采样和权重计算步骤流水实现，处理速度相比 基于r s r 的结构提高了约1 倍，相比基于s r 的结构提高了约两倍 ( 3 ) 在分布式结构中，提出了权重排序机制，包括全排序和部分排序，对存储开销 和处理速度进行优化。在全排序机制中，当多个p e 同时产生新的粒子及粒子权重时，对 这些粒子权重进行全排序，同时对当前每个p e 中的粒子权重和也进行全排序；粒子和粒 子权重按照p e 中权重和的倒序分别被送入相应的p e 中当所有粒子处理完毕后，由于 粒子数目较多，每个p e 中的权重和趋近于相等，因此可在各个p e 内部进行独立的重采 样，而且重采样后的粒子数目均相等在部分排序机制中，p e 和粒子权重分为若干组， 全排序则在组问和组内分别进行。所提的权重排序机制用权重排序代替了传统分布式结构 中专用的粒子重分配过程，减小了每个滤波周期所需的时间，同时较好地平衡了各个p e 中的粒子权重和，保证了重采样后的滤波精度 ( 4 ) 在分布式结构中，同时提出了分层重采样机制，包括粗粒度和细粒度两种方式， 对存储开销和处理速度进行优化在粗粒度分层重采样方式中，当各p e 完成采样和权重 计算步骤后，重采样在p e 和粒子两个层面同时进行在p e 层，以p e 作为粗粒度粒子， 以p e 数目为重采样数目，得到各p e 的复制次数以及对应的源p e ( 下一个周期中需要由 哪个p e 复制而来) ；在粒子层，重采样发生在各p e 内部，且重采样后粒子数目保持不变。 采用该分层方式可以在统计上得到和集中式重采样相同的精度，且p e 间粒子的重分配过 4 淅江大学硕士学位论文绪论 程可仅由多路选择器来完成。在细粒度分层重采样方式中，当各个p e 同时产生新的粒子 及粒子权重时，随即对粒子进行重采样，并将重采样后的粒子保存至各个p e 相应的粒子 状态存储器中，同时将粒子的权重和作为新的权重保存至全局权重存储器中；当采样和权 重计算步骤完成之后，根据全局权重存储器中的权重进行重采样，进而得到各p e 内部的 重采样粒子在该分层方式中，由于各个p e 状态存储器中对应位置的粒子具有相同的权 重，因而后一个重采样过程可以被所有p e 所共用，同时也将保存粒子权重所需的存储资 源减为集中式实现的1 k 左右，其中k 为p e 的数目 1 4 本文的内容及组织 本文共包含五个章节。除本章外，后续章节的内容组织如下： 第二章概述了粒子滤波算法的基本理论，讨论了其中的几个关键问题，并简介了几种 常见的粒子滤波算法。 第三章介绍了集中式粒子算法的硬件设计，重点讨论了在处理速度和存储开销两个方 面的优化方案。 第四章针对分布式的粒子滤波，详细阐述了基于权重排序和基于分层重采样的硬件设 计，并分别进行了性能的分析与比较 末章对全文进行了总结，并对下一步的工作方向进行了展望。 浙江大学硕士学位论文粒子滤波的基本理论 2 粒子滤波的基本理论 基于状态空间建模的系统通常可以由系统的状态转移模型和观察模型来描述，即 毪= f ( x k 一1 ) + 吒一l ( 2 1 ) 乙= 办( 以) + n k ( 2 2 ) 其中，x k 、名分别为系统在k 时刻的状态量和观测量，八) 、办( ) 分别为系统的状态转移 方程和观测方程，u 1 仇分别为两个模型的方差。 滤波问题即是根据一系列的观测量顺序地估计系统的状态( 参数或隐藏变量) ，也可以 理解为估计系统的后验概率密度p ( x kl 而，z 2 ，乙) ，如图2 1 所示常用的滤波器包括卡 尔曼滤波、扩展卡尔曼滤波、粒子滤波等等。 甲早甲 u n o b m s e r o v e d l1一l 础旷 iii 一 一国一一 图2 1 滤波问题 滤波器的性能不仅取决于算法本身，也依赖于应用的状态空间模型。在一般情况下， 状态估计的解析解无法得到，而且随着系统非线性的增强和噪声模型的复杂化，传统的滤 波算法精度将大打折扣这是因为传统的算法专注于求解估计状态的解析式，虽然可以在 一些特定的约束条件下得到最优解或近似解，但对大部分现实应用却并不奏效而近些年 出现的粒子滤波，突破了传统的理论框架，对系统的过程噪声和量测噪声没有任何限制， 可适用于任何非线性系统，精度可以逼近最优估计，是解决实际非线性问题强有力的工具 粒子滤波是一种基于蒙特卡洛模拟和贝叶斯估计的递推统计滤波方法。它的递推实质 是：首先利用上个时刻系统的多个可能状态分别对本时刻系统的状态进行模拟，再根据观 测模型和观测值对各次模拟的结果进行评估，最后由各个评估的结果加权得到本时刻系统 的估计状态从概率密度分布的角度理解，粒子滤波的思想可以描述为如图2 2 所示的分 布转换图【3 0 1 ：上一时刻的后验分布经过特定漂移( 由系统模型而定) 得到本时刻的转移分 布，在噪声的干扰下演化为预测分布，最后由观测值进行整形从而得到本时刻的后验分布 6 浙江大学硕士学位论文粒子滤波的基本理论 d e t e r m i n i s t i cd i j n d i s t u r b a n c eo f r e g u l a t i o no fm e a s u r e m e n t 图2 2 粒子滤波思想示意图 本章将从介绍序贯重要性采样( s i s ，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 ) 开始，在此基 础上讨论建议分布的选取和重采样两个关键技术，最后将简介几种常见的粒子滤波算法。 2 1 序贯重要性采样 s i s 是基本蒙特卡洛方法之一，也是粒子滤波的基础其核心思想是利用一组加权的 粒子来表示系统的后验概率密度分布和估计所需的状态，即，假设 。，以) 笠。代表系统后 验概率密度分布p ( x o ：tiz l ： ) 的一组随机采样集，其中 矗。，i = o ，1 ，m 为支撑点集， 嵋，f = 0 ，1 为对应的权重集合，黾。= 缸，= o ，l ，七) 为直至七时刻的所有状态集合， 那么系统在七时刻的后验概率密度分布可以近似表示为 p ( x 0 ：。h ) ：，以艿( ：。一：。) ( 2 3 ) 其中万( ) 表示狄拉克函数。 式( 2 3 ) 表示了后验概率密度分布的离散加权近似，其中权重的计算依赖于如下所 浙江大学硕士学位论文 粒子滤波的基本理论 述重要性采样原理【l j 。 假设p ( 曲刀( x ) 是一个不易直接采样的概率密度，而万( x ) 却可以被估计( p ( x ) 亦可 按比例估计) 。另假设g ( x ) ，i = 1 ，2 ，是从易于采样的分布g ( x ) 中得到的样本( g ( x ) 称 为重要性密度或建议分布) ，那么分布p ( x ) 的一个加权近似可以表示为 p ( x ) ：。w 。罗( x - - x ) ( 2 4 ) 其中 “鬻 ( 2 5 ) 为第i 个粒子的归一化权重因此，若：。是从重要性密度q ( x o ：。lz i ：。) 中采样的样本，则式 ( 2 5 ) 中的权重可以改写为 叫襞螺 6 ， 在序贯情况下，每一次递推即为用现有分布p ( h 。4i 钆q ) 中的近似样本点来估计一组 新的样本点以近似表示分布p ( h 。i 钆) 若重要性密度满足如下马尔科夫过程 q ( x o 尘i 毛尘) = q ( x oi ：i l ，乙：i ) 9 ( 】i li 刁：i 1 ) ( 2 7 ) 则可以通过已有样本量- 。q ( x 一。lz b ) 与新状态9 ( 埠i 石：- 。，弓量：相乘来得到样本 如g ( iz l ：七= ) 同时，由于后验概率密度p ( h 。iz l ：。) 可以转换为 p ( h 。z l ：k ) ：丝止雩导逃掣 p 乙lz l ：i l ， = 血睑裂急掣瓶h h 扣。) ( 2 28 一) p 【z iiz l ：i l j 。 lj = 血岩甓 p 卜l i z - 1 ) p 【z tz l ：t l ，) 芘p ( z tix i ) p ( x iix k - i ) p ( x o ：i 1iz l ：i 1 ) 因此，式( 2 6 ) 中的权重可以改写为 以a c 铡糕x o 焉) 黜q ( x oz l ：k - i ) 咄- 掣糟：k - i 9 ( j ：i ： 一l ，z l ：t：女一li 。一1 g ( x ：i x ：z l ：) 。 式( 2 9 ) 中，若g ( l n ，毛：。) = g ( i - ，气) ，则重要性密度将仅依赖于状态一。和 观测值乙，这对于通常情况下在每一步递推中只需要滤波结果p ( 以iz l ：k ) 时是十分有利的， 浙江大学硕士学位论文 粒子滤波的基本理论 因为可以忽略状态的路径以及观测z i ：。q 的集合在这种情况下，权重可以调整为 以眦铡辫z ( 2 口i 工ix 1 ，j 同时后验概率密度p ( l 毛：。) 可以近似为 p ( x kiz ) ：。以万( 一) ( 2 1 1 ) 当粒子数一时，式( 2 1 1 ) 可以接近真实后验概率密度p ( x ki 五：。) 。 在新的观测值到来时将粒子及权重按上述过程进行递推，我们便可以得到如下所示的 s i s 算法。 在由重要性采样构成的s i s 递推过程中，一个不可避免的i 司题是粒子权重的退化i 司题， 即在数步递推后，少数粒子的权重较大，而大部分粒子的权重可以忽略。相关文献已经证 明，粒子权重的方差随着时间的延续将不断增加1 3 1 1 ，因此权重退化问题是s i s 所固有的。 这将导致大量的计算浪费在对估计p ( 鼍i 五：。) 无足轻重的粒子上一种适合度量粒子权重 退化程度的方法是引入有效粒子数的概念，它定义如下 = 丽n ( 2 1 2 ) 其中 小耥 ( 2 是真实的粒子权重无法直接计算，但可以用下式估计 = 丽韶 他j 4 二- 忙i ”， 其中以为式( 2 1 0 ) 中归一化后的权重越小，表示权重退化越严重。要减小粒子权 重的退化，理论上可以采取以下三种方法：( i ) 增大粒子数( 2 ) 选取好的建议分布； ( 3 ) 使用重采样算法 浙江大学硕士学位论文粒子滤波的基本理论 相对于后两者，增大粒子数将直接增加计算量和计算所占用的资源，在实际应用中 通常不可取，下文将对后两种方法予以介绍。 2 2 建议分布的选取 评价建议分布优劣的标准是能否使哳( 以。) 最小化( 最大化) 。最优的建议分布依 赖于状态以一。和观测值气，可以表示如下 g ( x ki 小气) = p ( 吒i 巾乙) = 旦竺d 弓著铲 ( 2 1 5 ) 将式( 2 1 5 ) 代入式( 2 1 0 ) 可得 以芘以一。p ( 气l 一。) = 以一。f p o 是核带宽，以是状态x 的维数，叫是归一化的权重。核密 度k ( ) 是一个对称的概率密度函数且满足 肛( x ) 出= o ，j 1 1 x i l 2k ( x ) 出 ( 2 2 0 ) 核k ( ) 和带宽h 的选择标准是使真实后验分布与式( 2 1 8 ) 所示的正则经验分布之间 的积分均方误差均值( m i s e ，m e a ni n t e g r a t e ds q u a r ee r r o r ) 最小m i s e 定义如下 加s e ( p ) = e p ( 以lz t ：k ) 一p ( ti 小气) 】2 呶】 ( 2 2 1 ) 其中；( 小) 为式( 2 1 8 ) 所示的近似后验分布。在所有粒子具有相同权重的特殊情况下， 最优的核为e p a n e c l m i k o v 核 ：蓐”忙1 1 2 ) 州l 1 ( 2 2 2 ) 【0 ， o t h e r w i s e 其中巳。是吼 中单位超空问的体积此外，若潜在的密度函数为具有单位协方差矩阵的高 浙江大学硕士学位论文 粒子滤波的基本理论 斯分布，则最优的带宽为 h o p ! = a n ；w b + 4 ( 2 2 3 ) 其中 彳= 【( 吃+ 4 ) ( 2 万) 】v + 4 ( 2 2 4 ) 若样本粒子的经验方差矩阵为( 非单位矩阵) ，则需要通过线性变换进行白化设 s k = o k 珥，则有 4 占。= x ( 2 2 5 ) 其中x 为原始样本，s 为与之对应的白化样本这样对于具有均值i ，方差，最优带宽 的核函数k h 一i ) ，其样本为 x = i + 4 e 。 ( 2 2 6 ) 需要注意的是，式( 2 2 2 ) 至武( 2 2 6 ) 仅在粒子权重相等且重要性函数是高斯型时 是成立的，但仍可以扩展到一般情况以获得次优的滤波器。 另一种常见的解决方法是使用m c m c 方法m c m c 是一种利用马尔科夫链无记忆性 的随机模拟方法，其基本思想是根据所要采样的概率分布函数p ) ，构造出一个非周期、 不可约的马尔科夫过程，使该过程的平稳分布为贴) 粒子滤波中最常用的m c m c 方法 为m h ( m e t r o p o l i sh a s t i n g s ) 算法，该算法的过程可描述如下 m c m c 方法虽然有利于增加粒子的多样性，但为了保证收敛所需要的概率转移次数 较大，而且其收敛的判断也是个问题 2 4 粒子滤波算法 在上述几节的基础上，可以得到如图2 5 所示的粒子滤波算法的递推过程。一个完整 的递推过程包括预测( 采样) 、权重计算和重采样三个步骤：( 1 ) 采样一一在k 时刻，n 1 3 浙江大学颂士学位论文粒子滤波的基本理论 个k - 1 时刻重采样后的粒子经由建议分布产生个新的粒子；( 2 ) 权重计算一一根据k 时 刻的观测值对每一个新的粒子赋予相应的权重；( 3 ) 重采样一一复制权重较大的粒子，剔 除权重较小的粒子，最终得到权重相同的个粒子。虽然重采样并不是每个递推过程中的 必须步骤，但为方便起见我们仍在图2 5 中将其列出 i l 0 l 1 彳人 - ；t - p t c d i c t i - j tweight c a l c u l a t i o n 图2 5 粒子滤波算法的递推过程 基于上述递推过程，可以将基本的粒子滤波算法表述如下 其中，所为预先设置的有效粒子数阈值 将基本算法中的建议分布g ( 以i 小气) 改为先验分布p ( 吒i 一。) ，并在每次递推时均进 行重采样操作，即为最常见的采样重要性重采样( s i r ，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 ) 粒子滤波算法若用u k f 来产生建议分布，则对应算法为无迹粒子滤波【3 4 】( u p f ，u n s c e n t e d 1 4 伽砒 i c g 龇 吨 蚍 唧 c s m 胁 二：、tll 浙江大学硕士学位论文粒子滤波的基本理论 p a r t i c l ef i l t e r ) 。而当重采样选用正则重采样方法时，算法对应为正则粒子滤波( r p f ， r e g u l a r i z e dp a r t i c l ef i l t e r ) 重采样不仅可以采用当前时刻