Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院....

7
Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution 1 ZHANG Hongping 1,2 , SHI Chuang 1,2 , LV Haixia 3 , HAN Wenhui 3 , LIU Jingnan 2 1. State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing, Wuhan University, Wuhan, China, 430070 2. RGNSS Research Center, Wuhan University, 129 Luoyu Road, Wuhan 430079, China 3. School of Geodesy and Geomatics, Wuhan University, Wuhan, China, 430079 e-mail: [email protected] Abstract: This paper focuses on the embedded algorithm design of GNSS Receiver’s solution for Positioning, Velocity and Time (PVT). Based on the principle and characteristics of the receiver’s baseband, the strategies of preprocessing and noise setting of the measurements output to PVT, according to the power of each channel’s tracking signal and the DLL/FLL/PLL correlation status, are described with some formula illustration. Sequential Least Square (LSQ) and Expanded Kalman Filter (EKF) are also introduced into the PVT Solution with some improvement aided by the possible priori-information from the receiver’s altitude, position and TCXO’s frequency variation in the first positioning while power on the receiver and in the continuous working environment. Robust estimation by outlier detection and down-weighting in LSQ and EKF, adaptive dynamic noise adjusting in EKF according to the monitored acceleration variation of the vehicles are also used in this solution. To avoid the huge computation burden in the embedded ARM CPU, the sequential EKF is adopted too. Finally, the flow chart of the whole PVT design is provided. Keywords: GPS receiver; PVT; sequential least square; Kalman filter GPS 接收机嵌入式导航定位解算设计 1 章红平 1,2 ,施 1,2 ,吕海霞 3 ,韩文慧 3 ,刘经南 2 1 武汉大学测绘遥感信息工程国家重点实验室,武汉 中国 430070 2 武汉大学卫星导航与定位技术研究中心, 武汉 中国 430079 3 武汉大学测绘学院, 武汉 中国 430079 e-mail: [email protected] : 本文详细介绍了接收机嵌入式导航定位(PVT)算法设计。结合 GNSS 接收机基带工作原理, 给出了基带输出到 PVT 观测数据的预检测,以及相应观测噪声设定的原理和公式;结合接收机首次定 位与连续定位的各类先验信息,给出了序贯最小二乘算法、扩展 Kalman 滤波算法的详细设计。这两 类算法设计充分考虑了接收机定位可能存在的高程约束、位置约束、先验钟差和钟飘约束等各类先验 信息;同时,结合参数估计可能存在的粗差、接收机运动存在的各类机动情况、嵌入式软件计算的 MIPS 特定要求,设计了具有抗差、自适应、序贯滤波的算法,并给出了 PVT 解算的流程图。 关键词: GPS 接收机;PVT;序贯最小二乘;Kalman 滤波 1 概述 GNSS 接收机导航定位综合软件不仅是接收机调 试、测试等分析的基础软件,还是接收机性能指标的 最后一道防线。目前,国外的一些知名 GPS 接收机品 牌,如 SiRFMTKGlobalLocate 等,均有几十人的 团队从事 GNSS 导航定位软件包的研发工作;国内的 GPS 接收机设计公司还处于起步阶段,许多研究机构、 公司的接收机雏形,如西安的华讯、上海的华龙等, 接收机的导航定位综合软件包只是满足了演示版的定 位功能,且大多是采用最小二乘算法,软件包的设计 还不全面,不能很好的满足高性能接收机性的要求, 是接收机定位精度不够、可靠性和稳定性差的一个主 要原因,也是国内接收机设计不能成功走向市场的重 要因素。 1 本论文的研究工作得到了 973 项目 2009CB724002、863 项目 2009AA12Z321、测绘遥感信息工程国家重点实验室专项科研经费资助. 嵌入式 GNSS 接收机导航定位软件包是一综合的 81 Global Navigation Satellite System: Technology Innovation and Application 978-1-935068-03-7 © 2009 SciRes.

Transcript of Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院....

Page 1: Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院. 牌,如. Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution

Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution1

ZHANG Hongping1,2, SHI Chuang1,2, LV Haixia3, HAN Wenhui3, LIU Jingnan2

1.State Key Laboratory of Information Engineering in Surveying, Mapping and Remote Sensing,

Wuhan University, Wuhan, China, 430070 2.RGNSS Research Center, Wuhan University, 129 Luoyu Road, Wuhan 430079, China

