CN109425876B - 一种提高定位精度的改进卡尔曼滤波方法 - Google Patents

一种提高定位精度的改进卡尔曼滤波方法 Download PDF

Info

Publication number
CN109425876B
CN109425876B CN201710724015.6A CN201710724015A CN109425876B CN 109425876 B CN109425876 B CN 109425876B CN 201710724015 A CN201710724015 A CN 201710724015A CN 109425876 B CN109425876 B CN 109425876B
Authority
CN
China
Prior art keywords
satellite
kalman filtering
acceleration
speed
measurement
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710724015.6A
Other languages
English (en)
Other versions
CN109425876A (zh
Inventor
洪诗聘
胡文涛
李建华
王勋
李峰
左启耀
张丛巨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN201710724015.6A priority Critical patent/CN109425876B/zh
Publication of CN109425876A publication Critical patent/CN109425876A/zh
Application granted granted Critical
Publication of CN109425876B publication Critical patent/CN109425876B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/13—Receivers
    • G01S19/24—Acquisition or tracking or demodulation of signals transmitted by the system
    • G01S19/30—Acquisition or tracking or demodulation of signals transmitted by the system code related

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明属于滤波方法，具体涉及一种提高定位精度的改进卡尔曼滤波方法。它包括：步骤一：卫星捕获跟踪与同步。步骤二：提取数据。步骤三：卫星位置速度计算。步骤四：最小二乘解算。步骤五：扩展卡尔曼滤波量测和状态方程建立。步骤六：扩展卡尔曼滤波过程。步骤七：自适应调整。本发明的有益效果是：在卫星导航接收机单点定位过程，若卫星信号瞬时恶化引起伪距观测量瞬时恶化，会导致最小二乘解算得到的单点定位测速精度跳变，利用此套方法可以有效地消除此情况下的定位测速精度跳变，保持定位稳定性。

Description

一种提高定位精度的改进卡尔曼滤波方法
技术领域
本发明属于滤波方法，具体涉及一种提高定位精度的改进卡尔曼滤波方法。
背景技术
卫星定位解算是指接收机将捕获到的卫星信号进行解调、调制得到导航电文，然后根据导航电文进行PVT解算的过程。经过几十年的发展，卫星定位解算技术已经日趋完善。接收机的定位解算通常采用的方法有直接解算法、牛顿迭代法、最小二乘法和EKF(扩展卡尔曼滤波)。
直接解算法是通过对各卫星的距离方程变形求差，从而消去未知数的二次项，将定位的非线性距离方程转化成线性方程，然后直接求解坐标，无需初始坐标和迭代计算，计算简单，实时性好。然而在直接解算算法中，每个方程均包含卫星位置误差和伪距误差，而且方程的噪声扰动也几乎是迭代算法的两倍，所以直接解算算法的定位精度没有迭代定位算法的精度高：最小二乘法是牛顿迭代法的改进形式，通过将观测方程在初值处按泰勒级数展开，保留一阶项，得到线性方程组进而求解未知数的变化量，然后循环迭代直至变化量的二范数很小，定位精度不如卡尔曼滤波。EKF是传统非线性估计的代表，其基本思想是围绕状态初值对非线性模型进行泰勒级数展开，从而将非线性系统线性化，然后利用线性卡尔曼滤波算法进行线性递推估计，其上一时刻的最优估计值和下一时刻的系统观测值共同决定了下一时刻的最优估计结果。该方法精度要高于直接解算法和最小二乘迭代法。但是应用扩展卡尔曼滤波器时，滤波初值的选择是关键。如果滤波初值偏差较大，或者动态模型不准确，就很容易造成滤波震荡时间长或者滤波过程发散，实时性解算较差，而且如果前后两次的滤波结果相差太大该如何解决，这些都是需要克服的问题。
发明内容
本发明的目的是针对现有技术的缺陷，提供一种提高定位精度的改进卡尔曼滤波方法。
本发明是这样实现的：一种提高定位精度的改进卡尔曼滤波方法，其中，包括
下述步骤：
步骤一：卫星捕获跟踪与同步，
依次捕获GPS、BD2、GLONASS卫星，跟踪卫星进行位同步和帧同步；
步骤二：提取数据，
提取帧同步后的卫星码NCO、码片计数、1ms计数、位计数、字计数、帧计数和多普勒，得到发射时刻、伪距和伪距率；
步骤三：卫星位置速度计算，
按照卫星导航接口控制文件(ICD)提供的卫星位置计算方法，输入卫星发射时刻计算得到卫星位置和速度；
步骤四：最小二乘解算，
通过求得的跟踪卫星伪距、位置和速度进行最小二乘计算，得到载体的位置和速度；
步骤五：扩展卡尔曼滤波量测和状态方程建立，
通过求得的跟踪卫星伪距和伪距率、位置和速度，以接收机钟差
Figure GDA0003812519410000031
为测量模型建立卡尔曼滤波量测方程，其中R为星地几何距离，ρ为伪距测量值，δts为卫星钟差，δ∑为大气层延迟及接收机测量误差的总误差，c为光速，钟漂测量模型通过对钟差求导得到，
以载体位置(Xt，Yt，Zt)、速度
Figure GDA0003812519410000032
加速度
Figure GDA0003812519410000033
时钟偏差量cδt、时钟偏差变化率
Figure GDA0003812519410000034
为未知数建立状态方程，考虑到高动态环境，卡尔曼滤波动态模型采用当前统计模型
Figure GDA0003812519410000035
式中
Figure GDA0003812519410000036
为机动加速度“当前”均值，在每个采样周期内为常数；a(t)为机动加速度；
Figure GDA0003812519410000037
为载体的加速度；τ为机动加速度变化率的相关时间常数；w(t)是零均值的高斯白噪声，钟差采用一阶马尔科夫随机模型，
步骤六：扩展卡尔曼滤波过程，
量测方程和状态方程建立后，依次经过求解状态一步转移预测值、计算估计误差协方差矩阵一步转移预测值、求解滤波增益矩阵、求解信息向量、更新估计误差协方差矩阵以及估计当前时刻的状态估计值的步骤，得到当前时刻估计载体位置(Xt，Yt，Zt)、速度
Figure GDA0003812519410000038
加速度
Figure GDA0003812519410000039
时钟偏差量cδt、时钟偏差变化率
Figure GDA00038125194100000310
步骤七：自适应调整
在卡尔曼滤波过程中，根据当前得到的加速度(ax、ay、az)大小实时计算加速度方差(σ2 x，σ2 y，σ2 z)，以此调整模型噪声协方差Q，达到滤波方差自适应，调整方法为：当“当前”加速度为正时，其方差为：
Figure GDA0003812519410000041
其中amax为可设的加速度上限，y、z方向的方差计算同x。
如上所述的一种提高定位精度的改进卡尔曼滤波方法，其中，在卡尔曼滤波过程中，实时检测卡尔曼过程是否发散，检测方法为：统计N点最小二乘解算结果，载体位置和速度，与卡尔曼滤波结果差值，如果该差值连续T次超过设定的误差门限，则认为卡尔曼滤波发散，退回步骤四并重启卡尔曼滤波器，重新进行卡尔曼滤波解算。
本发明的有益效果是：本申请将直接解算法、最小二乘迭代法和EKF融合。该方法第一步通过直接解算法得到接收机的概略坐标，将其作为最小二乘法的迭代初值。第二步通过最小二乘法求解EKF滤波初值，保证滤波前期不会出现震荡。第三步通过EKF迭代估计各个状态的变化量，最终得到接收机状态信息。更重要的是在滤波过程中，当接收机状态突然变化时，首先调整滤波增益系数；当EKF出现发散时，启用最小二乘解算程序，保证了滤波的有效进行。本申请与传统方法相比，提高了定位解算的精度和稳定性。
在卫星导航接收机单点定位过程，若卫星信号瞬时恶化引起伪距观测量瞬时恶化，会导致最小二乘解算得到的单点定位测速精度跳变，利用此套方法可以有效地消除此情况下的定位测速精度跳变，保持定位稳定性。
具体实施方式
一种提高定位精度的改进卡尔曼滤波方法，其特征在于，包括下述步骤：
步骤一：卫星捕获跟踪与同步，
依次捕获GPS、BD2、GLONASS卫星，跟踪卫星进行位同步和帧同步；
步骤二：提取数据，
提取帧同步后的卫星码NCO、码片计数、1ms计数、位计数、字计数、帧计数和多普勒，得到发射时刻、伪距和伪距率；
步骤三：卫星位置速度计算，
按照卫星导航接口控制文件(ICD)提供的卫星位置计算方法，输入卫星发射时刻计算得到卫星位置和速度；
步骤四：最小二乘解算，
通过求得的跟踪卫星伪距、位置和速度进行最小二乘计算，得到载体的位置和速度；
步骤五：扩展卡尔曼滤波量测和状态方程建立，
通过求得的跟踪卫星伪距和伪距率、位置和速度，以接收机钟差
Figure GDA0003812519410000051
为测量模型建立卡尔曼滤波量测方程，其中R为星地几何距离，ρ为伪距测量值，δts为卫星钟差，δ∑为大气层延迟及接收机测量误差的总误差，c为光速，钟漂测量模型通过对钟差求导得到，
以载体位置(Xt，Yt，Zt)、速度
Figure GDA0003812519410000061
加速度
Figure GDA0003812519410000062
时钟偏差量cδt、时钟偏差变化率
Figure GDA0003812519410000063
为未知数建立状态方程，考虑到高动态环境，卡尔曼滤波动态模型采用当前统计模型
Figure GDA0003812519410000064
式中
Figure GDA0003812519410000065
为机动加速度“当前”均值，在每个采样周期内为常数；a(t)为机动加速度；
Figure GDA0003812519410000066
为载体的加速度；τ为机动加速度变化率的相关时间常数；w(t)是零均值的高斯白噪声，钟差采用一阶马尔科夫随机模型，
步骤六：扩展卡尔曼滤波过程，
量测方程和状态方程建立后，依次经过求解状态一步转移预测值、计算估计误差协方差矩阵一步转移预测值、求解滤波增益矩阵、求解信息向量、更新估计误差协方差矩阵以及估计当前时刻的状态估计值的步骤，得到当前时刻估计载体位置(Xt，Yt，Zt)、速度
Figure GDA0003812519410000067
加速度
Figure GDA0003812519410000068
时钟偏差量cδt、时钟偏差变化率
Figure GDA0003812519410000069
步骤七：自适应调整
在卡尔曼滤波过程中，根据当前得到的加速度(ax、ay、az)大小实时计算加速度方差(σ2 x，σ2 y，σ2 z)，以此调整模型噪声协方差Q，达到滤波方差自适应，调整方法为：当“当前”加速度为正时，其方差为：
Figure GDA00038125194100000610
其中amax为可设的加速度上限，y、z方向的方差计算同x。
在卡尔曼滤波过程中，实时检测卡尔曼过程是否发散，检测方法为：统计N点最小二乘解算结果，载体位置和速度，与卡尔曼滤波结果差值，如果该差值连续T次超过设定的误差门限，则认为卡尔曼滤波发散，退回步骤四并重启卡尔曼滤波器，重新进行卡尔曼滤波解算。

Claims (2)

1.一种提高定位精度的改进卡尔曼滤波方法，其特征在于，包括下述步骤：
步骤一：卫星捕获跟踪与同步，
依次捕获GPS、BD2、GLONASS卫星，跟踪卫星进行位同步和帧同步；
步骤二：提取数据，
提取帧同步后的卫星码NCO、码片计数、1ms计数、位计数、字计数、帧计数和多普勒，得到发射时刻、伪距和伪距率；
步骤三：卫星位置速度计算，
按照卫星导航接口控制文件(ICD)提供的卫星位置计算方法，输入卫星发射时刻计算得到卫星位置和速度；
步骤四：最小二乘解算，
通过求得的跟踪卫星伪距、位置和速度进行最小二乘计算，得到载体的位置和速度；
步骤五：扩展卡尔曼滤波量测和状态方程建立，
通过求得的跟踪卫星伪距和伪距率、位置和速度，以接收机钟差
Figure FDA0003812519400000011
为测量模型建立卡尔曼滤波量测方程，其中R为星地几何距离，ρ为伪距测量值，δts为卫星钟差，δ∑为大气层延迟及接收机测量误差的总误差，c为光速，钟漂测量模型通过对钟差求导得到，
以载体位置(Xt，Yt，Zt)、速度
Figure FDA0003812519400000012
加速度
Figure FDA0003812519400000013
时钟偏差量cδt、时钟偏差变化率
Figure FDA0003812519400000021
为未知数建立状态方程，考虑到高动态环境，卡尔曼滤波动态模型采用当前统计模型
Figure FDA0003812519400000022
式中
Figure FDA0003812519400000023
为机动加速度“当前”均值，在每个采样周期内为常数；a(t)为机动加速度；
Figure FDA0003812519400000024
为载体的加速度；τ为机动加速度变化率的相关时间常数；w(t)是零均值的高斯白噪声，钟差采用一阶马尔科夫随机模型，
步骤六：扩展卡尔曼滤波过程，
量测方程和状态方程建立后，依次经过求解状态一步转移预测值、计算估计误差协方差矩阵一步转移预测值、求解滤波增益矩阵、求解信息向量、更新估计误差协方差矩阵以及估计当前时刻的状态估计值的步骤，得到当前时刻估计载体位置(Xt，Yt，Zt)、速度
Figure FDA0003812519400000025
加速度
Figure FDA0003812519400000026
时钟偏差量cδt、时钟偏差变化率
Figure FDA0003812519400000027
步骤七：自适应调整
在卡尔曼滤波过程中，根据当前得到的加速度(ax、ay、az)大小实时计算加速度方差(σ2 x，σ2 y，σ2 z)，以此调整模型噪声协方差Q，达到滤波方差自适应，调整方法为：当“当前”加速度为正时，其方差为：
Figure FDA0003812519400000028
其中amax为可设的加速度上限，y、z方向的方差计算同x。
2.如权利要求1所述的一种提高定位精度的改进卡尔曼滤波方法，其特征在于：在卡尔曼滤波过程中，实时检测卡尔曼过程是否发散，检测方法为：统计N点最小二乘解算结果，载体位置和速度，与卡尔曼滤波结果差值，如果该差值连续T次超过设定的误差门限，则认为卡尔曼滤波发散，退回步骤四并重启卡尔曼滤波器，重新进行卡尔曼滤波解算。
CN201710724015.6A 2017-08-22 2017-08-22 一种提高定位精度的改进卡尔曼滤波方法 Active CN109425876B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710724015.6A CN109425876B (zh) 2017-08-22 2017-08-22 一种提高定位精度的改进卡尔曼滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710724015.6A CN109425876B (zh) 2017-08-22 2017-08-22 一种提高定位精度的改进卡尔曼滤波方法

Publications (2)

Publication Number Publication Date
CN109425876A CN109425876A (zh) 2019-03-05
CN109425876B true CN109425876B (zh) 2023-01-10

Family

ID=65497444

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710724015.6A Active CN109425876B (zh) 2017-08-22 2017-08-22 一种提高定位精度的改进卡尔曼滤波方法

Country Status (1)

Country Link
CN (1) CN109425876B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110632629B (zh) * 2019-09-17 2023-12-08 成都国星通信有限公司 一种适用于高动态非平稳系统的自适应卡尔曼滤波方法
CN111314007A (zh) * 2019-12-13 2020-06-19 中国人民解放军陆军工程大学 Tdma多址接入的分帧同步系统及其方法
CN110988941A (zh) * 2019-12-27 2020-04-10 北京遥测技术研究所 一种高精度实时绝对定轨方法
CN111856531A (zh) * 2020-07-10 2020-10-30 暨南大学 一种基于卫星定位的农业物联网方法
CN112097771B (zh) * 2020-08-18 2022-04-29 青岛职业技术学院 Gps延迟时间自适应的扩展卡尔曼滤波导航方法
CN112285645B (zh) * 2020-12-25 2021-03-26 中国人民解放军国防科技大学 一种基于高轨卫星观测平台的定位方法、存储介质及系统
CN113109848B (zh) * 2021-04-14 2023-03-14 长沙学院 一种载体低速旋转条件下的导航定位方法
CN113341171A (zh) * 2021-06-01 2021-09-03 北京全路通信信号研究设计院集团有限公司 一种具有低延迟特性的列车测速降噪滤波方法及装置
CN117289256B (zh) * 2023-11-24 2024-01-30 成都本原星通科技有限公司 一种基于低轨通信卫星的目标远距离高精度追踪方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2721257A1 (en) * 2009-12-18 2011-06-18 Thales Satellite-based positioning receiver
CN102645222A (zh) * 2012-04-10 2012-08-22 惠州市德赛西威汽车电子有限公司 一种卫星惯性导航方法和设备
CN102928858A (zh) * 2012-10-25 2013-02-13 北京理工大学 基于改进扩展卡尔曼滤波的gnss单点动态定位方法
CN103529459A (zh) * 2012-07-05 2014-01-22 上海映慧电子科技有限公司 一种采用单频gps和glonass组合精准定位的方法及其系统
CN104597465A (zh) * 2015-01-23 2015-05-06 河海大学 一种提高gps与glonass组合精密单点定位收敛速度的方法
CN105122083A (zh) * 2012-12-28 2015-12-02 天宝导航有限公司 全球导航卫星系统的接收器定位系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CA2721257A1 (en) * 2009-12-18 2011-06-18 Thales Satellite-based positioning receiver
CN102645222A (zh) * 2012-04-10 2012-08-22 惠州市德赛西威汽车电子有限公司 一种卫星惯性导航方法和设备
CN103529459A (zh) * 2012-07-05 2014-01-22 上海映慧电子科技有限公司 一种采用单频gps和glonass组合精准定位的方法及其系统
CN102928858A (zh) * 2012-10-25 2013-02-13 北京理工大学 基于改进扩展卡尔曼滤波的gnss单点动态定位方法
CN105122083A (zh) * 2012-12-28 2015-12-02 天宝导航有限公司 全球导航卫星系统的接收器定位系统
CN104597465A (zh) * 2015-01-23 2015-05-06 河海大学 一种提高gps与glonass组合精密单点定位收敛速度的方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
提高GPS定位精度的改进卡尔曼滤波算法研究;滕云龙 等;《现代电子技术 》;20081231(第3期);第4-6页 *

Also Published As

Publication number Publication date
CN109425876A (zh) 2019-03-05

Similar Documents

Publication Publication Date Title
CN109425876B (zh) 一种提高定位精度的改进卡尔曼滤波方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN107621264B (zh) 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN102096086B (zh) 一种基于gps/ins组合导航系统不同测量特性的自适应滤波方法
CN108594272B (zh) 一种基于鲁棒卡尔曼滤波的抗欺骗干扰组合导航方法
US11079494B2 (en) Positioning device
US20150153178A1 (en) Car navigation system and method in which global navigation satellite system (gnss) and dead reckoning (dr) are merged
CN108120994B (zh) 一种基于星载gnss的geo卫星实时定轨方法
US20160040992A1 (en) Positioning apparatus and global navigation satellite system, method of detecting satellite signals
CN108594271B (zh) 一种基于复合分层滤波的抗欺骗干扰的组合导航方法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN102508278A (zh) 一种基于观测噪声方差阵估计的自适应滤波方法
CN111380521B (zh) 一种gnss/mems惯性组合芯片定位算法中的多路径滤波方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN110133695B (zh) 一种双天线gnss位置延迟时间动态估计系统及方法
CN111750865B (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN110673148A (zh) 一种主动声纳目标实时航迹解算方法
CN109633703B (zh) 一种应对遮挡场景的北斗导航无源定位方法
CN110988894A (zh) 一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法
CN111751857A (zh) 一种车辆位姿的估算方法、装置、存储介质及系统
FR2944101A1 (fr) Systeme inertiel hybride a comportement non lineaire et procede d'hybridation par filtrage multi hypotheses associe
Rahman et al. Earth-centered Earth-fixed (ECEF) vehicle state estimation performance
US8174433B1 (en) Bias estimation and orbit determination
CN116358566B (zh) 一种基于抗差自适应因子的粗差探测组合导航方法
Kaniewski et al. Algorithms of position and velocity estimation in GPS receivers

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant