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

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 1 概述 GPS 计划始于 1973 年 已于 1994 年进入完全运行状态 利用该系统 用 户不仅可以在全球范围内实现全天候 连续 实时的三维导航定位和测速 还 能够进行高精度的时间传递和高精度的精密定位 GPS 利用已知四颗卫星的 空间坐标位置 以及接收到的用户与卫星之间的伪距进行绝对位置解算 GPS 整个系统由空间卫星星座 地面监控系统 用户接收设备组成 其中的空间部 分由 21 颗卫星和 3 颗备用卫星组成 这样的空间分布使得在地球上任何地点 任何时刻接收机都能至少观测到 4 颗卫星 1 随着目前科学技术的发展 GPS 的应用已经渗透在人们的日常生活中 可见怎样更好的利用 GPS 已成为 当下我国科技工作的一个刻不容缓的任务 近些年随着目前科学技术的发展 GPS 导航定位的应用在我国得到了广泛的 推广并渗透在人们的日常生活中 但是由于 GPS 导航定位在精度上尚且存在 提高的空间 因此 怎样进一步的改善 GPS 导航定位的动态性能和精度 将 成为当下研究的一个重点 1 2 研究背景 二十世纪以后 随着人类科学技术水平的提高和各种探险活动的频繁 人们 已不再满足于 GPS 的粗略定位 而是在精确定位 精确导航 建立高精度的 全国性的大地测量控制网 建立陆地海洋大地测量基准 监测地球板块运动状 态和地壳形变 测定航空航天摄影瞬间的相机位置等多方面对 GPS 都提出了 更高的要求 GPS 定位系统是目前一套不仅稳定性好而且精度高的卫星定位系统 它除了 能给用户提供运动中的物体的速度信息还有提供空间位置坐标 GPS 采用的是 测伪距定位原理 在接收机观测到的空间卫星数目至少达到 4 颗时 利用导航 的定位算法实现绝对位置解算 2 方案一 最小二乘算法 在日常生活中 由于惯性的存在 用户接收机位置无法突变 相邻时刻的位 置必然存在一定的联系 最小二乘算法虽能够实现 GPS 定位 但其却没有将 相邻的不同时刻的用户位置与之联系起来 这就使得最小二乘法存在着无法避 免的不足 定位精度有待提高 因此将最小二乘算法与卡尔曼滤波算法相比较 前者的定位结果较粗糙 定位精度存在一定的不足 故在工程实际应用中提倡 采用卡尔曼滤波算法 方案二 卡尔曼滤波算法 卡尔曼滤波技术最小方差估计就是让系统最初的真值减去经过滤波后的值 再对这个差值求方差 要让这个方差达到最小 它只需利用前一时刻的状态最 优估计值以及当前时刻获得的最新观测量便能求出当前时刻的状态最优估计值 卡尔曼滤波实质上就是系统协方差的递归 它的这种特点使它在没法存储大量 数据的系统中得到了广泛的应用 综上所述 研究 GPS 中的卡尔曼滤波定位算法有其不容忽视的实用价值 1 3 本文研究思路与工作安排 本论文的所做工作先是简单的介绍了 GPS 的研究背景 组成部分 基本定 位原理 然后与最小二乘法相比提出了更高精度的定位算法 卡尔曼滤波算 法 并对其进行了简单的仿真 本论文的工作安排如下 第一章 首先介绍了 GPS 的目前状况 研究背景以及意义 第二章 介绍了 GPS 导航定位系统的基础知识和卡尔曼滤波理论 第三章 讲述了卡尔曼滤波基于 matlab 的实验仿真 第四章 介绍了卡尔曼滤波基于 visual C 的实验仿真 3 第五章 总结与展望 总结了本文所做的主要工作 并且具体分析了所做工作 的不足之处 2 GPS 导航定位系统的基本原理 2 1 GPS 导航系统的组成及误差组成 GPS 导航系统有三个组成部分 空间卫星星座 地面监控系统 用户接收设 备 其中空间卫星星座是由位于 6 个轨道平面且轨道倾角为 55 度的 24 颗卫星 组成 当 GPS 进行定位时 接收机至少需要观测到 4 颗 GPS 卫星 联立方程 求解出待测点的空间三维坐标 2 这部分不仅可以提供卫星星历 时间信息和 其他有用的辅助信息还可以发射测量伪距的信号与载表信号 一个主控站 三 个注入站和五个监测站构成了 GPS 的地面监测系统 它是中心控制系统可以实 现时间同步跟踪卫星进行定轨 用户接收机主要负责捕获待测卫星的信号 接 收和处理卫星信号并对处理后的数据进行记录 另外的重要作用是为用户提供 了精确的定位信息 接收机配有石英钟 比原子钟的精度低很多 当接收到 GPS 信号时 接收机要复制 C A 码和 P 码 利用补偿技术提取信息 接收机构 造类型决定了解码的可能性 因此就决定了定位测量的精度 GPS 导航定位系统的组成分为三部分 由于星座的钟差 传播过程的损失 接收机的钟差等一些原因致使每一部分都存在着一定的无法避免的误差 误主 要包括 星历误差 传播误差和接受误差 3 4 图 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 5 图 2 2 GPS 定位原理图 2 1 综上所述的方程组有四个未知量 分别为 其中为为待测点的空间三围坐标 为已知的四颗卫星在 t 时刻的空间三维坐标 为星座卫星到用户接收机之间的 距离 为各已知卫星信号到达用户接收机所经历的时间 c 为光速 为接收机 的钟差 是未知的 为卫星的钟差 是已知的 5 将上述参数代入四个方程中 便可求解出其中的 即就是待测点的具体空间位置和接收机钟差 2 3 kalman滤波理论 Kalman滤波就是将上一时刻的最优估计与最新的量测序列合并后 计算出当 前时刻的最优估计 适宜应用在处理多变 时变和非平稳系统的随机过程 假 设线性系统的离散模型为 11kkkkxFxw 2 2 2 kkkk zHxv 6 3 Kalman 滤波的时间更新方程如下 2 1 11 kkkk k xFx 4 2 1 1 1 T kkkk kkk PFP FQ 5 量测更新方程如下 2 1 11 111 11 TT kkkkkkkkk kPHHPHR 6 2 1 11 1111 kkkkkkkkk xxKzHx 7 2 1 1111 kkkkkk PIKHP 8 在 时刻系统的初值分别为系统状态和系统状态的协方差 Kalman 滤波过程 k 主要体现在式 2 4 2 8 主要分为两个过程 时间更新阶段和两侧更新 阶段 6 时间更新阶段利用初值计算系统状态及其协方差的先验估计值 修正 阶段就是利用前面求得的先验估计值结合最新获得的量测值来计算后验估计和 滤波增益 式 2 8 另一种常见的形式为 2 1 11 111 T kkkkkkk PPKHK 9 可将计算的公式展开为如下形式 1 kk P 2 1 1 1 1 1 11 TTT kkkk kk kkkk kkkkk kkk PF PPHH PHRH PFQ 10 7 3 kalman 滤波算法基于 matlab 的验证仿真 3 1 一步预测 3 1 1 算法设计及验证 1 核心算法 图3 1 matlab程序 2 系统真实值给定连续的固定量 且没有噪声时 根据测量值来使用卡 尔曼滤波算法来对系统的真实值做最优估计 由于没有噪声则经过滤波器的 最优估计和真实值一样 8 图3 2 matlab程序输出结果 9 图3 3 matlab程序真值和滤波后的输出结果 3 系统真实值给定连续的固定量 且有噪声时 根据测量值来使用卡尔曼 滤波算法来对系统的真实值做最优估计 输出的最优估计如下图所示 图3 5 matlab程序输出结果 估计误差的方差为0 7288 量测误差的方差为3 1225 4 系统真实值给定连续的随机值 且有噪声时 根据测量值来使用卡尔曼 滤波算法来对系统的真实值做最优估计 输出的最优估计如下图所示 10 图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 11 图 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 12 图 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 01530 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 13 图 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 01530 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来对系统的真实值做最优估计 输出的最优估计如下图所 示 量测输出 14 滤波输出 累积卡尔曼误差为0 0776 累积测量误差为0 1284 2 系统真实值x real 0 82 且有噪声时 Rww 0 021 Rvv 0 63 根据测量 值来使用kalman filter来对系统的真实值做最优估计 输出的最优估计如下图 所示 量测输出 15 滤波输出 累积卡尔曼误差为0 0769 累积测量误差为0 1272 综上所述x real 0 82 且有噪声时改变Rww和Rvv的值 累积误差的变化 x real 0 82Rvv 0 542 Rww 0 018 Rvv 0 63 Rww 0 021 累积卡尔曼误差0 07760 0769 累积量测误差 0 1284 0 1272 表三 改变 Rvv 和 Rww 16 4 kalman 滤波算法基于 visual c 的验证仿真 4 1 一步预测 4 1 1 算法流程图及设计 算法流程图 系统状态和状态协方差的初始化 时间更新阶段 量测更新阶段 核心算法设计 X now 0 P now 0 H F 1 11 kkkk k xFx 1 1 1 T kkkk kkk PFP FQ 1 11 111 11 TT kkkkkkkkk kPHHPHR 1 11 1111 kkkkkkkkk xxKzHx 1 1111 kkkkkk PIKHP 17 4 1 2 算法测试和验证 1 系统真实值给定连续的固定量且没有噪声时 根据测量值来使用 kalman filter 来对系统的真实值做最优估计 由于没有噪声则经过滤波器的最 优估计和真实值一样 2 系统真实值给定连续的固定量 且有噪声一定时 根据测量值来使用 kalman filter来对系统的真实值做最优估计 输出的最优估计如下图所示 18 3 系统真实值给定连续的随机值 且有噪声时 根据测量值来使用 kalman filter来对系统的真实值做最优估计 输出的最优估计如下图所示 4 1 3 算法仿真和结果分析 系统真实值给定连续的固定量 且有噪声一定时 根据测量值来使用 kalman filter来对系统的真实值做最优估计 1 Rvv 0 32 Rww 0 892 19 2 Rvv 0 036 Rww 0 74 综上所述 输入一定 噪声一定时 改变Rww和Rvv的值 累积误差的变化 Rvv 0 32 Rww 0 892 Rvv 0 036 Rww 0 74 累积卡尔曼误差0 34470 8526 20 累积量测误差 1 2632 1 8000 表四 改变 Rvv 和 Rww 4 2 多步预测算法设计 4 2 1 算法流程图及设计 算法流程图 系统状态和状态协方差的初始化 时间更新阶段 量测更新阶段 X now 0 P now 0 H F 1 11 kkkk k xFx 1 1 1 T kkkk kkk PFP FQ 1 11 111 11 TT kkkkkkkkk kPHHPHR 1 11 1111 kkkkkkkkk xxKzHx 1 1111 kkkkkk PIKHP 1 11 kkkk k xFx 1 1 1 T kkkk kkk PFP FQ 更新系统状态及其协方差 21 核心算法设计 4 2 2算法仿真及结果分析 1 Rvv 0 892 Rww 0 036 2 Rvv 0 45 Rww 0 762 22 3 Rvv 0 012 Rww 0 892 23 综上诉述 输入相同条件下 改变Rvv和Rww值 两种累积误差和卡尔曼误 差所占比例 Rvv 0 892 Rww 0 036 Rvv 0 45 Rww 0 762 Rvv 0 012 Rww 0 892 卡尔曼累积误差0 1983930 252586 0 309337 测量累积误差0