KALMAN滤波在GPS导航定位中的应用.doc

GPS导航定位中Kalman算法的应用
摘要：GPS是美国从本世纪70年代开始研制，经过20年,耗资200亿美元，在1994年全面建成，具有在海陆空进行全方位实时三维导航与定位能力的新一代卫星导航与定位系统。GPS技术包括很多优势，其中最吸引人的地方就是它的全球性、全能性、全天侯性，不仅使得它能为用户随时随地提供定位信息，而且使它可以广泛应用于全球各个军事部门和民用等诸多领域，但是因为GPS定位包含多种误差源，这就使其定位精度受到了一定的影响。
本文以介绍全球定位系统为前提，首先简单的介绍GPS，其次在利用GPS定位测伪距和已知卫星坐标经过绝对位置解算的前提下，为了改善和提高定位算法的精度，也为了GPS定位技术的应用更加广泛，从而提出Kalman滤波算法。
综上所述，本文的研究工作对导航定位系统中的传统滤波方法的改进有一定的参考，并对Kalman滤波算法在改善GPS动态导航的动态性能和提高定位精度方面的应用具有一定的实用价值。
关键词: GPS;伪距测量;卡尔曼滤波

Abstract
GPS is developed since 70 s of this century in the United States, after 20 years, at a cost of $20 billion, completed in 1994, has in carring all-round real-time three-dimensional navigation and positioning capability of a new generation of satellite navigation and positioning system.GPS technology including many advantages, one of the most attractive place is its global, totipotency, 24 hours a day, not only make it for the user to provide location information anytime and anywhere, and makes it can be widely used in many fields such as military and civilian, but because the GPS contains a variety of error sources, which makes its positioning accuracy is influenced by a certain.
Based on global positioning system is introduced in this paper, first of all, a simple introduction to GPS, second is using GPS pseudorange measurement and known satellite coordinates through the absolute position under the premise of solving, in order to improve and improve the accuracy of localization algorithm, but also to the application of GPS technology is more extensive, and Kalman filter algorithm is put forward.
To sum up, in this paper, the research work of navigation and positioning system of traditional filtering method improvement has a certain reference, and the Kalman filtering algorithm in improving the dynamic performance and improve the positioning precision GPS navigation application has a certain practical value.
Keywords：GPS; recursive least squares; kalman filter.

目录
摘要 II
Abstract II
1绪论 2
1.1 概述 2
1.2 研究背景 2
1.3 本文研究思路与工作安排 2
2 GPS导航定位系统的基本原理 3
2.1 GPS导航系统的组成及误差组成 2
2.2 GPS导航定位原理 4
2.3 kalman滤波理论 5
3 kalman滤波算法的matlab验证仿真 2
3.1.一步预测 6
3.1.1算法设计及验证 6
3.1.2算法仿真及结果分析 8
3.2 多步预测 11
4 kalman滤波算法的c语言算法程序 13
4.1.一步预测 13
4.1.1算法设计及验证 13
4.1.2算法测试及验证 14
4.1.3算法仿真及结果分析 15
4.2 多步预测 16
4.2.1.算法流程图及设计 16
4.2.2.算法仿真及结果分析 17
总结与展望 19
致 谢 19
参考文献 2

