第３３卷第１期 ２ ０ １ ４年３月 计 算技术与 自 动 化 Ｖｏ１．３３，Ｎｏ．１ Ｍａｒ．２ ０ １ ４ Ｃｏｍｐｕｔｉｎｇ Ｔｅｃｈｎｏｌｏｇｙ ａｎｄ Ａｕｔｏｍａｔｉｏｎ 文章编号：１００３—６１９９（２０１４）０１—００６９—０４ 基于正则化的高斯粒子滤波算法 刘梦菱，秦摘岭 ４３００２２） （武汉轻工大学，电气与电子工程学院，湖北武汉要：针对非线性系统的状态估计问题，提出一种改进的高斯粒子滤波算法。该算法是基于正则化 粒子滤波（ＲＰＦ），将重采样中离散的概率分布函数近似为连续分布，进而在高斯粒子滤波（ＧＰＦ）中引入正则 化粒子滤波算法得到的最新预测值，并利用这一观测值进行状态估计的更新。最后，对ＲＧＰＦ和ＧＰＦ两种 算法进行综合分析和实验仿真，结果表明，与标准ＧＰＦ算法相比，ＲＧＰＦ具有较高的滤波精度。 关键词：高斯粒子滤波；正则化粒子滤波；概率分布；粒子退化 中图分类号：ＴＰ１４ 文献标识码：Ａ Ｇａｕｓｓｉａｎ Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒ Ａｌｇｏｒｉｔｈｍ Ｂａｓｅｄ ｏｎ ＲｅｇｕｌａｒｉｚａｔｉＯｎ ＬＩＵ Ｍｅｎｇ—ｌｉｎｇ，ＱＩＮ Ｌｉｎｇ （Ｗｕｈａｎ Ｐｏｌｙｔｅｃｈｎｉｃ Ｕｎｉｖｅｒｓｉｔｙ，Ｅｌｅｃｔｒｉｃ ａｎｄ Ｅｌｅｃｔｒｏｎｉｃ Ｅｎｇｉｎｅｅｒｉｎｇ Ｉｎｆｏｒｍａｔｉｏｎ Ｄｅｐａｒｔｍｅｎｔ，Ｗｕｈａｎ ４３００２２，Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｉｎ ｔｈｉｓ ｐａｐｅｒ，ａ ｎｅｗ ｉｍｐｒｏｖｅｄ Ｇａｕｓｓｉａｎ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ ａｌｇｏｒｉｔｈｍ ｉｓ ｐｒｏｐｏｓｅｄ ｆｏｒ ｔｈｅ ｓｔａｔｅ ｅｓｔｉｍａｔｉｏｎ ｐｒｏｂｌｅｍ ｏｆ ｎｏｎｌｉｎｅａｒ ｓｙｓｔｅｍｓ．Ｔｈｅ ｎｅｗ ｐａｒｔｉｃｌｅ ａｌｇｏｒｉｔｈｍ ｉｓ ｂａｓｅｄ ｏｎ Ｒｅｇｕｌａｒ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ，ｏｆ ｗｈｉｃｈ ｔｈｅ ｄｉｓｃｒｅｔｅ ｐｒｏｂａｂｉｌｉｔｙ ｄｉｓｔｒｉ— ｂｕｔｉｏｎ ｆｕｎｃｔｉｏｎ ａｐｐｒｏｘｉｍａｔｅｓ ｔｈｅ ｃｏｎｔｉｎｕｏｕｓ ｆｕｎｃｔｉｏｎ ｉｎ ｒｅｓａｍｐｌｅ．Ｎａｍｅｌｙ，ｔｈｅ ｌａｓｔ ｍｅａｓｕｒｅｍｅｎｔｓ ｏｆ ＲＰＦ ａｒｅ ｉｎｔｒｏｄｕｃｅｄ ｔｏ ｔｈｅ ＧＰＦ ａｎｄ ｔｈｅｎ ｔｈｅ ｐｒｅｄｉｃｔｅｄ ｖａｌｕｅｓ ａｒｅ ｕｓｅｄ ｔＯ ｕｐｄａｔｅ ｔｈｅ ｓｔａｔｅ ｅｓｔｉｍａｔｉｏｎ．Ａｎａｌｙｓｉｓ ｂｙ ｓｙｎｔｈｅｓｉｓ ａｎｄ ａ ｓｉｍｕｌａｔｉｏｎ ｅｘｐｅｒｉ— ｍｅｎｔ ｉｎｄｅｐｅｎｄｅｎｔｌｙ ｂｅｔｗｅｅｎ ＲＧＰＦ ａｎｄ ＧＰＦ ａｒｅ ｐｒｅｃｅｄｅｄ．Ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈａｔ ＲＧＰＦ ａｌｇｏｒｉｔｈｍ ｈａｓ ｍｏｒｅ ａｃｃｕｒａｃｙ ｃｏｍｐａｒｉｎｇ ｗｉｔｈ ｓｔａｎｄａｒｄ ＧＰＦ ａｌｇｏｒｉｔｈｍ． Ｋｅｙ ｗｏｒｄｓ：ｇａｕｓｓｉａｎ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ；ｒｅｇｕｌａｒ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒ；ｐｒｏｂａｂｉｌｉｔｙ ｄｉｓｔｒｉｂｕｔｉｏｎ ｆｕｎｃｔｉｏｎ；ｐａｒｔｉｃｌｅ ｄｅｇｒａｄａｔｉｏｎ 和小权值粒子的删除，可以部分改善粒子的退化问 １ 引 言 在处理非线性系统的状态估计问题时，基于贝 叶斯估计［ 的蒙特卡洛方法 应用较为广泛，如粒 子滤波算法ｌ＿３］（Ｐａｒｔｉｃｌｅｓ Ｆｉｌｔｅｒ），粒子滤波的关键 在于如何合理地选择重要性密度函数［４］。在工程 应用当中，重要性密度函数通常选用系统的状态转 题，但相应会带来计算膨胀问题，使之在实际应用 中的实时性大大降低。而另一种方法是对粒子滤 波算法重采样进行改进，如高斯粒子滤波算法＿７ ］ （Ｇａｕｓｓｉａｎ Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒ，ＧＰＦ）。其基本思想是将 估计状态的后验概率密度近似为高斯分布，并利用 粒子滤波方法求取其相关参数。此方法不仅解决 了粒子的退化问题，而且不需要重采样，降低了算 法的计算量。并且易于并行处理，因此，ＧＰＦ算法 具有一定的应用性 。 移概率ｌ＿５］。然而，经过多次迭代后，粒子滤波算法 中的粒子会出现明显的退化现象 ］，即大多数粒子 的权值会趋于零，对信息更新没有任何贡献，但是 会消耗一定的计算成本。为了解决这个问题，一种 方法是采用重采样技术，通过对大权值粒子的复制 收稿日期：２０１３—０６—１９ 然而，ＧＰＦ的问题在于粒子采样是在离散近 似分布中进行的，虽然算法的实时性较好，但是精 度较低。为此，一些学者对标准ＧＰＦ算法进行了 作者简介：刘梦菱（１９９１一），女，湖北襄阳人，学士，研究方向：目标跟踪（Ｅ—ｍａｉｌ：ｐｏｐｕｌａｒｑｉｎ＠１６３．ｃｏｍ）；秦汉人，副教授，硕士，研究方向：目标跟踪。 岭（１９７６一），男，湖北武 计算技术与自动化 适当的改进。文献［１１］通过对量测误差的分布参 数进行实时统计和更新，并以此得到粒子的权值， 进而提高了估计精度对误差变换的不敏感。文献 ［１２］在ＧＰＦ的状态更新过程中采用卡尔曼滤波对 分布参数及均值和方差进行更新，减少了计算量， 提高了滤波精度。 本文在上述研究基础上提出了一种新的改进 ＧＰＦ算法，即正则化高斯粒子滤波。正则化滤波 算法是将后验概率密度的离散分布重建为连续分 布，然后通过对粒子的核密度采样实现连续分布采 样。因此可以在ＧＰＦ的预测更新后，利用正则化 滤波算法得到后验概率密度的近似分布参数，并以 此采样粒子，进而对状态变量进行估计。最后，仿 真结果表明，本文提出的算法滤波精度与标准 ＧＰＦ算法相比有一定程度的提高。 ２高斯粒子滤波（ＧＰＦ） 高斯粒子滤波算法是通过粒子滤波方法得到 一个高斯密度函数来近似估计未知状态的后验概 率分布。在非线性系统的条件下，高斯粒子滤波算 法获得的是次优贝叶斯分布。 设高斯随机变量Ｘ的密度为： Ｎｉｘ；／ｚ，∑）一（２丌）一ｍ／２ ｌ∑ｌ一１／２ｅｘｐ（一 （ ）丁∑一 （ｚ一 ）／２） 其中， 和∑分别为ｎ维状态向量ｚ的均值和 协方差矩阵。设在ｋ时刻，有 Ｐ（ｚ ｙｋ）一Ｎ（ｘ ； ，∑ ） 在得到当前测量值后，由以下两步分别得到滤 波和预测的概率分布： Ｓｔｅｐ１ 测量更新 （１）对重要性密度函数ｑ（ｘ Ｙ ）进行抽样 得到样本集｛ｚｌ ｝ Ｎ 。 （２）计算单个粒子的权重并归一化 ｑ ｉｏ２７ 苇ＹＩ ０ ： 砌 一 ，／ （３）由式（１）和（２）计算ｋ时刻估计状态的均 值和方差 一∑训 （１） ∑ ＝∑ （ 一ｚ ）（ 一ｚ ） （２） Ｓｔｅｐ２ 预测更新 （１）假设量测更新后状态估计量的后验概率 密度近似为高斯分布Ｎ（ｘ ；ｍ，∑ ），对其采样后得 新的粒子集｛ ｝ Ｎ； （２）由转移概率分布ｐ（ｘ Ｉ ｚ 一ｚ ）抽样 得到ｋ＋１时刻的粒子集｛ｚ ）￣＿ 。 （３）计算预测状态的均值与协方差 Ｎ １ 抖 一 ｚ （３） １ Ｎ 抖 一 （互抖 一ｚ ）（五 ＋ 一ｚ ） （４） 此时ＧＰＦ的预测概率分布可近似为高斯分布 Ｐ（ｊＥ ＋１ Ｉ Ｙｏ； ）≈Ｎ（ｘ ＋１；／ｚ ＋１，∑ ＋１） ３ 正则化粒子滤波（ＲＰＦ） 重采样用于解决粒子滤波算法中粒子衰竭的 问题，然而重采样过程是在离散分布上进行的，会 导致产生粒子退化现象，为了解决这一问题， Ｃｈｒｉｓｔｉａｎ Ｍｕｓｓｏ等人提出了一种新的滤波算 法 ¨ ：正则化粒子滤波（Ｒｅｇｕｌａｒｉｚｅｄ Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒ）。它是将离散分布的问题转化为连续分布的 问题，即根据密度估计理论计算出后验分布的连续 密度，并从中采样得到粒子集，从而使粒子的退化 问题得到改善。 核密度近似函数可近似为 Ⅳ ｐ（ｘ ｊ ｚ Ｚｋ）≈∑叫 Ｋ ｉｘ 一ｚ （５） 其中Ｋ （ｚ）一（１／ｈ ）忌（ｘ／ｈ）为核密度函数， ＞０为核带宽，核密度函数满足以下条件。 Ｉ ｘＫ（ｘ）ｄｘ＝０，Ｉ ｚ Ｋ Ｉｌ－ｚ｛ ｌｄｘ＜。。 当粒子权重都相等，且为 时，最优核密度函 数为 Ｋ 一！ （１一ｌ ｌｚ ｌｌ ），Ｉｉ ｚ Ｉｌ＜１（６） 厶Ｃ” Ｋ 一０，ｌｌ ｚ ｌｌ＞０ （７） 其中，Ｃ为在Ｒ 上单位超球体的体积。当未知 状态的先验密度是具有单位协方差矩阵的高斯密 度时，存在最佳带宽，即 ｈ 一Ａ（Ｋ）Ｎ南 其中，Ａ（Ｋ）一Ｅ８ｃ （ ＋４）（２√ ）” 丰 。 ４ 正则化高斯粒子滤波（ＲＧＰＦ） 在ＧＰＦ的实际工程应用中，一般将系统的状 第３３卷第１期 刘梦菱等：基于正则化的高斯粒子滤波算法 态转移概率密度作为重要性密度函数。由于重要性 函数不具备观测的最新数据，因此直接从重要性函 数中采样得到的样本与其真实的状态存在着较大 的误差，使得跟踪的效果不理想，尤其是在其似然 函数位于系统状态转移概率密度函数的尾部或者 似然函数位于尖峰型时，跟踪效果更差。 本文提出利用正则化粒子滤波来优化高斯粒 子滤波，使之能够有效地克服上述问题。优化的根 本思想是在高斯粒子滤波的预测更新阶段中，引入 正则化粒子滤波算法，将滤波结果作为高斯滤波的 测量更新密度函数的分布参数，由于在正则化粒子 滤波中引入了最新的观测值，因此，状态预测值更 加逼近真实值，从而提高了高斯滤波的精度。 ＲＧＰＦ的具体算法步骤如下： （１）正则化更新 Ｓｔｅｐ１从重要性函数ｑ（ｘ（ｋ。ｌ Ｙ ）中抽取Ｎ个 粒子ｚ ，ｉ一１，…，Ｎ，其中重要性密度函数为 ｑ（ｘ ｌ Ｙ女）一Ｎ（ｚ ； ，∑ ）； Ｓｔｅｐ２计算每个粒子的权重 ，一 ～ ｌ：：！ ＝三 三 ：：！ ：至 ｑ（ｘｔ”／ｙ ） Ｎ 并归一化得到叫： 一 ／∑ ：“； Ｓｔｅｐ３设定阈值Ｎｔｈ，并计算样本容量 １ Ｎｅ｛｛一 －＿一， ＞：（叫 ） ｉ＝１ 若Ｎｅｆｆ＜Ｎｔｈ，计算｛ ，叫 ｝的协方差矩 Ｎ 阵ＳＫ一∑叫 （ｚ 一 ）（ 一 ） ，进而得最 ｚ＝１ 小均方误差Ｄ Ｄ —Ｓ ； Ｓｔｅｐ４从最优核密度函数中采样￡ ～Ｋ。 并 计算相应的状态值 ｚ 一ｚ ＋ｈｏｐｔＤ ｅ Ｓｔｅｐ５根据ｋ时刻最新的观测值，并由式（１）和 （２）求出ｋ时刻的状态估计值 与协方差∑ ，并得 到后验概率近似分布为： Ｐ（３２ｋ Ｊ Ｙ ）≈Ｎ（ｘｔ； ，∑ ）。 （２）高斯粒子滤波预测 Ｓｔｅｐ６从后验概率分布中抽取新的粒子集 ｛ｚ ） ； Ｓｔｅｐ７由状态转移方程户（ｚ抖。Ｉ．７Ｃ 一ｚ ）采 样得到ｋ＋１时刻的粒子集｛ ｝ ，； Ｓｔｅｐ８由式（３）和（４）计算预测状态的均值与 协方差，进而得到预测概率分布。 ５ 仿真与分析 选取如下的一维系统模型： ３２ 一０．５ｘｋ－－１＋２．５－ｚ ～１／（１＋ｚ 一１）＋ ８ｃｏｓ（１．２（ｋ一１））＋Ｗｋ Ｙ ＝＝＝ｚ；／ｚｏ＋ 其中，Ｖｋ～Ｎ（Ｏ， ）和训 ～Ｎ（Ｏ，Ｒ ）分别 为服从高斯分布的过程噪声和测量噪声。 仿真参数设定为：Ｑ ：Ｒ 一１，初始状态ｚ一 ０．１，初始状态方差Ｐ一２，模拟长度ｔ ｒ一６Ｏ，粒子 数Ｎ一１００。 定义一次实验的均方误差为： ｆｆ ， ＭＳＥ一（ ∑（， ＝】 主 一ｚ ） ）１／２ 在一次实验中，ＧＰＦ和ＲＧＰＦ两种滤波 算法对系统状态估计情况如图（１）所示。图（２）为 ＧＰＦ和ＲＧＰＦ分别运行５０次的均方误差曲 线图。 ｔｉｒｔ￣ｓｔｅｐ 图１ 两种算法比较结果 图２ ５０次蒙特卡罗ＲＭＳＥ 计算技术与自动化 从图（１）可以看出，经过一次仿真实验， ＲＧＰＦ对系统真实状态的跟踪效果明显要好于 ＧＰＦ，这是因为正则化高斯粒子滤波器在粒子量测 更新之前加入了正则化步骤，使得更新之前的粒子 得到优化，故对非线性系统的状态有相对更好的估 计效果。 由图（２）可以看出，经过５０次实验仿真 后，正则化高斯粒子滤波的ＲＭＳＥ曲线小于高斯 粒子滤波的ＲＭＳＥ。说明正则化高斯粒子滤波器 在处理非线性系统状态估计的问题时，精度高于高 斯粒子滤波器，因此，ＲＧＰＦ更适用于某些对系统 状态估计精度要求高的工程当中。 ６ 结束语 针对非线性系统的状态估计问题，高斯粒子滤 波能取得较好的效果。在对系统状态估计精度要 求较高的工程中，本文提出了基于正则化的高斯粒 子滤波算法，该算法采用正则化粒子滤波结果对量 测更新中后验概率密度函数的分布参数进行更新， 由于考虑到了新量测值所带来的新息，因此，能够 在增强粒子多样性的同时使粒子的分布更加逼近 于真实状态分布，从而提高了对非线性状态的估 计精度。在下一步工作当中，可以考虑将正则化高 斯粒子滤波器应用于例如水下目标跟踪等工程实 际问题中。 参考文献 ［１］ Ａｖｉｔｚｏｕｒ．Ａ ｓｔｏｃｈａｓｔｉｃ ｓｉｍｕｌａｔｉｏｎ Ｂａｙｅｓｉａｎ ａｐｐｒｏａｃｈ ｔｏ ｍｕｌｔｉ—ｔａｒｇｅｔ ｔｒａｃｋｉｎｇ［Ａ］．ＩＥＥ Ｐｒｏｃｅｅｄｉｎｇ ｏｎ Ｒａｄａｒ，Ｓｏｎａｒ ａｎｄ Ｎａｖｉｇａｔｉｏｎ ＵＫ：ＩＥＥＥ，１９９５． ［２］ Ｇ．Ｋｉｔａｇａｗａ．Ｍｏｎｔｅ Ｃａｒｌｏ ｆｉｌｔｅｒ ａｎｄ ｓｍｏｏｔｈｅｒ ｆｏｒ ｎｏｎ Ｇａｕｓｓｉａｎ ｎｏｎｌｉｎｅａｒ ｓｔａｔｅ ｓｐａｃｅ ｍｏｄｅｌｓ［Ｊ］．Ｊｏｕｒｎａｌ ｏｆ Ｃｏｒｎ— ｐｕｔａｔｉｏｎａｌ ａｎｄ Ｇｒａｐｈｉｃａｌ Ｓｔａｔｉｓｔｉｃｓ，１９９６，５：１—２５． ［３］Ａｎｄｒｉｅｕ Ｃ，Ｄｏｕｃｅｔ Ａ，Ｓｉｎｇｈ Ｓ Ｓ，ｅｔ ａ１．Ｐａｒｔｉｃｌｅ ｍｅｔｈｏｄｓ ｆｏｒ ｃｈａｎｇｅ ｄｅｔｅｃｔｉｏｎ，ｓｙｓｔｅｍ ｉｄｅｎｔｉｆｉｃａｔｉｏｎ，ａｎｄ ｃｏｎｔｒｏｌ［Ｊ］． Ｐｒｏｃ．ＩＥＥＥ，２００４，９２（３）：４２３—４３８． ［４］夏克寒，许化龙，张朴睿．粒子滤波的关键技术及应用［Ｊ］． 电光与控制，２００５，１２（６）：１—４． ［５］Ｎ Ｊ Ｇｏｒｄｏｎ，Ｄ Ｊ Ｓａｌｍｏｎｄ，Ａ Ｆ Ｍ Ｓｍｉｔｈ．Ｎｏｖｅｌ ａｐｐｒｏａｃｈ ｔＯ ｎｏｎｌｉｎｅａｒ ａｎｄ ｎｏｎ—Ｇａｕｓｓｉａｎ Ｂａｙｅｓｉａｎ ｓｔａｔｅ ｅｓｔｉｍａｔｉｏｎ［Ａ］． ＩＥＥＥ Ｐｒｏｃｅｅｄｉｎｇｓ Ｆ［ｃ］．ＵＫ：ＩＥＥＥ，１９９３，１４０（２）：１０７ １１３． ［６］杨小军，潘泉，王睿．粒子滤波进展与展望［Ｊ］．控制理论与应 用，２００６，２３（２）：２６１—２６７． ［７］Ｋｏｔｅｃｈａ ＪＨ，Ｄｊｕｒｉｃ ＰＭ．Ｇａｕｓｓｉａｎ ｐａｒｔｉｃｌｅ ｆｉｌｔｅｒｉｎｇ［Ｊ］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｓｉｇｎａｌ Ｐｒｏｃｅｓｓｉｎｇ，２００３，５１（１０）：２５９２— ２６Ｏ１ ［８］ Ｋａｚｕｆｕｍｉ Ｉｔｏ，Ｘｉｏｎｇ Ｋａｉｑｉ．Ｇａｕｓｓｉａｎ ｆｉｌｔｅｒｓ ｆｏｒ ｎｏｎｌｉｎｅａｒ ｆｉｌ— ｔｅｒｉｎｇ ｐｒｏｂｌｅｍｓ［Ｊ￣．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｕｔｏｍａｔｉｃ Ｃｏｎ— ｔｒｏｌ，２０００，４５（５）：９１０—９２７． ［９］ 王宁，王从庆．高斯粒子滤波器及其在非线性估计中的应用 ［Ｊ］．南京航空航天大学学报。２００６，３８（ｚ１）：１３２—１３５． ［１ｏ］熊剑，郭杭，熊智，等．ＧＰＳ／ＩＮＳ组合导航系统中的高斯粒子 滤波混和算法［Ｊ］．中国惯性技术学报，２０１２，２ｏ（２）：２２５ ２２９． ［１１］韩松，张晓林，陈雷，等．基于改进高斯粒子滤波器的目标跟 踪算法［Ｊ］．系统工程与电子技术，２０１０，３２（６）：１１９１— １１９４． ［１２］周翟和，刘建业，赖际舟，等．一种新的改进高斯粒子滤波算 法及其在ＳＩＮＳ／ＧＰＳ深组合导航系统中的应用口］．控制与 决策，２０１１，２６（１）：８５—８８． ［１ ３］Ｍｕｓｓｏ Ｃ，Ｏｕｄｊａｎｅ Ｎ，Ｌｅｇｌａｎｄ Ｆ．Ｉｍｐｒｏｖｅｄ Ｒｅｇｕｌａｒｉｚｅｄ Ｐａｒ— ｔｉｃｌｅ Ｆｉｌｔｅｒ［Ｍ］．Ｓｅｑｕｅｎｔｉａｌ Ｍｏｎｔｅ Ｃａｒｌｏ Ｍｅｔｈｏｄｓ ｉｎ Ｐｒａｃ— ｔｉｃｅ．Ｎｅｗ Ｙｏｒｋ，Ｓｐｒｉｎｇｅｒ，２００１：２４７—２７２．