3.School of Geodesy and Geomatics, Wuhan University, Wuhan, China, 430079

e-mail: [email protected]

Abstract: This paper focuses on the embedded algorithm design of GNSS Receiver’s solution for Positioning, Velocity and Time (PVT). Based on the principle and characteristics of the receiver’s baseband, the strategies of preprocessing and noise setting of the measurements output to PVT, according to the power of each channel’s tracking signal and the DLL/FLL/PLL correlation status, are described with some formula illustration. Sequential Least Square (LSQ) and Expanded Kalman Filter (EKF) are also introduced into the PVT Solution with some improvement aided by the possible priori-information from the receiver’s altitude, position and TCXO’s frequency variation in the first positioning while power on the receiver and in the continuous working environment. Robust estimation by outlier detection and down-weighting in LSQ and EKF, adaptive dynamic noise adjusting in EKF according to the monitored acceleration variation of the vehicles are also used in this solution. To avoid the huge computation burden in the embedded ARM CPU, the sequential EKF is adopted too. Finally, the flow chart of the whole PVT design is provided.

Keywords: GPS receiver; PVT; sequential least square; Kalman filter

GPS 接收机嵌入式导航定位解算设计 1

章红平 1,2,施 闯 1,2,吕海霞 3,韩文慧 3,刘经南 2

1武汉大学测绘遥感信息工程国家重点实验室,武汉 中国 430070 2武汉大学卫星导航与定位技术研究中心, 武汉 中国 430079

3武汉大学测绘学院, 武汉 中国 430079

e-mail: [email protected]

摘 要: 本文详细介绍了接收机嵌入式导航定位(PVT)算法设计。结合 GNSS 接收机基带工作原理,

给出了基带输出到 PVT 观测数据的预检测,以及相应观测噪声设定的原理和公式;结合接收机首次定

位与连续定位的各类先验信息,给出了序贯最小二乘算法、扩展 Kalman 滤波算法的详细设计。这两

类算法设计充分考虑了接收机定位可能存在的高程约束、位置约束、先验钟差和钟飘约束等各类先验

信息;同时,结合参数估计可能存在的粗差、接收机运动存在的各类机动情况、嵌入式软件计算的

MIPS 特定要求,设计了具有抗差、自适应、序贯滤波的算法,并给出了 PVT 解算的流程图。

关键词: GPS 接收机;PVT;序贯最小二乘;Kalman 滤波

1 概述

GNSS 接收机导航定位综合软件不仅是接收机调

试、测试等分析的基础软件,还是接收机性能指标的

最后一道防线。目前,国外的一些知名 GPS 接收机品

牌,如 SiRF、MTK、GlobalLocate 等,均有几十人的

团队从事 GNSS 导航定位软件包的研发工作;国内的

GPS 接收机设计公司还处于起步阶段,许多研究机构、

公司的接收机雏形,如西安的华讯、上海的华龙等,

接收机的导航定位综合软件包只是满足了演示版的定

位功能,且大多是采用最小二乘算法,软件包的设计

还不全面,不能很好的满足高性能接收机性的要求,

是接收机定位精度不够、可靠性和稳定性差的一个主

要原因,也是国内接收机设计不能成功走向市场的重

要因素。 1本论文的研究工作得到了 973 项目 2009CB724002、863 项目2009AA12Z321、测绘遥感信息工程国家重点实验室专项科研经费资助. 嵌入式 GNSS 接收机导航定位软件包是一综合的

81

Global Navigation Satellite System: Technology Innovation and Application

978-1-935068-03-7 © 2009 SciRes.

Page 2: Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院. 牌,如. Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution

软件系统,涉及到数字信号处理参数控制、硬件模块

的配置与管理、导航定位算法等,其显著特点是直接

处理接收机的基带数据,向用户提供导航定位结果,

主要功能包括:数字基带管理、软件/硬件系统管理、

观测数据生成、导航定位解算。由于直接与接收机底

层数字基带、硬件模块进行数据交互,相对于常用的

接收机后端观测量定位处理模式而言,嵌入式 GNSS

导航定位解算的可用信息多,导航定位模式、计算过

程中的算法优化、质量控制等更具有优越性。为此,

研究各类基带数据信息、硬件参数信息对导航定位的

影响,区分它们的贡献,严格控制、弥补它们的不足,

是实现这一优越性的必然途径。

对于导航定位解算算法,国内以武汉大学 GNSS

工程技术研究中心、西安测绘研究所为代表的测绘行

业对 GNSS 精密定位技术进行了深入的研究,尤其是

高精度静态差分定位测量数据处理技术,动态导航定

位技术。通常,导航定位解算算法采用 Kalman 滤波。

Kalman 滤波算法自上世纪 60 年代诞生以来,在工程

应用领域得到了飞速的发展和广泛的应用。Kalman

滤波诞生 40 多年以来,许多学者针对它在诸多领域的

各类应用问题,在非线性问题、稳健估计、自适应估

计等做了深入的研究工作。近十多年来,Kalman 滤波

在 GNSS 导航定位中也得到了广泛的应用,国内外一

些学者的研究状况如下:

1.1 非线性滤波问题

GNSS 导航定位滤波解算是非线性问题,即滤波

的动力学模型所体现的运动状态的非线性特征和观测

模型的非线性。为了解决 GNSS 定位观测模型解算的

非线性问题,美国学者 Bancroft 于 1985 年提出了一种

称之为“闭合形求解”的全局非线性最小二乘算法

(Bancroft,1985;陶本藻,张勤,2003);基于此,张勤

提出了一种动态定位非线性代数解的Kalman滤波器,

利用前一历元的状态估值和当前历元的 Bancroft 解,

递推算出当前历元的状态估值(张勤,2002)。GNSS

用户载体运动动力学过程一般是非线性问题,通常采

用常速模型(CV 模型)或常加速度模型(CA 模型)

描述,采用扩展 Kalman 滤波解算。CV 模型或 CA 模

型实际上是载体运动过程函数的一阶或二阶逼近,而

更高阶次的逼近项被当作动态噪声,在扩展 Kalman

滤波方程中,它通过状态向量预报的协方差矩阵影响

GNSS 导航定位的精度和可靠性。针对运动过程的非

线性滤波问题,有文献已经证明采用泰勒级数展开至

二阶的非线性滤波并未能真正改善估值的偏差量(张

勤,2002)。Xu 研究并证明了扩展 Kalman 滤波和将

非线性模型展开至二次项的非线性滤波结果的精度和

偏差(Xu, 1999)。为了提高非线性模型滤波解的精度

和抗差性,也有学者提出利用半参数模型处理导航解

算中的非线性问题(王新洲,2002)。

1.2 抗差 Kalman 滤波

标准 Kalman 滤波解实为最小二乘滤波解或最小

方差滤波解,其假设前提是,观测向量及状态预测向

量均服从正态分布。实践中,观测向量、动态模型信

息均可能存在异常,此时若仍利用标准 Kalman 滤波,

则参数解算极不可靠。由贝叶斯估计原理可知,基于

不同的先验分布可推导出不同的状态参数解及不同的

状态参数后验分布。利用贝叶斯估计原理,根据不同

的先验分布可建立状态参数的抗差 M-LS 滤波、LS-M

滤波和 M-M 滤波(Yang, 1997; 周江文等,1997)。根

据动力学模型和观测模型可能出现的秩亏问题,也可

构建相应的秩亏抗差滤波模型(Koch and Yang,1998)。

抗差 Kalman 滤波涉及到等价权矩阵和等价协方差矩

阵的确定。如果观测量独立,则有多种抗差权函数可

供选择,如丹麦权函数(Kubik,1982)、IGGIII 权函数

(周江文,1989)、Huber 函数(Huber,1981)和 Tukey

的二次权函数(Hampel et al., 1986)等;如果观测量

相关,则可采用双因子等价权函数(Yang et al., 2002)。

1.3 自适应 Kalman 滤波

GNSS 接收机用户导航定位过程中,运动载体一

般具有非均匀加速度,很难用统一的动力学模型加以

描述,即使加入了 IMU 等 INS 传感器,惯性测量单

元也会存在观测误差,由此仍会引入动力学模型的观

测误差。这种动力学模型与运动载体实际运动轨迹之

间的差异会导致 Kalman 滤波结果发散。对此,许多

学者进行了深入的研究,提出了许多自适应 Kalman

滤 波 的 方 法 , 如 模 型 方 差 自 适 应 补 偿 法

(Jazwinski,1970)利用观测信息自适应地生成模型误