1绪论
1.1概述
GPS 计划始于1973 年，已于1994 年进入完全运行状态。利用该系统，用户不仅可以在全球范围内实现全天候、连续、实时的三维导航定位和测速，还能够进行高精度的时间传递和高精度的精密定位。GPS 利用已知四颗卫星的空间坐标位置，以及接收到的用户与卫星之间的伪距进行绝对位置解算。GPS整个系统由空间卫星星座、地面监控系统、用户接收设备组成。其中的空间部分由21颗卫星和3颗备用卫星组成。这样的空间分布使得在地球上任何地点、任何时刻接收机都能至少观测到4颗卫星[1]。随着目前科学技术的发展， GPS的应用已经渗透在人们的日常生活中，可见怎样更好的利用GPS已成为当下我国科技工作的一个刻不容缓的任务。
近些年随着目前科学技术的发展，GPS导航定位的应用在我国得到了广泛的推广并渗透在人们的日常生活中。但是由于GPS导航定位在精度上尚且存在提高的空间，因此，怎样进一步的改善GPS导航定位的动态性能和精度，将成为当下研究的一个重点。
1.2研究背景
二十世纪以后，随着人类科学技术水平的提高和各种探险活动的频繁，人们已不再满足于GPS的粗略定位，而是在精确定位、精确导航、建立高精度的全国性的大地测量控制网、建立陆地海洋大地测量基准、监测地球板块运动状态和地壳形变、测定航空航天摄影瞬间的相机位置等多方面对GPS都提出了更高的要求。
GPS定位系统是目前一套不仅稳定性好而且精度高的卫星定位系统，它除了能给用户提供运动中的物体的速度信息还有提供空间位置坐标。GPS采用的是测伪距定位原理，在接收机观测到的空间卫星数目至少达到4颗时，利用导航的定位算法实现绝对位置解算。
方案一：最小二乘算法
在日常生活中，由于惯性的存在，用户接收机位置无法突变，相邻时刻的位置必然存在一定的联系。最小二乘算法虽能够实现GPS定位，但其却没有将相邻的不同时刻的用户位置与之联系起来，这就使得最小二乘法存在着无法避免的不足，定位精度有待提高。因此将最小二乘算法与卡尔曼滤波算法相比较，前者的定位结果较粗糙，定位精度存在一定的不足，故在工程实际应用中提倡采用卡尔曼滤波算法。
方案二：卡尔曼滤波算法
卡尔曼滤波技术最小方差估计就是让系统最初的真值减去经过滤波后的值，再对这个差值求方差，要让这个方差达到最小。它只需利用前一时刻的状态最优估计值以及当前时刻获得的最新观测量便能求出当前时刻的状态最优估计值。卡尔曼滤波实质上就是系统协方差的递归，它的这种特点使它在没法存储大量数据的系统中得到了广泛的应用。
综上所述，研究GPS中的卡尔曼滤波定位算法有其不容忽视的实用价值。
1.3本文研究思路与工作安排
本论文的所做工作先是简单的介绍了GPS的研究背景、组成部分、基本定位原理。然后与最小二乘法相比提出了更高精度的定位算法——卡尔曼滤波算法，并对其进行了简单的仿真。
本论文的工作安排如下：
第一章：首先介绍了GPS的目前状况、研究背景以及意义。
第二章：介绍了GPS导航定位系统的基础知识和卡尔曼滤波理论。
第三章：讲述了卡尔曼滤波基于matlab的实验仿真。
第四章：介绍了卡尔曼滤波基于visual C的实验仿真。
第五章：总结与展望，总结了本文所做的主要工作，并且具体分析了所做工作的不足之处。

