214 数据采集与 处 理 第28卷 入式单点GPS原始制造商(Original equipment man- 数，vk-1为系统噪声，：为观测噪声。 ufacture,OEMD模块进行定位。动态单点定位方式 1.1基于序贯重点采样的粒子滤波 在选择可用性(Selective availability,SA)政策取消 后，其圆概率误差(Circular error probable.,CEP)定 P℉的核心思想是利用有限个随机采样样本 位精度在理想情况下一般能达到10m左右。而在 (这些样本被称为“粒子”)的加权和来近似表示状 实际中，卫星的分布情况、天气情况、地理位置、接收 态变量的后验概率分布，从而得到状态的估计 机和天线等引人的系统误差的影响等都可能产生更 值10。 大的误差值。尤其是在城市环境中，受到多径效应 设k一1时刻有一组后验粒子集{x-1(i), 的影响，观测误差服从的分布不是严格的高斯分 wg-1(i);i=1,2,…,N},其中N为粒子数目， 布)。另外，在GPS的技术指标中通常给定的是 x-1(i)为k一1时刻的第i个粒子，wk-1(i)为k一1 CEP误差值，因此对个别测量值的误差值达到几十 时刻第i个粒子的权重。 米也并不少见。为了平滑定位输出数据，常用的滤 (1)粒子集初始化，k=0: 波技术有卡尔曼滤波(Kalman filtering,KF)、扩展卡 根据先验概率密度p(X,)抽取随机样本， 尔曼滤波(Extended Kalman filtering,EKF)和无迹 X。,X。,…,X。(N为随机样本数)。 卡尔曼滤波(Unscented Kalman filtering,UKF)等。 (2)当k=1,2,…时，执行以下步骤： 这些算法均要求观测噪声和过程噪声为独立不相关 (a)状态预测 的高斯白噪声，不能对非高斯的误差进行修正处 根据系统的状态方程抽取k时刻的先验粒子： 理2-。而粒子滤波(Particle filter,PF)算法作为一 {Xk-1(i);i=1,2,…,N〉~(XX-1)。 种非线性滤波方法，可以克服上述缺点，随着采样粒 (b)更新 子数的增大，逐渐趋向状态的后验概率密度，在解决 首先，进行权值更新。在获得测量值之后，根据 非高斯误差问题时具有明显的优势。因此，在处理 系统的观测方程并利用式(2)计算粒子的权值》 非线性非高斯问题时得到了广泛应用，如：卫星系统 we=w-1n(Z.|X.m）i=l,…,N(2) 姿态估计、目标跟踪等]。目前粒子滤波算法在 归一化权值为 GPS导航定位数据处理中也获得了广泛应用，将粒 (3) 子滤波用于动态单点定位、高动态定位中等？)，但 在精确度方面仍可改善，抑制粒子退化是方法之一。 文中首先介绍粒子滤波的原理，接着详细地对 然后，计算有效粒子数N“,并与设定的阈值 马尔可夫链蒙特卡罗(Markov chain Monte Carol, Nh进行比较；如果NH<Vhs,则对先验粒子集 MCMC)粒子滤波进行分析，给出所建立的系统动态 (X-:D,@⊙)进行重采样，得到N个等权值的 状态空间模型与观测模型。结合实例与基本P℉进 行比较，实验结果表明：MCMC粒子滤波算法能有 粒子(X-“,为.香则，执行下面的步囊 效降低GPS定位位置和速度误差，其滤波性能优于 (c)估计 基本P℉算法。 计算当前时刻系统的状态估计值 N MCMC粒子滤波算法描述 X=∑X-1as” (4) =1 P℉是一种基于蒙特卡罗模拟和递推贝叶斯估计 1.2MCMC粒子滤波算法 的滤波方法。Gordon提出基于蒙特卡罗方法的序贯 P℉的重采样抑制了权的退化，但也带来粒子 重要性重采样P℉算法以来，P℉成为非线性非高斯系 不再独立，简单的收敛性结果不再成立，甚至会引 统状态估计问题的一个研究热点，广泛用于自动控 起粒子贫化等问题。为此，对每个粒子引人MC 制、机器人技术、统计信号处理等研究领域)。 MC移动步骤来解决粒子退化问题[山。MCMC方 假设描述动态系统的状态方程和观测方程为 法对每个服从后验概率p(t:。0Iy1:k)的粒子 X4=∫4(Xk1,yk) (1) {元。：。}实施核为K(xo:|xo:A)的马尔可夫链变 Z:=h(X::ng) 式中：X。为状态向量，Z为测量向量，∫，为状态转 换，使得K(xo:A|交o)p(ok|y1:t)do= 移函数，为状态向量和观测向量之间的传递函 (xoy),则新粒子。®服从同样的概率密 万方数据214 数 据 采 集 与 处 理 第28卷 入式单点GPS原始制造商(Original equipment man— ufacture，OEM)模块进行定位。动态单点定位方式 在选择可用性(Selective availability，SA)政策取消 后，其圆概率误差(Circular error probable，CEP)定 位精度在理想情况下一般能达到10 m左右。而在 实际中，卫星的分布情况、天气情况、地理位置、接收 机和天线等引入的系统误差的影响等都可能产生更 大的误差值。尤其是在城市环境中，受到多径效应 的影响，观测误差服从的分布不是严格的高斯分 布L1 J。另外，在GPS的技术指标中通常给定的是 CEP误差值，因此对个别测量值的误差值达到几十 米也并不少见。为了平滑定位输出数据，常用的滤 波技术有卡尔曼滤波(Kalman filtering，KF)、扩展卡 尔曼滤波(Extended Kalman filtering，EKF)和无迹 卡尔曼滤波(Unscented Kalman filtering，UKF)等。 这些算法均要求观测噪声和过程噪声为独立不相关 的高斯白噪声，不能对非高斯的误差进行修正处 理[2门]。而粒子滤波(Particle filter，PF)算法作为一 种非线性滤波方法，可以克服上述缺点，随着采样粒 子数的增大，逐渐趋向状态的后验概率密度，在解决 非高斯误差问题时具有明显的优势。因此，在处理 非线性非高斯问题时得到了广泛应用，如：卫星系统 姿态估计、目标跟踪等[4。6]。目前粒子滤波算法在 GPS导航定位数据处理中也获得了广泛应用，将粒 子滤波用于动态单点定位、高动态定位中等[7墙]，但 在精确度方面仍可改善，抑制粒子退化是方法之一。 文中首先介绍粒子滤波的原理，接着详细地对 马尔可夫链蒙特卡罗(Markov chain Monte Carol， MCMC)粒子滤波进行分析，给出所建立的系统动态 状态空间模型与观测模型。结合实例与基本PF进 行比较，实验结果表明：MCMC粒子滤波算法能有 效降低GPS定位位置和速度误差，其滤波性能优于 基本PF算法。 MCMC粒子滤波算法描述 PF是一种基于蒙特卡罗模拟和递推贝叶斯估计 的滤波方法。Gordon提出基于蒙特卡罗方法的序贯 重要性重采样PF算法以来，PF成为非线性非高斯系 统状态估计问题的一个研究热点，广泛用于自动控 制、机器人技术、统计信号处理等研究领域[9]。 假设描述动态系统的状态方程和观测方程为 X女一ft(X卜1，1，卜1) … 磊=h女(X^，肛女) 式中：墨为状态向量，五为测量向量，^为状态转 移函数，h+为状态向量和观测向量之间的传递函 数，h一。为系统噪声，n。为观测噪声。 1．1 基于序贯重点采样的粒子滤波 PF的核心思想是利用有限个随机采样样本 (这些样本被称为“粒子”)的加权和来近似表示状 态变量的后验概率分布，从而得到状态的估计 值㈨。 设忌一1时刻有一组后验粒子集{z㈩(i)， 叫t一。(i)；i一1，2，…，N)，其中N为粒子数目， zt一，(i)为忌一1时刻的第i个粒子，叫卜。(i)为忌一1 时刻第i个粒子的权重。 (1)粒子集初始化，是一0： 根据先验概率密度P(X。)抽取随机样本， X0n’，X。∞’，…，X。Ⅲ’(N为随机样本数)。 (2)当志一1，2，…时，执行以下步骤： (a)状态预测 根据系统的状态方程抽取愚时刻的先验粒子： {X^l^一l(i)；i一1，2，…，N)～p(墨I Xk—1)。 (b)更新 首先，进行权值更新。在获得测量值之后，根据 系统的观测方程并利用式(2)计算粒子的权值(-Oku’ ∞^“’一C．Ok-1“’户(五I X^“’) i一1，…，N(2) 归一化权值为 甜t，：#，∑N甜。一，1(3) 叫^一1r一’厶叫^一 、o’ ∑∞+“㈡’1 然后，计算有效粒子数N叫，并与设定的阈值 Nm。进行比较；如果NeH<Nm。，则对先验粒子集 (墨"。“’，砚“’)进行重采样，得到N个等权值的 ． 1 粒子(x小～。“’，寺)。否则，执行下面的步骤。 』’ (c)估计 计算当前时刻系统的状态估计值 N 文^一1只x^I卜l㈤矾㈤ (4) 酉 1．2 MCMC粒子滤波算法 PF的重采样抑制了权的退化，但也带来粒子 不再独立，简单的收敛性结果不再成立，甚至会引 起粒子贫化等问题。为此，对每个粒子引入MC— MC移动步骤来解决粒子退化问题[11]。MCMC方 法对每个服从后验概率P(瓯：。“’I Y，。。)的粒子 {既：。“’)实施核为K(z。：。lz。，；)的马尔可夫链变 r 换，使得I K(x㈣l 37呲)P(37。：^I Y，；。)妇。：。= P(z。：。I y，。。)，则新粒子‰。。“’服从同样的概率密 万方数据