差的协方差矩阵,使预报残差与相应的统计量保持良

好的一致性;基于开窗逼近法的 Sage-Husa 滤波

( Sage-Husa, 1969; Mehra, 1970; Mohamed and

Schwarz, 1999; Wang et al., 1999)利用前 m 步残差序

列来估计观测向量的协方差阵和状态误差协方差阵;

渐消滤波(夏启军等,1999)或衰减记忆滤波法(张

金槐,蔡洪,1995)将前一个历元状态参数估值向量

的协方差矩阵膨胀,使其对状态参数估值的影响减小;

Yang et al. 提出了组合Sage滤波与抗差自适应滤波的

新方法(徐天河,杨元喜,2002),提出了基于方差分

量估计的抗差自适应滤波(Yang and Xu, 2003);还有

学者提出了对动力学模型信息各分量进行自适应调节

82

Global Navigation Satellite System: Technology Innovation and Application

978-1-935068-03-7 © 2009 SciRes.

Page 3: Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院. 牌,如. Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution

(欧吉坤等,2004;Ren et al., 2005)的方法,该方法

本质上与抗差 M-M 滤波的原理一致(杨元喜,1997)。

为了满足各类 GNSS 接收机对嵌入式导航定位软

件包稳定性、可靠性的需求,导航定位数据处理算法

必须稳健。为此,基于贝叶斯估计的 Kalman 滤波原

理,研究其抗差估计、自适应滤波、随机过程的扩展

滤波等数据处理方法备受关注。

1.4 接收机自主完备性监测技术

自主完备性监测技术是接收机导航定位软件的

重要组成部分,是软件稳定性、可靠性的最后一道防

线。自主完备性监测的目的是当接收机(或导航)系

统出现故障观测而使定位误差超过用户允许限值时,

接收机(或系统)能在规定的时间内通知用户(陈金

平等,2005)。接收机自主完备性监测的理论基础是数

理统计的假设检验,即是数据处理的粗差探测、定位

理论,算法上可以结合最小二乘(如“基于漏检概率的

RAIM可用性分析方法”,陈金平等),也可以是Kalman

滤波(如“基于 INS 辅助的 GPS 接收机完好性监测算

法研究”,黄晓瑞等)。从数理统计的角度来看,自主

完备性监测技术的核心是对故障观测进行检测、定位,

并评估可能产生的定位精度损失,而故障观测的检测、

定位的必要条件是观测向量(包括虚拟观测量)维数

大于待估参数向量维数,且冗余度越大越好。在 GNSS

卫星导航定位过程中,可视卫星的数量有限,一般 6-7

颗。当两颗以上的卫星出现观测故障时,有效观测个

数降到 5 颗以下,很难识别故障观测。为此,自主完

备性监测技术一直以来是接收机导航定位的一个难

点,也是一项关键技术。

2 导航定位解算软件设计

2.1 数据准备及预处理模块

数据准备及预处理模块的主要是对基带输出的观

测数据作初步处理,剔除那些明显含有粗差的观测量;

对观测量中含有的各类系统误差进行模型改正;计算

出当前历元卫星的位置、速度。为 PVT 观测方程建立

与参数估计模块准备好比较干净的数据。主要功能如

下:

交互相关(Cross-Correlation)检测剔除

基于基带数据处理信息的错误观测量的初

步剔除

基于自相关函数多路径探测与剔除

卫星位置、速度、加速度、加速度变化,

时钟误差和时钟漂移的计算

观测量电离层延迟、对流层、钟差、相对

论效应改正

2.2 观测量方差确定

2.2.1 伪距观测量的方差

伪距观测量的方差(或观测噪声)与接收机基带

设计、信号传播误差有关,另外,在 GPS 定位中,还

需考虑将卫星位置误差、卫星钟差等效到观测的 LOS

方向上的误差。为此,GPS 定位用到的伪距方差计算

如下: 2

_22222

clksvorbitiontropbasebandepseudorang 2baseband 与基带对 C/A 码的采样频率、相关器处

理的相关间隔、DLL 环路设计有关。TCXO 的采样频

率一般为 16M,而中频数据的采样频率一般为 5M,

采样分辨率为 1/16chip,精度为 9 米左右,相关处理

的分辨率为 1/5chip,为 60 米左右,相关精度为 30 米