2 GPS导航定位系统的基本原理
2.1 GPS导航系统的组成及误差组成
GPS导航系统有三个组成部分：空间卫星星座、地面监控系统、用户接收设备。其中空间卫星星座是由位于6个轨道平面且轨道倾角为55度的24颗卫星组成。当GPS进行定位时，接收机至少需要观测到4颗GPS卫星，联立方程求解出待测点的空间三维坐标[2]。这部分不仅可以提供卫星星历、时间信息和其他有用的辅助信息还可以发射测量伪距的信号与载表信号。一个主控站、三个注入站和五个监测站构成了GPS的地面监测系统。它是中心控制系统可以实现时间同步跟踪卫星进行定轨。用户接收机主要负责捕获待测卫星的信号，接收和处理卫星信号并对处理后的数据进行记录，另外的重要作用是为用户提供了精确的定位信息。接收机配有石英钟，比原子钟的精度低很多，当接收到GPS信号时，接收机要复制C/A码和P码，利用补偿技术提取信息。接收机构造类型决定了解码的可能性，因此就决定了定位测量的精度。
GPS导航定位系统的组成分为三部分，由于星座的钟差、传播过程的损失、接收机的钟差等一些原因致使每一部分都存在着一定的无法避免的误差，误主要包括：星历误差、传播误差和接受误差[3]。
图2-1 导航定位误差组成
2.2 GPS导航定位原理
GPS系统的基本定位原理是：通过记录卫星发射的测距码信号传送到到用户接收机的时间乘以光速来算出伪距，再利用卫星星历查出已知卫星在确定的某时刻的具体位置坐标，最后根据以上所获得得数据来计算出用户接收机的具体位置坐标。因为空间卫星星载的时钟和用户接受机使用的时钟做到永远同步几乎是不可能的，一定存在某个时刻两者的时钟不同步，故除了用户接收机的三维空间坐标的x、y、z外，还需要增加一个未知数，即就是已知的空间卫星和用户接收机之间的钟差，用Δt表示。从以上所述可知，为了能够求出上面所提到的四个未知数那么就需要建立四个方程，所以用户接收机观测到的卫星数目至少为四颗，接着接收来自这四颗卫星发送的信号，才可确定出用户接收机的具体空间位置。如下图所示，假设t时刻用户接收机测定出GPS信号到达接收机的时间△t，接着利用接收机所接收到的来自卫星的信号查出卫星星历等其它数据就可以可以确定以下四个方程式[4]。

S1 (x1,y1,z1) S2(x2,y2,z2) S3(x3,y3,z3)
S4(x4,y4,z4)

图2-2 GPS定位原理图
（2-1）
综上所述的方程组有四个未知量，分别为，其中为为待测点的空间三围坐标，为已知的四颗卫星在t时刻的空间三维坐标，为星座卫星到用户接收机之间的距离，为各已知卫星信号到达用户接收机所经历的时间，c为光速，为接收机的钟差，是未知的。为卫星的钟差，是已知的[5]。将上述参数代入四个方程中，便可求解出其中的，即就是待测点的具体空间位置和接收机钟差。
2.3 kalman滤波理论
Kalman滤波就是将上一时刻的最优估计与最新的量测序列合并后，计算出当前时刻的最优估计。适宜应用在处理多变、时变和非平稳系统的随机过程。假设线性系统的离散模型为：
（2-2）
（2-3）
Kalman滤波的时间更新方程如下：
（2-4）
（2-5）
量测更新方程如下：
（2-6）
（2-7）
（2-8）
在时刻系统的初值分别为系统状态和系统状态的协方差。Kalman滤波过程主要体现在式（2-4）～（2-8）。主要分为两个过程：时间更新阶段和两侧更新阶段[6]。时间更新阶段利用初值计算系统状态及其协方差的先验估计值，修正阶段就是利用前面求得的先验估计值结合最新获得的量测值来计算后验估计和滤波增益。式（2-8）另一种常见的形式为：
（2-9）
可将计算的公式展开为如下形式：
（2-10）

3 kalman滤波算法基于matlab的验证仿真
3.1一步预测
3.1.1算法设计及验证
（1）核心算法：
图3-1 matlab程序
（2）系统真实值给定连续的固定量，且没有噪声时，根据测量值来使用卡尔曼滤波算法来对系统的真实值做最优估计。由于没有噪声则经过滤波器的最优估计和真实值一样。

图3-2 matlab程序输出结果

图3-3 matlab程序真值和滤波后的输出结果
（3）系统真实值给定连续的固定量，且有噪声时，根据测量值来使用卡尔曼滤波算法来对系统的真实值做最优估计。输出的最优估计如下图所示：

图3-5 matlab程序输出结果
估计误差的方差为0.7288，量测误差的方差为3.1225

（4）系统真实值给定连续的随机值，且有噪声时，根据测量值来使用卡尔曼滤波算法来对系统的真实值做最优估计。输出的最优估计如下图所示:

图3-6 matlab程序输出结果
估计误差的方差输出结果为0.0102，量测误差的方差为0.0363
3.1.2算法仿真及结果分析
为了测试kalman filter algorithm，我设计了一个简单的实验，来验证kalman filter的正确性。程序中给定系统真实值的初值，然后给定一组测量值，验证kalman filter估值的准确性。根据kalman filter algorithm，我们需要给定系统初始值，系统噪声Q(程序中用Rww表示)和测量噪声R（程序中用Rvv表示）（Q和R分别是系统过程噪声和量测噪声的方差）以及初始值所对应的协方差P2（1）。为了验证优劣性，还需要用给定的真实值和量测序列来计算kalman filter误差，以及测量误差。
（1）实验中给定连续的真实值，噪声一定且x_bef(1)=0,P_now(1)=0.02可以通过适当改变Rvv和Rww来获得更好的估计结果。
1）Rvv=0.056,Rww=0.1

图3-7为Rvv=0.056,Rww=0.1的输出结果
估计误差的方差输出结果为0.0153，量测误差的方差为0.2274
2）Rvv=0.072,Rww=0.1

图3-8 为Rvv=0.072，Rww=0.1的输出结果
估计误差的方差输出结果为0.0175，量测误差的方差为0.2274
3）Rvv=0.072,Rww=0.8

图3-9为Rvv=0.072 Rww=0.8的输出结果
估计误差的方差输出结果为0.0053，量测误差的方差为0.2274
综上所述，当有x_bef（1）=0，P_now（1）=0.02，改变Rvv和Rww
估计误差方差和量测误差方差有不同的结果：

Rvv=0.056
Rww=0.1
Rvv=0.072
Rww=0.1
Rvv=0.072
Rww=0.8
估计误差方差
0.0153
0.0175
0.0053
量测误差方差
0.2274
0.2268
0.2271
表一 改变Rvv和Rww
（2）实验中确定系统真实值不变，Rww和Rvv不变，改变P_now（1）的值，由于kalman filter是对协方差的递归算法来估计信号数据的，所以P2(1)对算法结果的影响较大。
1）P2(1)=10

图3-10改变P2(1)=10的输出结果
估计误差的方差输出结果为0.0154，量测误差的方差为 0.2275
综上所述，当有x_bef（1）=0，Rvv=0.056,Rww=0.1,改变P_now（1）的值，估计误差方差和量测误差方差有不同的结果：

P_now（1）=0.02
P_now（1）=10
估计误差方差
0.0153
0.0154
量测误差方差
0.2274
0.2275
表二 改变P_now（1）
3.2多步预测算法设计（为了编程简单，选取F=1，H=1）
（1）系统真实值x_real=0.82，且有噪声时(Rww=0.018,Rvv=0.542)，根据测量值来使用kalman filter来对系统的真实值做最优估计。输出的最优估计如下图所示：
量测输出：

滤波输出：

累积卡尔曼误差为0.0776 ，累积测量误差为0.1284
（2）系统真实值x_real=0.82，且有噪声时(Rww=0.021,Rvv=0.63)，根据测量值来使用kalman filter来对系统的真实值做最优估计。输出的最优估计如下图所示：
量测输出：

滤波输出： 累积卡尔曼误差为0.0769 ，累积测量误差为0.1272
综上所述x_real=0.82，且有噪声时改变Rww和Rvv的值，累积误差的变化：

x_real=0.82
Rvv=0.542
Rww=0.018
Rvv=0.63
Rww=0.021
累积卡尔曼误差
0.0776
0.0769
累积量测误差
0.1284
0.1272
表三 改变Rvv和Rww

4 kalman滤波算法基于visual c的验证仿真
4.1一步预测
4.1.1算法流程图及设计
算法流程图：

系统状态和状态协方差的初始化
X_now[0], P_now[0],H,F

时间更新阶段

量测更新阶段

核心算法设计：

4.1.2算法测试和验证
（1）系统真实值给定连续的固定量且没有噪声时，根据测量值来使用kalman filter来对系统的真实值做最优估计。由于没有噪声则经过滤波器的最优估计和真实值一样。

