第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)。 ②更新 万方数据