左右,DLL 环路滤波器对相关处理结果进行搜索匹

配,信号能量越高,匹配越精确,C/A 码测量精度与

采样分辨率越接近。 2trop 与对流层延迟模型的精度有关,常用的

Hopfield 模型精度在 80%左右,在 LOS 方向残余误差

一般不会超过 1 米。 2ion 与电离层延迟模型的精度有关,电离层延迟

模型参数在导航电文中发布给用户,是 8 参数的

Klobuchar 模型,改正精度一般在 60%左右。残余的

模型误差与时间又密切关系,在晚上,LOS 方向模型

残差一般在 2 米左右,在白天中午时间段会达到 5 米

-20 米,另外,与地理纬度有关。 2orbit 与卫星星历的精度有关,卫星星历精度一般

在 5-10 米左右。 2

_ clksv 与卫星钟差模型有关,导航电文发布的钟

差模型,精度一般在 3 米左右

2.2.2 多普勒的方差

多普勒观测量的误差方差与基带数字信号处理模

式(如 FFT 点数)、采样分辨率有密切关系,目前,

导航型接收机多普勒的观测精度一般为 0.2m/s。

2.3 序贯最小二乘

最小二乘算法一般用作接收机开机后初次定位解

算和 Kalman 滤波算法的初始化。对导航终端客户而

言,从开机到首次定位时间(TTFF)长短以及首次定

位精度的高低,是接收机性能优劣的一主要指标,最

小二乘算法的稳健性是这一指标的内在要求,接收机

丢失所有卫星信号一段时间后重新定位的精度好坏也

与最小二乘密切相关。充分利用各类已知或先验信息,

并用其来改造最小二乘,是提高 TTFF 性能和接收机

83

Global Navigation Satellite System: Technology Innovation and Application

978-1-935068-03-7 © 2009 SciRes.

Page 4: Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院. 牌,如. Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution

重新定位性能的必然途径。算法特点如下:

2.3.1 对观测量加权(伪距/多普勒)最小二乘

依据上述观测量的精度信息,对位置解算、速度

解算采用加权最小二乘算法。

2.3.2 对位置、钟差的序贯最小二乘

对位置、钟差的序贯约束的最小二乘,是利用假

定的位置与速度、钟差与钟飘的线性关系,依据上一

历元结果,预报当前时刻接收机的位置、钟差及其先

验方差协方差信息(考虑接收机速度变化、TCXO 稳

定性参数引起的动态噪声),并用作虚拟观测量,进行

位置、钟差的四参数解算。

2.3.3 对速度、钟飘的序贯最小二乘

与 2 类似,但预报按照定常系统进行,协方差信

息考虑动态噪声,然后进行速度、钟飘的四参数解算。

2.3.4 对高程约束的最小二乘

对高程约束的最小二乘一般适用于接收机开机初

次定位且只捕获到三颗卫星的情况,对 HDOP 较大时

的接收机初次/重新定位的情况也适用。高程约束的虚

拟观测量有两种类型,一是初始位置全然未知的情况,

可以依据地球平均半径,建立半径约束方程;另外,

初始位置和初始高程不准确时,可以依据初始位置和

高程,建立约束方程。将这两类约束方程作为虚拟观

测方程,引入一定的噪声,与观测方程一起求解接收

机位置和钟差。

2.3.5 对初始位置约束的最小二乘

初始位置约束的最小二乘一般用在接收机开机后

初次定位,或所有信号丢失一段时间后重新定位。是

依据 PND 用户记录的位置,或递推的位置,约束定位

参数解算。

2.3.6 对钟飘初始约束的最小二乘

接收机钟飘对应于前端 RF 接入的 TCXO 频率稳

定性,导航型 GPS 接收机芯片采用的 TCXO 一般是

0.5ppm-2ppm,其稳定性一般与温度变化相关,振动、

加速度对 TCXO 的影响对 PND 用户影响较小,但对

弹载用户影响较大。为此,对 PND 用户而言,钟飘先

验信息可以用作虚拟观测量,约束接收机速度的最小

二乘解算。

2.3.7 对观测量粗差降权/剔除的抗差最小二乘

依据最小二乘残差的 Chi-square 检验和 Baardar

粗差探测因子,考虑观测量方差大小,并依据接收机

基带的自相关函数输出的分布状况、信号能量强弱,

