粒子滤波在GPS/DR组合导航中的应用
摘要：粒子滤波算法作为一种基于蒙特卡罗方法的递推贝叶斯估计，适用于非线性非高斯系统的分析，被广泛应用于跟踪、定位等问题的研究中。本文详细介绍了粒子滤波的基本原理并给出了粒子滤波在GPS/DR组合导航中的一个应用实例。
关键词：GPS/DR 组合导航 粒子滤波
中图分类号：TN713文献标识码：A文章编号：1007-9416(2011)08-0064-02
传统的卡尔曼滤波理论只适用于线性系统，要求系统模型准确，噪声为已知高斯过程，是一种线性最小方差估计。现实中非线性滤波问题是极为普遍的，卡尔曼滤波对于非线性系统并不适用 ，在非线性系统中应用最广泛的是扩展卡尔曼滤波（EKF），EKF的基本思想是将非线性模型线性化，然后再进行卡尔曼滤波。EKF实现简单，但由于线性化过程中会引起误差，导致估计精度下降，粒子滤波适用于强非线性、非高斯噪声系统模型的滤波。
1、GPS/DR组合导航模型
取GPS/DR组合导航系统的状态向量为 ，其中分别为车辆在k时刻的东向位置、速度和加速度，分别为k时刻的北向位置、速度和加速度，采用机体载体的“当前”统计模型来描述车辆加速度的统计分布，则系统的状态方程（参考文献[1]）为：
其中为系统噪声向量，为一步状态转移矩阵，
将GPS输出的东向位置信息，北向位置信息，角速率陀螺的输出以及里程计在一个采样周期内输出的距离为观测量，则系统的离散观测方程为：
其中、、为观测噪声，分别近似为、、、的高斯白噪声，可以看出系统的观测方程为非线性的。
2、粒子滤波算法
粒子滤波以蒙特卡罗方法和贝叶斯理论为基础，粒子滤波基本思想是首先根据系统状态向量的经验条件分布，在状态空间中产生一组随机样本集合，然后根据观测量不断地调整粒子的权重和位置，通过调整后的粒子信息，修正最初的经验条件分布，即粒子滤波利用状态空间中的随机样本对状态的后验概率密度进行近似。若为一后验概率密度函数，（i=1,2…N）为N个来自后验概率密度函数的独立同分布的随机变量，则后验概率密度函数可表示为：式中，为相应粒子的权重，这样就将贝叶斯估计中的积分问题转换为和运算的问题。然而后验概率密度函数一般为多维非标准函数，直接从后验概率密度抽样比较困难。这时就需要一个容易从中进行采样的密度函数，即重要性概率密度函数。通常的做法是选取先验概率密度为重要性密度函数，即则相应的权重可表示为 由此可得到基于序列重要性采样（SIS）的基本粒子滤波算法，其典型SIS算法步骤如下：
(1)从中随机抽取n个有限样本
(2)逐点计算对应的和
(3)利用权值公式计算对应样本的重要性权系数
(4)对权进行归一化处理
(5)对进行估计
序列重要性采样(SIS)算法存在的一个基本问题就是退化现象，即经过几步迭代递推后，许多粒子的权重变得非常小，大量的计算则浪费在小权值粒子上毫无意义。退化现象的程度可以由有效粒子数来衡量，有效粒子数定义如下：
减少粒子退化现象一个最直接的方法就是增加采样的粒子数，一味的增加粒子数会带来计算量大的问题，通常将重采样方法引入到SIS中。重采样的思想主要是去除权值小的粒子，增加那些权值大的粒子。在SIS的基础上采用重采样方法就形成了重要性重采样（SIR）粒子滤波算法，即标准粒子滤波算法[2-4]。
3、实验仿真
假定车辆以m/s的速度沿45°航向角匀速直线运动,共行驶500s。采样周期T= 1s,仿真条件和有关参数为:初始状态X0=[0,10,0,0,10,0],初始状态协方差矩阵P0=diag[100,1,0.04,100,1,0.04],观测噪声协方差矩阵R=diag[225,256,0.000025,0.49],系统噪声协方差矩阵Q=diag[0,0,0.09,0,0,0.09]，仿真结果如图1,图2：
由图1,图2可以看出经过粒子滤波之后车辆的东向位置误差主要控制在-5m～5m之间，而北向位置误差主要控制在-10m～10m之间。仿真结果表明, 采用粒子滤波的GPS/DR组合导航具有很高的导航精度。
4、结语
本文详细讲述了粒子滤波的基本原理，研究了PF算法在GPS/DR组合导航系统中的应用，采用标准粒子滤波算法对GPS/DR系统进行了仿真。仿真结果表明, 粒子滤波能够应用于非线性导航系统。
参考文献
[1]付梦印.Kalman滤波理论及其在导航系统中的应用.科学出版社,2003.
[2]胡士强.粒子滤波算法原理及其应用.科学出版社,2010.
[3]朱志宇.粒子滤波算法及其应用.科学出版社,2010.
[4]张共愿.粒子滤波算法及其在惯性导航系统中的应用.西北工业大学,2007.
注：本文中所涉及到的图表、注解、公式等内容请以PDF格式阅读原文