第28卷第2期 数据采集与处理 Vol.28 No.2 2013年3月 Journal of Data Acquisition &Processing Mar.2013 文章编号：1004-9037(2013)02-0213-06 MCMC粒子滤波的GPS定位数据处理算法 王尔申蔡明庞涛 (沈阳航空航天大学电子信息工程学院，沈阳，110136) 摘要：针对基于传统粒子滤波的GPS(Global positioning system)定位数据处理方法存在粒子退化的问题，研究了基于 马尔可夫链蒙特卡罗(Markov chain Monte Carol,MCMC)粒子滤波的GPS定位数据处理算法，引入典型的MCMC方 法一Metropolis Hastings(MH)抽样算法。利用观测伪距非高斯误差分布，建立重要密度函数，将MCMC粒子滤波 与建立的GPS系统非线性状态空间模型结合。实测数据实验结果表明，MCMC粒子滤波可有效柳制粒子逃化，解决 了GS定位数据滤波这一非线性非高斯问题，避免了噪声的高斯假设和非线性部分的线性化误差，与基于传统粒子滤 波的GS定位数据处理方法相比，该方法降低了定位数据经纬度和速度估计误差，获得了更高的定位精度，并能够在 GPS信号质量较差情况下，对GS定位数据有效滤波，保证载体在此期间内保持较高的位置精度。 关键词：全球定位系统：拉子滤波：马尔可夫链蒙特卡罗方法；数据处理 中图分类号：V241.6;TN967,1 文献标志码：A GPS Positioning Data Processing Algorithm Based on MCMC Particle Filter Wang Ershen,Cai Ming,Pang Tao (School of Electronic and Information Engineering,Shenyang Aerospace University,Shenyang,110136,China) Abstract:Global positioning system (GPS)positioning data processing algorithm based on the Markov chain and Monte Carlo (MCMC)particle filter is discussed to solve the problem of GPS positioning data processing based on the standard particle filter (PF)suffers from severe sample degeneracy.The standard MCMC method,Metropolis Hastings (M-H)sampling,is incorporated into the particle filter algorithm framework,and applied to GPS positioning data processing.It combines the particle filter with the GPS system nonlinear dynamic state-space model.The MCMC method is adapted to solve the degeneracy phenomenon of particle filter.It is effective to nonlinear and non-Gaussian state estimation problems in GPS positioning data processing.Experimental results based on real GPS data show that the MCMC particle filter can increase the sample variety and reduce sample degeneracy.GPS positioning data processing based on the MCMC particle filter reduces the error of position and velocity,compared with GPS positioning data processing based on the standard particle filter.Moreover,the MCMC particle filter can provide a highly accurate positioning value as an aided method when the GPS signal quality is poor. Key words:GPS;particle filtering;Markov chain Monte Carlo (MCMC)method;data pro- cessing GPS)在军事和民用方面均得到了越来越广泛的应 引 言 用。比如在汽车和船舰导航、交通管理、车辆监控、 警车指挥、航空摄影、海洋捕捞作业和农业工程等应 目前，全球定位系统(Global positioning system, 用领域，目前多采用动态单点式的GPS接收机或嵌 基金项目：国家自然科学基金(61101161)资助项目：航空科学基金(2011ZC54010)资助项目：辽宁省博士启动基金 (20101081)资助项目。 收稿日期：2012-08-12；修订日期：2012-10-18 万方数据
第28卷第2期 2013年3月 数 据 采 集 与 处 理 Journal of Data Acquisition&Processing V01．28 No．2 Mar．2013 引 文章编号：1004—9037(2013102—0213—06 MCMC粒子滤波的GPS定位数据处理算法 王尔申 蔡 明 庞 涛 (沈阳航空航天大学电子信息工程学院，沈阳，110136) 摘要：针对基于传统粒子滤波的GPS(Global positioning system)定位数据处理方法存在粒子退化的问题，研究了基于 马尔可夫链蒙特卡罗(Markov chain Monte Carol，MCMC)粒子滤波的GPS定位数据处理算法，引入典型的MCMC方 法——Metro叫is Hastings(M-H)抽样算法。利用观测伪距非高斯误差分布，建立重要密度函数，将MCMC粒子滤波 与建立的GPS系统非线性状态空间模型结合。实测数据实验结果表明，MCMC粒子滤波可有效抑制粒子退化，解决 了GPS定位数据滤波这一非线性非高斯问题，避免了噪声的高斯假设和非线性部分的线性化误差，与基于传统粒子滤 波的GPS定位数据处理方法相比，该方法降低了定位数据经纬度和速度估计误差，获得了更高的定位精度，并能够在 GPS信号质量较差情况下，对GPS定位数据有效滤波，保证栽体在此期间内保持较高的位置精度。 关键词：全球定位系统；粒子滤波；马尔可夫链蒙特卡罗方法；数据处理 中图分类号：V241．6；TN967．1 文献标志码：A GPS Positioning Data Processing Algorithm Based on MCMC Particle Filter Wang Ershen，Cai Ming，Pang Tao (School of Electronic and Information Engineering，Shenyang Aerospace University，Shenyang，110136，China) Abstract：Global positioning system(GPS)positioning data processing algorithm based on the Markov chain and Monte Carlo(MCMC)particle filter iS discussed to solve the problem of GPS positioning data processing based on the standard particle filter(PF)suffers from severe sample degeneracy．The standard MCMC method，Metropolis Hastings(M—H)sampling，is incorporated into the particle filter algorithm framework，and applied tO GPS positioning data processing．It combines the particle filter with the GPS system nonlinear dynamic state—space model．The MCMC method is adapted to solve the degeneracy phenomenon of particle filter．It is effective to nonlinear and non—Gaussian state estimation problems in GPS positioning data processing．Experimental results based on real GPS data show that the MCMC particle filter can increase the sample variety and reduce sample degeneracy．GPS positioning data processing based on the MCMC particle filter reduces the error of position and velocity，compared with GPS positioning data processing based on the standard particle filter．Moreover，the MCMC particle filter can provide a highly accurate positioning value as an aided method when the GPS signal quality is poor． Key words：GPS；particle filtering；Markov chain Monte Carlo(MCMC)method；data pro— cessing 目前，全球定位系统(Global positioning system， GPS)在军事和民用方面均得到了越来越广泛的应 用。比如在汽车和船舰导航、交通管理、车辆监控、 警车指挥、航空摄影、海洋捕捞作业和农业工程等应 用领域，目前多采用动态单点式的GPS接收机或嵌 基金项目：国家自然科学基金(61101161)资助项目；航空科学基金(201IZC54010)资助项目；辽宁省博士启动基金 (20101081)资助项目。 收稿日期：2012—08—12；修订日期：2012—10—18 {||日 万方数据
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，。。)，则新粒子‰。。“’服从同样的概率密 万方数据
第2期 王尔申，等：MCMC粒子滤波的GPS定位数据处理算法 215 度，并且新的粒子已经移到不同的位置，分布更加 式中：w,w分别为(0，)，(0，a)的高斯白噪 合理，避免了粒子贫化问题。文中在P℉基本算法 声；xe,tN分别为运动载体东向和北向的机动加速 基础中引人Metropolis Hastings(M-H)抽样方 度变化率的相关时间常数；aE,aN分别为运动载体 法。具体过程为： 的东向和北向机动加速度分量的当前均值。 (1)从区间[0,1]按照均匀概率分布抽样得到 门限值u,u~Uo,]。 2.2观测方程 (2)依据重要性概率密度函数p(x|x-1⑧)抽 将GPS接收机输出的东向位置信息、北向 样得到工-1，即x-1~p(xxg-1)。 位置信息xN、速度以及方向角0作为观测量，即 (3)如果u<min{1,p(ya|xk-:a)/p(y| Z=[zzwv]T。则，观测量和状态变量之 x-1分)}，则工0=x-10；否则，x:0= 间的关系如下 1k-1D。 之E=xE十EE 2基于粒子滤波的GPS定位数据处理 ZN =IN+EN 由于GPS信号是视距传播，GPS信号易受到 v=√十十ew 建筑物及树木等物体的遮挡，此时，GPS接收机定 0=arctan+ 位误差将变大或者无法进行定位，降低了定位数据 UN 的可用性，加上GPS信号受地面反射的多径信号 于是，系统的观测方程为 影响，都将引起定位误差变大，造成其在应用过程 TE [ZE CE 中定位偏离真实位置的问题，GPS的定位精度一 IN 般在10m以内，但有时定位误差会更大)。为 ZN Z √十环 (6) 此，引入P℉算法对GPS数据进行处理，改善GPS 定位效果。 arctan Vg Ea UN 2.1运动状态的描述与状态方程 式中：eE,eN分别是GPS接收机输出的东向位置和 GPS接收机输出的参数包括经度、纬度、速 北向位置的观测噪声，可近似为(0)，(0，品)的 度、方向角等信息。根据距离、速度以及加速度之 高斯白噪声；e,ea为接收机输出的速度和方向角 间的关系，列出系统状态方程和系统输出方程。 的观测噪声，可近似为(0，a),(0,)的高斯白噪 选取状态变量为X=[xE,E,aE,xN,N, 声。 aw]T,其中，xE,xN分别为运动载体东向和北向的 位置分量；E和分别为运动载体东向和北向的 2.3基于MCMC粒子滤波的GPS定位数 速度分量；aE和aN分别为运动载体东向和北向的 据处理算法 加速度分量。则系统的状态方程为 PF用于GPS定位数据处理的算法描述为： (t)=AX(t)+U+W(t) (5) 设k一1时刻有一组后验粒子集为{x4-1(i), 01 000 0 g-1(i):i=1,2,…,N},其中N为粒子数目， 001 00 0 xk-1(i)为k一1时刻的第i个粒子，w-1(i)为k一1 00 -1/xe00 0 A= 时刻第i个粒子的权重。 00001 0 (1)粒子集初始化，k=0: 00 0 00 1 在初始值X点（载体的初始位置：东经123° 00 0 00 -1/EN] 24.15436'、北纬41°55.53523)，根据先验概率密 0 「07 度产生N个粒子，得到粒子集{}1，并设置初 0 始化权值为u,=1/N。 (2)Fork=1,2,…执行以下步骤： TE U= ,W(t)= 0 0 ①状态预测根据系统的状态方程抽取k时刻 0 0 的先验粒子{Xg-1(i):i=1,2,…,N}~p(Xk| N X-1)。 ②更新 万方数据
第2期 王尔申，等：MCMC粒子滤波的GPS定位数据处理算法 215 度，并且新的粒子已经移到不同的位置，分布更加 合理，避免了粒子贫化问题。文中在PF基本算法 基础中引入Metropolis Hastings(M—H)抽样方 法。具体过程为： (1)从区间[o，1]按照均匀概率分布抽样得到 门限值U，M～U[o'11。 (2)依据重要性概率密度函数p(x。I zt一。“’)抽 样得到z小一l“’，即z小一l“’～p(x^I z^一1)。 (3)如果U<rain{1，P(挑I z小一1“’)／p(弘l 2女I±一l“’)}，贝4．27々“’=zill—l“’；否则，z}“’= 氟j^一l‘o。 2基于粒子滤波的G】PS定位数据处理 由于GPS信号是视距传播，GPS信号易受到 建筑物及树木等物体的遮挡，此时，GPS接收机定 位误差将变大或者无法进行定位，降低了定位数据 的可用性，加上GPS信号受地面反射的多径信号 影响，都将引起定位误差变大，造成其在应用过程 中定位偏离真实位置的问题，GPS的定位精度一 般在10 m以内，但有时定位误差会更大¨2|。为 此，引入PF算法对GPS数据进行处理，改善GPS 定位效果。 2．1 运动状态的描述与状态方程 GPS接收机输出的参数包括经度、纬度、速 度、方向角等信息。根据距离、速度以及加速度之 间的关系，列出系统状态方程和系统输出方程。 选取状态变量为x一[zE，uE，口E，zN，73N， aN]T，其中，z。，zN分别为运动载体东向和北向的 位置分量；口。和刁N分别为运动载体东向和北向的 速度分量；ae和a。分别为运动载体东向和北向的 加速度分量。则系统的状态方程为 X(t)一AX(t)+【，+W(t) (5) A== 0 1 O O O 0 0 1 0 0 0 0—1／rE 0 0 O 0 O O 1 0 O O 0 0 O O O 0 O U== O 0 1 一仃E rE O O 1 一盯N rN ，W(￡)一 0 0 0 O 1 —1／rN 0 O O)E 0 O 叫N 式中：OgE，∞N分别为(0，盯：。)，(o，如2。)的高斯白噪 声；r。，rN分别为运动载体东向和北向的机动加速 度变化率的相关时间常数；a。，aN分别为运动载体 的东向和北向机动加速度分量的当前均值。 2．2观测方程 将GPS接收机输出的东向位置信息2。、北向 位置信息2N、速度掣以及方向角0作为观测量，即 z=■e zN口日]T。则，观测量和状态变量之 间的关系如下 zE=zE+eE 2N=zN+eN u一√口t+口☆+￡。 0一arctan V_E_E+￡日 口N 于是，系统的观测方程为 Z=2 ZE ZN U 日 XE XN 、厅丽 arctan堡口N + eE ￡N ￡。 ￡口 (6) 式中：e。，eN分别是GPS接收机输出的东向位置和 北向位置的观测噪声，可近似为(0，髭)，(o，口‰)的 高斯白噪声；e。，幻为接收机输出的速度和方向角 的观测噪声，可近似为(o，盯2。)，(o，彳)的高斯白噪 声。 2．3基于MCMC粒子滤波的GPS定位数 据处理算法 PF用于GPS定位数据处理的算法描述为： 设k一1时刻有一组后验粒子集为{X。一。(i)， c￡，t一。(i)：i=1，2，…，N)，其中N为粒子数目， z卜，(i)为是一1时刻的第i个粒子，∞。一。(i)为k一1 时刻第i个粒子的权重。 (1)粒子集初始化，k=0： 在初始值凰点(载体的初始位置：东经123。 24．154367、北纬41。55．53523 7)，根据先验概率密 度产生N个粒子，得到粒子集{x：}墨。，并设置初 始化权值为∞。“’一1／N。 (2)For k=1，2，…执行以下步骤： ①状态预测根据系统的状态方程抽取k时刻 的先验粒子{X^i女一。(i)：i=1，2，…，N)～P(Xk Xl一1)。 ②更新 万方数据
216 数据采集与处理 第28卷 (a)权值更新在获得测量值之后，根据系统的 处理，得到结果如图1所示。 观测方程并利用式(7)计算粒子的权值)。 图1中横轴为经度，纵轴为纬度，单位10-3′。 w4⊙=w-op(Z4|Xo）i=1,…,N(7) 图中，a点所示为运动的初始位置，b点附近由于操 归一化权值为 场看台遮阳棚的遮挡，数据采集平台未正常接收到 N (8) GPS定位数据，故图中量测状态曲线在b点附近 ∑台 出现间断，PF和MCMC粒子滤波依然可以给出 状态估计，说明在没有量测数据可利用的情况下， (b)重要性重采样对先验粒子集(X-1®， 利用P℉算法也可以根据以前的经验对状态X作 而。)进行采样，得到粒子集合为(X-1⑧，i=1, 出估计。但从图1可以明显看出，MCMC粒子滤 …,N)。 波对状态的估计性能要优于基本PF。两种算法处 (c)M-H抽样利用MCMC方法对(b)中得到 理后的不同参数如表1所示。 的粒子进行M-H采样，得到新的粒子集{X, 640 1/N;i=1,…N} 620 ③估计计算当前时刻系统的状态估计值 600 X=∑X-1oa:0 实际位置 580 MCMC活计值 3 实测实验与结果分析 560 3.1实验条件 540 利用U-BLOX公司的RCB4HGPS接收机 520%809010010120130140150160 作为实验数据采集设备，在沈阳航空航天大学操场 经度/103 绕椭圆形跑道一圈，采集的数据包括经度、纬度、速 图1MCMC粒子滤波和PF的结果对比 度、方向角等。采样频率设置为最大更新率4Hz, GPS接收机初始位置为东经123°24.15436'、北纬 表1采用不同算法处理后的参数比较 4155.53523'、初始速度为0，方向角为164.22°， 算法 粒子数平均有效样本RMSE 采集数据时间为5min。RCB4H型GPS接收机， PF 100 24.6501 4.9234 其定位精度为2.5m(CEP)。 MCMC粒子滤波100 29.0548 3.1786 3.2实验数据处理与结果分析 如表1所示，粒子数目同为N=100,且过程噪 首先对所采集到的经纬度数据进行预处理。 声方差Q=5、量测噪声方差R=1时，MCMC粒子 由于实验中GPS数据采集系统移动范围相对较 滤波算法的RMSE=3.1786,基本PF算法的 小，获取的位置经度和纬度数值仅以分的小数部分 RMSE=4.9234,说明MCMC粒子滤波比基本 发生变化，而其度和分的整数部分不变。为此，为 PF算法估计精度高。此时，MCMC粒子滤波的有 了使实验的结果更加直观，方便程序计算，对采集 效样本为29.0548，PF为24.6501，说明MCMC 到位置的经度和纬度数据进行如下处理，具体为： 粒子滤波比基本P℉更能有效地抑制样本退化，而 去除经纬度数据中小数点前面的数据位，将小数点 且，在同等条件下，有效样本越多，估计精度越高。 后面的数据扩大1000倍。速度单位转换为m/s。 MCMC粒子滤波和基本PF在定位经纬度和速度 如：其中一组数据为：经度为123°24.15436'，纬度 的估计误差对比曲线如图2~5所示。 为41°55.53523'，速度为1.046节/s,方向角为 图2,3为PF的经度和纬度的估计误差对比 164.22°。预处理后数据为：经度154.36×10-3， 曲线。图中横轴为时间，单位为S,纵轴为经度误 纬度535.23×10-3，速度0.5381m/s,方向角 差，单位为10-3'。图4和图5横轴为时间，单位为 164.22°。其他数据做类似的处理，不再赘述。 $,纵轴分别为东向速度误差和北向速度误差，单位 将采集到的数据作为实验数据，对这些数据分 为m/s。 别利用文中1.1节的P℉和MCMC粒子滤波进行 从图2~5可知，MCMC粒子滤波算法在GPS 万方数据
216 数 据 采 集 与 处 理 第28卷 (a)权值更新在获得测量值之后，根据系统的 观测方程并利用式(7)计算粒子的权值∞。‘”。 ∞^“’=(Ok-．-l“’夕(厶I墨“’)i=1，…，N(7) 归一化权值为 f订 N 0—3t㈤=}，∑05。∽一1(8) ∑∞；“㈡叫 i一1 (b)重要性重采样对先验粒子集(Xm一。“’， 亩t“’)进行采样，得到粒子集合为(X叭一。“’，i一1， …，N)。 (c)M—H抽样利用MCMC方法对(b)中得到 的粒子进行M—H采样，得到新的粒子集{嬲“， 1／N；i一1，…N) ③估计计算当前时刻系统的状态估计值 N X^一≥：溉㈣“h03^“’ i笠1 3实测实验与结果分析 3．1 实验条件 利用U—BLOX公司的RCB_4H GPS接收机 作为实验数据采集设备，在沈阳航空航天大学操场 绕椭圆形跑道一圈，采集的数据包括经度、纬度、速 度、方向角等。采样频率设置为最大更新率4 Hz， GPS接收机初始位置为东经123。24．154 36’、北纬 41。55．535 23’、初始速度为0，方向角为164．22。， 采集数据时间为5 min。RCB-4H型GPS接收机， 其定位精度为2．5 m(CEP)。 3．2实验数据处理与结果分析 首先对所采集到的经纬度数据进行预处理。 由于实验中GPS数据采集系统移动范围相对较 小，获取的位置经度和纬度数值仅以分的小数部分 发生变化，而其度和分的整数部分不变。为此，为 了使实验的结果更加直观，方便程序计算，对采集 到位置的经度和纬度数据进行如下处理，具体为： 去除经纬度数据中小数点前面的数据位，将小数点 后面的数据扩大l 000倍。速度单位转换为m／s。 如：其中一组数据为：经度为123。24．154 367，纬度 为41。55．535 23 7，速度为1．046节／s，方向角为 164．22。。预处理后数据为：经度154．36×10～， 纬度535．23×10～，速度0．538 1 m／s，方向角 164．22。。其他数据做类似的处理，不再赘述。 将采集到的数据作为实验数据，对这些数据分 别利用文中1．1节的PF和MCMC粒子滤波进行 处理，得到结果如图1所示。 图1中横轴为经度，纵轴为纬度，单位101’。 图中，口点所示为运动的初始位置，b点附近由于操 场看台遮阳棚的遮挡，数据采集平台未正常接收到 GPS定位数据，故图中量测状态曲线在b点附近 出现间断，PF和MCMC粒子滤波依然可以给出 状态估计，说明在没有量测数据可利用的情况下， 利用PF算法也可以根据以前的经验对状态X作 出估计。但从图1可以明显看出，MCMC粒子滤 波对状态的估计性能要优于基本PF。两种算法处 理后的不同参数如表1所示。 r、 甲 2 ∑ 划 婚 经度，10-3’ 图1 MCMC粒子滤波和PF的结果对比 表l 采用不同算法处理后的参数比较 如表1所示，粒子数目同为N=100，且过程噪 声方差Q=5、量测噪声方差R一1时，MCMC粒子 滤波算法的RMSE一3．178 6，基本PF算法的 RMSE一4．923 4，说明MCMC粒子滤波比基本 PF算法估计精度高。此时，MCMC粒子滤波的有 效样本为29．054 8，PF为24．650 1，说明MCMC 粒子滤波比基本PF更能有效地抑制样本退化，而 且，在同等条件下，有效样本越多，估计精度越高。 MCMC粒子滤波和基本PF在定位经纬度和速度 的估计误差对比曲线如图2～5所示。 图2，3为PF的经度和纬度的估计误差对比 曲线。图中横轴为时间，单位为S，纵轴为经度误 差，单位为10_’。图4和图5横轴为时间，单位为 S，纵轴分别为东向速度误差和北向速度误差，单位 为m／s。 从图2～5可知，MCMC粒子滤波算法在GPS 万方数据
第2期 王尔申，等：MCMC粒子滤波的GPS定位数据处理算法 217 3.0 10 南P℉估计值 ★P℉估计值 MCMC估计值 口MCMC估计值 2.5 2.0 10 1.5 o 1.0 10 10 0.5 10 100 150200250300 0 50 100 150200 250 300 时间/s 时间/s 图5 MCMC粒子滤波和PF算法北向速度估计误差对比 图2 MCMC粒子滤波和PF算法经度估计误差对比 4 结束语 P℉估计值 MCMC估计值 将MCMC粒子滤波算法应用于GPS定位数 据处理问题中，解决了P℉算法在粒子抽样过程中 粒子退化问题，提高了对接收机定位状态的估计 精度。通过建立动态状态空间模型，利用GPS数 据采集设备获取的实验数据分别对PF算法和 MCMC粒子滤波算法的滤波效果进行了实验验 证，结果表明MCMC粒子滤波可以有效地抑制样 00 0 0 1 8 b口 本退化，其滤波效果优于P℉，提高了GPS定位数 据处理的效果。同时，在GPS信号质量较差或者 0 50100150200250300 时间/s 短时无法定位时，MCMC滤波算法作为一种辅助 图3 方案为系统提供了较准确的导航定位数据。本研 MCMC粒子滤波和PF算法纬度估计误差对比 究的实验表明MCMC粒子滤波算法在GPS定位 数据处理和改善定位状况方面是可用的且有效的。 5.0 ★PF估计值 MCMC估计值 参考文献： [1]Sayim I,Pervan B,Pullen S,et al.Experimental 35 and theoretical results on the LAAS sigma overbound 0501510 [C]//Proceedings of the ION GPS.Portland,Ore- gon:[s.n.,2002:29-38. [2] Mojtaba Bahrami,Marek Ziebart.A Kalman filter based Dopper-smoothing of code pseudoranges in GNSS challenged environments[C]//Proceedings of the ION GNSS 2011.Portland:[s.n.]2011:1-11. 0.0 50 100 150 200 250 300 [3] Zhou Junchuan,Yang Yuhong,Zhang Jieying,et al. 时间/s Applying quaternion-based unscented particle filter on 图4MCMC粒子滤波和PF算法东向速度估计误差对比 INS/GPS with field experiments[C]//Proceedings of the ION GNSS 2011.Portland:[s.n.],2011:1-14. [4] 吴海亮，王惠南，陈志明，等.基于粒子滤波的微小卫 定位的经纬度和速度的处理效果方面要优于基本 星姿态确定算法[」].中国惯性技术学报，2007,15 P℉，提高了GPS定位数据参数的可用性。该算法 (4):426-430. 实现中的参变量不需要其他辅助设备，可用于汽 Wu Hailiang,Wang Huinan,Chen Zhiming,et al. 车、轮船、航空等运动载体定位数据的滤波处理。 Particle filtering-based algorithm for micro-satellite 万方数据
第2期 王尔申，等：MCMC粒子滤波的GPS定位数据处理算法 217 ● b o ＼ jIl|j 媸 蜊 骊 时间／s f ? 邑＼ j{llj 媸 魁 蝴 ·匠 菩 时间／s 图2 McMc粒子滤波和PF算法经度估计误差对比 图5 McMc粒子滤波和PF算法北向速度估计误差对比 ●n 2 、一＼ j|{1j 账 {趟 婚 时间／8 图3 MCMC粒子滤波和PF算法纬度估计误差对比 f ? 邑＼ jll|j 账 越 制 ·叵 长 时间／s 图4 MCMC粒子滤波和PF算法东向速度估计误差对比 定位的经纬度和速度的处理效果方面要优于基本 PF，提高了GPS定位数据参数的可用性。该算法 实现中的参变量不需要其他辅助设备，可用于汽 车、轮船、航空等运动载体定位数据的滤波处理。 4 结束语 将MCMC粒子滤波算法应用于GPS定位数 据处理问题中，解决了PF算法在粒子抽样过程中 粒子退化问题，提高了对接收机定位状态的估计 精度。通过建立动态状态空间模型，利用GPS数 据采集设备获取的实验数据分别对PF算法和 MCMC粒子滤波算法的滤波效果进行了实验验 证，结果表明MCMC粒子滤波可以有效地抑制样 本退化，其滤波效果优于PF，提高了GPS定位数 据处理的效果。同时，在GPS信号质量较差或者 短时无法定位时，MCMC滤波算法作为一种辅助 方案为系统提供了较准确的导航定位数据。本研 究的实验表明MCMC粒子滤波算法在GPS定位 数据处理和改善定位状况方面是可用的且有效的。 参考文献： [1] [2] [3] F4] Sayim I，Pervan B。Pullen S，et a1．Experimental and theoretical results on the LAAS sigma overbound [c]／／Proceedings of the ION GPS．Portland，Ore— gon：Is．n．]，2002：29—38． Moj taba Bahrami，Marek Ziebart．A Kalman filter based Dopper-smoothing of code pseudoranges in GNSS challenged environments[c]／／Proceedings of the ION GNSS 2011．Portland：Is．n．]，2011：卜11． Zhou Junchuan，Yang Yuhong，Zhang Jieying，et a1． Applying quaternion—based unscented particle filter on INS／GPS with field experimentsEC]／／Proceedings of the ION GNSS 2011．Portland：Is．n．]，2011：卜14． 吴海亮，王惠南，陈志明，等．基于粒子滤波的微小卫 星姿态确定算法[J]．中国惯性技术学报，2007，15 (4)：426—430． Wu Hailiang，Wang Huinan，Chen Zhiming，et a1． Particle filtering-based algorithm for micro-satellite 万方数据
218 数据采集与处理 第28卷 altitude determination[J.Journal of Chinese Inertial and its application for kinematic GPS positioning with Technology,2007,15(4):426-430. high speed movement[J].Acta Geodaetica et Carto- [5]章飞，周杏鹏，陈小惠.基于粒子滤波的交互式多模型 graphica Sinica,2007,36(3):274-278. 多机动目标跟踪[J].数据采集与处理，2011,26(2)： [9]Fredrik Gustafsson,Fredrik Gunnarsson.Particle filters 181-187. for positioning,navigation,and tracking[J].IEEE Zhang Fei,Zhou Xingpeng,Chen Xiaohui.Interac- Trans on Signal Processing,2002,50(2):425-437. ting multiple model tracking algorithm of multiple [10]Arulampalam M S,Maskell S,Gordon N,et al.At- maneuvering targets based on particle filter[J].Jour- utorial on particle filters for online nonlinear/non- nal of Data Acquisition &Processing,2011,26(2): Gaussian Bayesian tracking[J].IEEE Transactions 181-187. on Signal Processing,2002,50(2):174-188. [6]程水英，张剑云.粒子滤波算法中的有限收敛界[J门， [11]赵琳，聂琦，高伟.基于MCMC方法的正则粒子滤波 数据采集与处理，2008,23(4)：481-485. 算法及其应用[J].仪器仪表学报，2008,29(10)： Cheng Shuiying,Zhang Jianyun.Limited conver- 2156-2162. gence bound in particle filtering algorithms[J].Jour- Zhao Lin,Nie Qi,Gao Wei.Regularized particle fil- nal of Data Acquisition Processing,2008,23(4): tering algorithm and its application based on MCMC 481-485. method[J].Chinese Journal of Scientific Instrument, [7]聂建亮，杨元喜，吴富梅.一种基于改进粒子滤波的动 2008,29(10):2156-2162. 态精密单点定位算法[J].测绘学报，2010,39(4)： [12]Kaplan E,Hegarty C.Understanding GPS:princi- 338-343. ples and applications[M].2nd Edition.MA,US: Nie Jianliang,Yang Yuanxi,Wu Fumei.An algo- Artech House,Inc,2006:239-264. rithm of dynamic precise point positioning based on modified particle filtering[J].Acta Geodaetica et 作者简介：王尔申(1980-)，男，副教授，研究方向：GPS接收 Cartographica Sinica,2010,39(4):338-343. 机信号处理、卫星导航，E-mail:wes2016@sau.edu.cn;蔡 [8]田世君，陈俊，皮亦鸣，粒子滤波在高动态GPS定位 明(1987-)，男，硕士研究生，研究方向：GPS接收机自主完 中的应用[J].测绘学报，2007,36(3)：274-278. 好性监测算法；庞涛(1976-)，女，博士研究生，研究方向：卫 Tian Shijun,Chen Jun,Pi Yiming.Particle filtering 星导航、智能控制。 万方数据
218 数 据 采 集 与 处 理 第28卷 E5] [63 I-71 [8] altitude determination[J]．Journal of Chinese Inertial Technology，2007，15(4)：426—430． 章飞，周杏鹏，陈小惠．基于粒子滤波的交互式多模型 多机动目标跟踪[J]．数据采集与处理，2011，26(2)： 181—187． Zhang Fei，Zhou Xingpeng，Chen Xiaohui．Interac— ring multiple model tracking algorithm of multiple maneuvering targets based on particle filter[J]．Jour— nal of Data Acquisition＆Processing，2011，26(2)： 18卜187． 程水英，张剑云．粒子滤波算法中的有限收敛界[J]． 数据采集与处理，2008，23(4)：481—485． Cheng Shuiying，Zhang Jianyun．Limited conver— gence bound in particle filtering algorithms[J]．Jour— nal of Data Acquisition&Processing，2008，23(4)： 481—485． 聂建亮，杨元喜，吴富梅．一种基于改进粒子滤波的动 态精密单点定位算法[J]．测绘学报，2010，39(4)： 338—343． Nie Jianliang，Yang Yuanxi，Wu Fumei．An algo— rithm of dynamic precise point positioning based on modified particle filtering[J]．Acta Geodaetica et Cartographica Sinica，2010，39(4)：338—343． 田世君，陈俊，皮亦鸣．粒子滤波在高动态GPS定位 中的应用[J]．测绘学报，2007，36(3)：274—278． Tian Shijun，Chen Jun，Pi Yiming．Particle filtering [9] [10] [11] [123 and its application for kinematic GPS positioning with high speed movement[J]．Acta Geodaetica et Carto— graphica Sinica，2007，36(3)：274—278． Fredrik Gustafsson，Fredrik Gunnarsson．Particle filters for positioning，navigation，and tracking[J]．IEEE Trans on Signal Processing，2002，50(2)：425—437． Arulampalam M S，Maskell S，Gordon N，et a1．At— utoriaI on particle filters for online nonlinear／nonGaussian Bayesian tracking[J]．IEEE Transactions on Signal Processing，2002，50(2)：174—188． 赵琳，聂琦，高伟．基于MCMC方法的正则粒子滤波 算法及其应用[J]．仪器仪表学报，2008，29(10)： 2156—2162． Zhao Lin，Nie Qi，Gao Wei．Regularized particle fil— tering algorithm and its application based on MCMC method[J]．Chinese Journal of Scientific Instrument， 2008，29(10)：2156—2162． Kaplan E，Hegarty C． Understanding GPS：princi— ples and applications[M]．2nd Edition．MA，US： Artech House，Inc，2006：239-264． 作者简介：王尔申(1980一)，男，副教授，研究方向：GPS接收 机信号处理、卫星导航，E-mail：wes2016@sau．edu．cn；蔡 明(1987一)，男，硕士研究生，研究方向：GPS接收机自主完 好性监测算法；庞涛(1976一)，女，博士研究生，研究方向：卫 星导航、智能控制。 万方数据