综合考虑信息/残差的分组分布情况,判断粗差与否,

对较大的残差进行剔除,对一定范围内的残差,进行

降权处理。

2.3.8 对多普勒粗差降权/剔除的抗差最小二乘

同 7。

2.4 Kalman 滤波

2.4.1 算法描述

Kalman 滤波基本公式(参见书《Kalman 滤波理

论及其在导航系统中的应用》,付梦印等):

11,1,ˆˆ

kkkkk XX

Tkkkkk

Tkkkkkkk QPP 1,11,1,11,1,

11,1, )( k

T

kkkk

T

kkkk RHPHHPK

]ˆ[ˆˆ1,1, kkkkkkkk XHZKXX

1,)( kkkki

k PHKIP

由于 GPS 定位观测是非线性问题,载体的运动轨

迹也具有非线性。为了更好的描述导航定位,实现连

续、光滑、高精度的运动轨迹估计,这里采用扩展

Kalman 滤波算法。扩展 Kalman 滤波导航定位的思想

是:将载体的运动展开成一阶(即位置、速度),认为

载体加速度以上的二阶模型残差是动态噪声,并将其

引入到接收机位置、速度估计的状态噪声中;利用伪

距、多普勒观测,形成观测方程,与载体运动的一阶

方程一起联合解算,对接收机的位置、速度进行更新。

Kalman 滤波需要估计的参数包括三维位置、三维

速度、钟差、钟飘,是 8 维状态向量。常规的伪距、

多普勒观测量对其更新计算中需要进行 8*8 维矩阵求

逆,为了有效的降低运算量,可以采用序贯的方式完

成观测量的更新运算。此时,只存在除法,没有了矩

阵求逆运算。Kalman 滤波序贯运算的公式如下(参见

书《卡尔曼滤波与组合导航原理》83 页,秦永元等): 111 )( i

k

Tik

ik

ik

Tik

ik

ik RHPHHPK

)ˆ(ˆˆ 11 ik

iki

ik

ik

ik XHZKXX

1)( ik

ik

ik

ik PHKIP

公式中, i 表示第 i 个观测量, 表示第 时刻。k kP 是状态向量的协方差阵, H 是观测量的设计矩阵,

对 于 单 观 测 量 而 言 , 它 是 行 向 量 。 因 此 ,

实际上是一个实数,对比批处理的设

计矩阵

ikR

Tik

ik HP 1i

kH

H ,则矩阵求逆变成了除法运算,由此计算量

随之下降。

由于载体运动被展开成一阶,由此截断的加速度

以上的高阶运动,变成了扩展 Kalman 滤波的模型误

差。另外,接收机的运动状态随时间变化较快,但在

84

Global Navigation Satellite System: Technology Innovation and Application

978-1-935068-03-7 © 2009 SciRes.

Page 5: Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院. 牌,如. Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution

短时间内,如 1 秒以内,可以认为是一个随机过程。

这里,我们把它当成高斯白噪声驱动的随机过程,即

随机游走或随机常数。只要确定了短时间内运动状态

的高阶变化的谱密度,即可在 Kalman 滤波更新的间

隔内,推导出动态噪声矩阵的大小。运动状态变化的

高阶运动,可以依据加速度的变化确定,也可利用

doppler 的信息更新推导。假定在更新间隔内,状态向

量的随机过程是随机游走,即:

)()( twtN , 是接收机的运动速度 )(tN

)(tw 是白噪声过程,且 )()]()([ tqwtwE w

为此,由此产生的接收机速度向量的动态噪声为:

tqdttwW w

t

tk

k

k

1

)(1

接收机位置向量的动态噪声为:

21 5.0)(

11tqdttwQ w

t

t

t

tk

k

k

k

k

该随机过程的实现在 Kalman 滤波预报函数中。

自适应 Kalman 滤波就是要如何确定 ,通常,我们

取它为加速度的平方。 wq

另外,由于多普勒观测量的精度较高,一般在 cm/s

以内(取决于 FLL 的数据处理间隔或 PLL 环路参数),

接收机 TCXO 的稳定性一般在 0.1~0.2m/s,而接收机

动态机动一般会达到米/秒/秒,因此多普勒观测量的

新息,主要包含了接收机动态新息。为此,我们近似

的认为,多普勒的新息的均方根为动态噪声,该动态

自适应近似处理在 Kalman 滤波动态噪声确定函数中

实现。为了更准确的描述动态机动噪声,我们取地方

坐标系,即 ENU 坐标系中,平面、高程方向上的机

动新息为加速度信息,然后转换到 ECEF 坐标系中。

Figure 1. PVT main process

图 1. PVT 主要工作流程图

当前结果的质量控制模块,确定解算结果的合理性,最后 NMEA 格式输出。

Kalman 滤波模块的流程设计主要考虑当前历元

状态参数与上一历元参数之间的约束关系,以及观测

量与参数解算的方差匹配关系,另外还要考虑 TCXO

的稳定性参数与位置参数、观测量误差之间的匹配关

系。算法实现上,Kalman 滤波还要考虑粗差探测、系

统误差检测与排除等问题。由于在消费类用户的使用

习惯中,Kalman 滤波系统方程不存在严密的动力学关

系,只是松散的预报、递推、约束关系,为此,动态

噪声的确定,具有随机性,需要依据当前观测信息、

位置信息之间的差异作近似调整。另外,Kalman 滤波

粗差探测具有比最小二乘更优越的条件,即位置、速

度、钟差、钟飘的近似已知,故先验预报确定的信息

中,可以比较直观的判断大的粗差,而对于中小型粗

差,则需要通过后验残差信息,作数理统计假设检验,

并适当调整观测量的权阵。图 3 给出了 Kalman 滤波

的流程框图。

2.4.2 Kalman 算法特点

上述 Kalman 滤波算法具有如下特点:

1.扩展 Kalman 滤波

2.序贯 Kalman 滤波

3.动态自适应 Kalman 滤波

4.对观测量粗差探测/降权/剔除的抗差滤波

2.5 程序流程设计

PVT 的主流程如图 1 所示。伪距、多普勒等观测

量、数据送入 PVT 后,先进行数据准备及预处理模块,

完成空余通道剔除、观测量系统误差改正、当前时刻

卫星位置/速度计算等工作;然后对当前接收机状态进

行检测,判断载体是否处于静止、正常车载运动、高

动态运动、行走、隧道模式等状态模式;针对当前载

体的运动状态,进行算法的切换、算法参数的设定等

工作,进入位置、速度、钟差、钟飘的参数算法解算

的 LSQ 或 Kalman 滤波模块。位置/速度获取后,进入

85

Global Navigation Satellite System: Technology Innovation and Application

978-1-935068-03-7 © 2009 SciRes.

Page 6: Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院. 牌,如. Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution

Figure 2. LSQ module process design Figure 3. Kalman module process design

图 2. LSQ 模块流程设计图 图 3. Kalman 模块流程设计图

3 小结

PVT 算法与软件是接收机基带工作流程中最

后一道关口,其可靠性、稳定性以及精度指标等是

接收机性能的直观表现。本文总结了 GNSS 接收机

嵌入式 PVT 算法、软件设计的相关工作,着重对最

小二乘、Kalman 滤波的工程改进,针对基带输出观

测量的粗差探测与接收机动态特性的自适应探讨

等,期望为接收机导航定位程序开发的工程实践提

供一些经验。

References (参考文献)

[1] Bancroft S. An algebraic solution of the GPS equations. IEEE Transactions on Aerospace and Electronic Systems, 1985, AES- 21: 56-59.

[2] Tao Benzao, Zhang Qin. Homotopy Least Squares Model of GPS Nonlinear Data Processing. Editorial Board of Geomatics and Information Science of Wuhan University, 2003, 28: 115-118.

陶本藻, 张勤. GPS 非线性数据处理的同伦最小二乘模型. 武汉大学学报: 信息科学版, 2003, 28: 115-118.

[3] 张勤. 非线性最小二乘理论及其在 GPS 定位中应用研究[D].武汉: 武汉大学, 2002.

[4] Peiliang Xu. Bias and accuracy of, and an alternative to, discrete nonlinear filters. Journal of Geodesy, 1999, 73: 35-46.

[5] 王新洲. 非线性模型参数估计理论与应用[M]. 武汉大学出版

社, 2002. [6] Yang Yuanxi. Estimaors of Covariance Matrix at Robust

Estimation. ZfV, 1997, 122(4): 166-174. [7] 周江文. 抗差最小二乘法[M]. 华中理工大学出版社, 1997. [8] K. R. Koch, Y. Yang. Robust Kalman filter for rank deficient

observation models. Journal of Geodesy, 1998, 72(7-8): 436-441. [9] Kubik. An error theory for the Danish Method[Z]. Accuracy

Aspects and quality control. ISP. Commission 3. Helsinki, 1982. [10] Zhou Jiangwen. Classical theory of errors and robustestimation.

Acta Geodaetica Et Cartographic Sinica, 1989, 18(4): 115-120. 周江文. 经典误差理论与抗差估计. 测绘学报, 1989, 18(4): 115-120.

[11] Huber P. Robust Statistics [M]. New York: Wiley, 1981. [12] Hampel F R, Ronchetti E M, et al. Robust Statistics[M]. New

York: Wiely, 1986. [13] Yang Y, Song L, Xu T. Robust Estimator for Correlated

86

Global Navigation Satellite System: Technology Innovation and Application

978-1-935068-03-7 © 2009 SciRes.

Page 7: Design of GPS Receiver Embedded Positioning and Navigation ... · 3. 武汉大学测绘学院. 牌,如. Design of GPS Receiver Embedded Positioning and Navigation Algorithm Solution

Observations Based on Bifactor Equivalent Weight. Journal of Geodesy, 2002, 76.

[14] Jazwinski A. Stochastic processes and filtering theory[M]. Academic Press, New York, 1970.

[15] Sage A P Husa G W. Adaptive Filtering with Unknown Prior Statistics. Joint American Control Conference, 1969, 769-774.

[16] Mehra R K. On the Identification of Variances and Adaptive Kalman Filtering. IEEE Transactions on Automatic Control. 1970.

[17] Mohamed A H, Schwarz K P. Adaptive Kalman filtering for INS/GPS. Journal of Geodesy, 1999.

[18] Wang J, Stewart MP. And Tsakiri M. Adaptive Kalman filtering for integration of GPS with GLONASS and INS.Presentation at the XXIIth International Union of Geodesy and Geophysics, Birmingham, UK, 19-30July, 1999.

[19] Xia Qijun, Sun Youxian, Zhou Chunhui. An optimal adaptive algorithm for fading kalman filter and its application. Acta Automatica Sinica, 1990, 16(3). 夏启军, 孙优贤, 周春晖. 渐消卡尔曼滤波器的最佳自适应

算法及其应用. 自动化学报, 1990, 16(3). [20] Cai Hong, Zhang Jinhuai. Adaptive Filtering Approach for

Missile Flight Test Trajectory Determination under Multiple Station Measuring. AEROSPACE CONTROL, 1997. 张金槐, 蔡洪. 导弹飞行试验多站测量定轨的自适应滤波方

法.航空控制, 1997. [21] Yang Y, Song L, Xu T. Robust Parameter Estimation for

Geodetic Correlated Observations. Acta Geodaetica Et Cartographic Sinica, 2002, 31(2). 杨元喜, 宋力杰, 徐天河. 大地测量相关观测抗差估计理论.测绘学报, 2002, 31(2).

[22] Yang YX, Xu TH. An adaptive Kalman filter based on Sage windowing weights and variance components. Journal of Navigation, 2003, 56(2): 231-240.

[23] 欧吉坤, 柴艳菊, 袁运斌. 自适应滤波[C]. 大地测量与地球

动力学进展会议. 武汉, 2004. [24] Ren Chao, Ou Jikun, Yuan Yunbin. Application of adaptive

filtering by selecting the parameter weight factor in precise Kinematic GPS position.PROCESS IN NATURAL SCIENCE. 2005, 15(1), 41-46(6).

[25] 杨元喜. 动态系统的抗差 Kalman 滤波. 测绘学院学报. 1997, 14(2).

[26] 陈金平, 周建华, 唐波. 基于检漏概率的RAIM可用性分析方

法. 测绘学院学报, 2005, 22(1). [27] Huang Xiaorui, Li Bo. Research on Autonomous Integrity

Monitoring Algorithm of GPS Receiver Based on INS. Telemetry & Telecontrol, 2003. 黄晓瑞, 李波. 基于 INS 辅助的 GPS 接收机完善性监测算法

研究. 遥测遥控, 2003.

87

Global Navigation Satellite System: Technology Innovation and Application

978-1-935068-03-7 © 2009 SciRes.