（2）系统真实值给定连续的固定量，且有噪声一定时，根据测量值来使用kalman filter来对系统的真实值做最优估计。输出的最优估计如下图所示：

（3）系统真实值给定连续的随机值，且有噪声时，根据测量值来使用kalman filter来对系统的真实值做最优估计。输出的最优估计如下图所示:

4.1.3算法仿真和结果分析
系统真实值给定连续的固定量，且有噪声一定时，根据测量值来使用kalman filter来对系统的真实值做最优估计。
（1）Rvv=0.32,Rww=0.892

（2）Rvv=0.036,Rww=0.74

综上所述：输入一定，噪声一定时，改变Rww和Rvv的值，累积误差的变化：

Rvv=0.32
Rww=0.892
Rvv=0.036
Rww=0.74
累积卡尔曼误差
0.3447
0.8526
累积量测误差
1.2632
1.8000
表四 改变Rvv和Rww
4.2多步预测算法设计
4.2.1算法流程图及设计
算法流程图：
系统状态和状态协方差的初始化
X_now[0], P_now[0],H,F

时间更新阶段

更新系统状态及其协方差
量测更新阶段

核心算法设计：

4.2.2算法仿真及结果分析
（1）Rvv=0.892,Rww=0.036

（2）Rvv=0.45,Rww=0.762

（3）Rvv=0.012,Rww=0.892

综上诉述：输入相同条件下，改变Rvv和Rww值，两种累积误差和卡尔曼误差所占比例：

Rvv=0.892
Rww=0.036
Rvv=0.45
Rww=0.762
Rvv=0.012
Rww=0.892
卡尔曼累积误差
0.198393
0.252586
0.309337
测量累积误差
0.312517
0.312517
0.312517
卡尔曼误差所占比例
37%
20%
2%
表五 改变Rvv和Rww

总结与展望
经过第四章的仿真结果与数据分析可以看出，当初始时刻的位置估计较准确时，kalman 的滤波性能就很好。考虑到时间、精力和能力水平等方面的因素，本论文中还有很多地方需要改进。GPS 系统已经在全球范围内获得了广泛的应用，其中不乏一些应用场合对它的定位精度提出了更高的要求，使用单一的某种定位技术目前已经很难满足特殊场合提出的高精度的需求，将卡尔曼滤波算法和其它算法相结合必然成为了今后 GPS系统滤波发展的趋势，因此值得我们对此进行深入探索。

参考文献
[1]刘基余， GPS卫星导航定位原理与方法［M］． 北京: 科学出版社，2003．
[2]刘延飞，郭锁利，卡尔曼滤波在GPS定位误差处理中的应用[A].电子科技杂志, 2011, 24( 9) ：140-148
[3]刘轩黄 ，最小二乘递推算法和Kalman滤波算法.华东交通大学学报，1998-06
[4]杨元喜,徐天河.基于移动幵窗法协方差估计和方差分量估计的自适应滤波[J].武汉大学学报(信息科学版).2003(06). 714-718.
[5]夏启军,孙优贤,周春晖.渐消卡尔曼滤波器的最佳自适应算法及其应用
[J],自动化学报.1990(03).210-216.
[6]张宗麟,惯性导航与组合导航.航空工业出版社,2000.8.
[7]刘豹,王正欧.系统辨识.北京:机械工业出版社,1993.6.
[8]郑建芬,基于高频注入和卡尔曼滤波永磁同步电机无传感器控制.
[9] Munir A，Atherton D.P. Adaptive interacting multiple model algorithm for tracking a maneuvering target [J]. IEE Proc.- Radar Sonar and Navigation，1995，142(1)：11–17.
[10] Yi Xu，Li Ping. Single Observer Bearings-Only Tracking with the unscented Kalman Filter [A]. Proceedings of ICCCAS’04[C]. Chengdu，China：ICCCAS，2004，2：901–905.