GPS单点定位动态滤波算法研究

… … … … … … … … …… …
0 h xm 0 0 hym 0 0 hzm 0 0 - 1
量测噪声协方差 : 2 2 2 2 2 2 2 2 σ σ σ σ σ σ σ R ( n) = [σ 1 , 2 , 3 , 4 , 5 , 6 , 7 , 8 ]
2 2 σ i≤ 4 ) 为伪距观测方差 ,σ i≤ 8 ) 为 ai i (1 ≤ i (5 ≤
03 ×3 03 ×3
2 λ σ 2 ¨ z Q¨ z ( n)
03 ×2 03 ×2 03 ×2
2 λ σ 2 ξ ξQ ξ ( n)
足如下公式 :
ai = x u ・h xj + y u ・h yj + z u ・hzj - ξ+ n2 i ( t) = p i ( X ( t) , t) - ξ( t) + n2 i ( t) ( 5)
采用最小二乘法和动态卡尔曼滤波法时 , x 、
y 方向的位置误差分别如图 1 、 图 2 所示 。
^ z u - x sj hzj = ^ rj
将式 ( 4) , 式 ( 5) 合并可得 : ρ ξ( t) 4 ×1 1 ×m r ( X ( t) , t) Z( t) = = + p ( X ( t) , t) a1 ×m - ξ( t) 4 ×1
Vol. 29 No . 2 Ap r 2009
GPS 单点定位动态滤波算法研究
周桃云1 ,张 怡2 ,伍铁斌1
(1 湖南人文科技学院 ,湖南娄底 417000 ; 2 西北工业大学电子信息学院 ,西安 710072)
3
摘 要 : 将卡尔曼最优估计理论应用于 GPS 导航定位系统 ,它主要针对高动态用户设计了一种基于卡尔曼最 优估计理论的定位算法 ,采用卡尔曼滤波技术来提高动态 GPS 导航定位精度和动态性能 ; 对最小二乘法和卡 尔曼滤波法的定位系统进行了仿真 ,并对定位误差和测速误差作了比较分析 。结果表明 ,与最小二乘法相比 , 动态卡尔曼滤波算法能得到较好的滤波效果 ,定位精度有了明显提高 。 关键词 : GPS ; 最小二乘算法 ; 卡尔曼滤波算法 ; 高动态 中图分类号 : V2491 3 ; P2281 4 文献标志码 :A
04 ×4 04 ×4 0 1 0 0
04 ×4 0 1 -λ
似为 :
0 0 1 -λ t
Q¨ y ( n) = Q¨ z ( n) =
T
T T T T Q ξ ( n) = T
3
5
T T T T
2
4
T T
3
F x = F y = Fz = 0
, Ft =
20
4
8
3
6
2
0
T U ( t) = [ 0 0 λ 0 0 λ 0 0 λ 0 0 ]
λ T
S ( n) 的协方差为 :
ξ ( t) + n1 i ( t) = ri [ X ( t) , t ] +ξ ( t) + n1 i ( t)
( 4)
( x si ( t) , y si ( t) , z si ( t) ) 为 t 时刻卫星的坐标 , ai 满
03 ×3
2 λ σ 2 ¨ y Q¨ y ( n)
1 0
(1 - e
1
-λ ξT -λ ξT
e
1 0 0
U x ( n) U ( n) = U y ( n) U z ( n) Uξ ( n) U x ( n) = U y ( n) = U z ( n) =
Φ1ξ = Φ ξ=
) /λ ξ
, Uξ ( n) =
e
1. 2 量测模型的建立
观察 m 颗卫星的伪距和多普勒频率后 , 可 ρ 得观测矩阵 Z = [ρ 1 , …, m , a1 , …, am ] , 伪距的计 算公式为[ 2 - 3 ] : ρ i ( t) =
2 2
对式 ( 1) 进行离散化处理得到 :
X ( n + 1 ) = Φ( n + 1 , n) X ( n) + U ( n) a + S ( n) ( 2)
2
( n + 1 ) 的一步估计 X 把X ¨ ¨ [ ( n + 1)
-
] 作为
式中 : Φs Φ( n + 1 , n) =
03 ×3 03 ×3 02 ×3 03 ×3 03 ×3 03 ×3 03 ×2 03 ×2 03 ×2
n1 ( t) n2 ( t)
+
( 6)
= h ( X ( t) , t) + n ( t)
对其进行离散化可得 :
H ( n) = h x1 h xm
9h [ X ( t) , t ] 9X ( t)
0 h y1 0
X( t) = X( n) , t = t n
= 0 1 0 1 0 0
0 0
0 hz1 0 0 hzm 0 0
( x si ( t) - x ( t) ) 2 + ( y si ( t) - y ( t) ) 2 + ( z si ( t) - z ( t) ) 2 +
λ
1
(- T+
λT2 1 - e - λT ) + λ 2
1 - eλ T
T-
λ
1 - eQ ( n) =
2 λ σ 2 ¨ x Q¨ x ( n)
11 . 984 4 . 762
8 . 314 3 . 394
的观测方差 。
1. 3 卡尔曼滤波方程的建立
可以发现 , 采用动态滤波算法进行定位时 , 在 x、 y 方向的定位精度均比最小二乘法单点定 σ 位精度高 , 采用动态滤波算法其位置误差的 1 值为 6 . 147 , 采用最小二乘法的单点定位 , 其位 σ值为 15 . 172 , 动态滤波法的定位精 置误差的 1 度提高了 59. 5 % 。
03 ×3 03 ×3 02 ×3
03 ×3 02 ×3
02 ×3
其中 hxj , h yj , hzj 满足 :
第2期
周桃云等 : GPS 单点定位动态滤波算法研究
・2 9 ・
h xj = h yj =
x u - x sj ^ rj y u - x sj ^ rj
了仿真 。
2. 1 定位位置误差分析
… … … … … … … … …… …
0 h ym 0 0 0
h x1 0 h y1 0 hz1 0 0 - 1 ( 7)
对此 , x 、 y 方向定位误差进行统计获得其均 方差如表 1 所示 。
表 1 x , y 方向动态滤波定位误差 方法 最小二乘法 动态卡尔曼滤波法 样本标准差σ /m
x 方向 y 方向
n T 时刻的当前加速度 , 则可得自适应均值的状
态向量一步转移公式为 :
X ( n + 1 ) = Φ1 ( n + 1 , n) X ( n) ( 3)
Φy
03 ×3 02 ×3
-λ T
Φz
02 ×3
2
其中 : Φ1 x Φ1 ( n + 1 , n) =
03 ×3 03 ×3 02 ×3 1 03 ×3 03 ×3 03 ×3 03 ×2 03 ×2 03 ×2
1 GPS 动态滤波数学模型 0 引言
全球定位系统 ( GPS) 是新一代的精密卫星 导航定位系统 。但由于 GPS 定位包含许多误差 源 ,尤其是测量随机误差和卫星的几何位置误 差 ,使定位精度受到影响 。利用传统的最小二乘 定位算法已不能满足系统性能要求 。消除 GPS 定位随机误差的重要方法是 GPS 动态滤波 , 即 利用特定的滤波方法消除各种随机误差 , 从而提 高 GPS 导航定位精度 。文中采用卡尔曼滤波技 术来 提 高 动 态 GPS 导 航 定 位 精 度 和 动 态 性 能[ 1 - 2 ] 。
Φ ξ
Φx = Φy = Φz =
1 0 0
T ( - 1 +λT + e (1 - e
-λ T -λ T -λ ξT -λ ξT
Φ1 y
03 ×3 02 ×3
T
2
) /λ
Φ1 z
02 ×3
T /2 T
1 0
) /λ ) /λ ξ
Φ1ξ
e
Φ ξ=
1 0
(1 - e
Φ1 x = Φ1 y = Φ1 z =
・28 ・
Fx F=
弹箭与制导学报
第 29 卷
04 ×4
Fy
04 ×4 04 ×4
Fz
04 ×4 04 ×4 04 ×4
Ft
2 2 2 σ σ 其中σ y、 z 方向分量的方 ¨ x 、 ¨ y 、 ¨ z 为加速度在 x 、
04 ×4 04 ×4 04 ×4
差。
ξ ( n) 近 经化简 , 可得 Q¨ x ( n) , Q¨ y ( n) , Q¨ z ( n) , Q
8
3
6
2
2
T
s ( t) = [ 0 0 s x ( t) 0 0 s y ( t) 0 0 s z ( t) 0 s ξ ( t) ]
其中 : λ σ λ σ D ( s x ( t) ) = 2 ¨ x ( t) , D ( s y ( t) ) = 2 ¨ y ( t)
2 2
6 3
2
2 2
T
λ σ λ σ ξ ( t) ) = z ( t) , D ( s t ξ
箭与制导学报第29方向测速误差方法样本标准差方向最小二乘法629动态卡尔曼滤波法767仿真结果发现采用动态滤波算法的测速精度比最小二乘法高其速度误差的而采用最小二乘法时其位置误差的结论采用动态卡尔曼滤波技术主要研究了航空领域的高动态飞行体
第 29 卷 第2期 2009 年 04 月
弹 箭 与 制 导 学 报 Jo urnal of Projectiles , Rocket s , Missiles and Guidance
Research on Dynamic Filtering Algorithm of GPS Singal Positioning
ZHOU Taoyun1 , ZHAN G Yi2 , WU Tiebin1 ( 1 Hunan Instit ute of Humanities Science and Technology , Hunan Lo udi 417000 ,China ; 2 School of Elect ro nics and Info rmation , Nort hwestern Polytechnical University , Xi’ an 710072 ,China) Abstract :Applying Kalman optimal estimation t heo ry to GPS navigatio n and po sitioning system , it has designed a po si2 tio ning algorit hm which bases on Kalman optimal estimation t heory for high2dynamic users. The Kalman filtering tech2 nology st udied here is mainly used in high2dynamic flight of aviation system. Finally , it has simulated t he po sitioning system which adopt s least squares algorit hm and Kalman filtering algo rit hm individually , and has co mpared t heir po si2 tio ning error and speeding erro r. The simulaition result s show t hat dynamic Kalman filtering algorit hm has a better fil2 tering accuracy and t heir po sitio ning accuracy has been markedly imp roved. Keywords : GPS ; least2squares algo rit hm ; Kalman filtering algo rit hm ; high dynamic
X ( t) = F ( t) X ( t) + U ( t) a ( t) + s ( t) ( 1)
式中 :
3 收稿日期 :2008 - 06 - 05 基金项目 : 湖南省自然科学联合基金 (07JJ 6105) 资助 作者简介 : 周桃云 ( 1981 - ) ,女 ,湖南娄底人 ,讲师 ,硕士 ,研究方向 : 通信 、 导航 、 制导与控制 。
1. 1 系统模型的建立
取载体状态矢量[ 1 - 4 ] :
X= [ x x ¨ x y y ¨ y z z ¨ z ξξ]
T
( x , y , z ) 为载体在 W GS284 坐 标系 中的 位置 , ( x , y , z) 、 (¨ x,¨ y,¨ z ) 分别为载体的速度分量和加
ξ为接收机时间偏差 Δt 引起的伪距 速度分量 。 偏差 ,ξ为时钟漂移引起的伪距偏差变化率 。动 态模型采用当前统计模型 , 时钟漂移用一阶马尔 柯夫过程描述 。