CN113109848A - 一种载体低速旋转条件下的导航定位方法及其应用系统 - Google Patents
一种载体低速旋转条件下的导航定位方法及其应用系统 Download PDFInfo
- Publication number
- CN113109848A CN113109848A CN202110398195.XA CN202110398195A CN113109848A CN 113109848 A CN113109848 A CN 113109848A CN 202110398195 A CN202110398195 A CN 202110398195A CN 113109848 A CN113109848 A CN 113109848A
- Authority
- CN
- China
- Prior art keywords
- state estimation
- estimation value
- current epoch
- mean square
- square error
- 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.)
- Granted
Links
Images
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/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种载体低速旋转条件下的导航定位方法及其应用系统,方法包括:构建卡尔曼滤波模型并设置参数;预测当前历元的状态估计值和状态估计值的均方误差阵;计算观测量残余和测量方差;若观测量残余与测量方差的比值超出门限值且当前历元的滤波深度超过阈值,根据观测量残余与测量方差的比值调整测量方差,并根据调整后的测量方差计算卡尔曼滤波增益,根据卡尔曼滤波增益进行校正;相较于传统的卡尔曼滤波算法可能导致定位结果发生跳变,本发明通过观测量残余与测量均方差的比值,调整超限观测量的方差,减小其滤波增益,达到弱化超限观测值的权重的目的,实现所有可见卫星均参与滤波,使得定位结果不会发生跳变,使得定位结果更加准确。
Description
技术领域
本发明涉及卫星导航接收机技术领域,特别涉及一种载体低速旋转条件下的导航定位方法及其应用系统。
背景技术
随着卫星导航接收机在炮弹等载体中的应用,接收机需要考虑载体旋转引起的可见卫星组合的变化带来的定位结果的跳变。以某型探空火箭为例,最低转速可到0.25转/秒,考虑到天线的接收信号范围及其方向图特性,为了保证能够收齐定位解算所需的卫星数,所以在弹体两侧对向各均安装一个天线。为了方便测量,采用双天线合路设计,但双天线合路会导致接收机接收到的信号能量降低约3dB,所以定位精度也会随之变差,这就对导航定位算法提出了更高的要求。
目前传统的导航定位算法有:最小二乘算法和卡尔曼(Kalman)滤波算法。最小二乘算法没有将不同时刻的定位值联系起来相互制约,因而定位结果通常显得粗糙又杂乱。卡尔曼滤波算法考虑了相邻时刻的位置相关性,但它要求外界提供的过程噪声方差和测量噪声方差是准确的,否则卡尔曼增益和状态估计等参量不会达到最优值,甚至还可能引起滤波器发散。例如,如果输入的过程噪声方差值小于真实值,则滤波增益值将会偏小,致使测量值中所含的信息不能被及时、全部地反映在状态估计值上,最终会导致滤波器发散。对各个时刻的过程噪声和测量噪声方差值进行准确估算是卡尔曼滤波算法中重要的一步。
发明内容
本发明旨在至少解决现有技术中存在的技术问题之一。为此,本发明提出一种载体低速旋转条件下的导航定位方法及其应用系统,利用观测量残余与测量均方差的比值,调整超限观测量的方差,减小其滤波增益,达到弱化超限观测值的权重的目的,实现所有可见卫星均参与滤波,使得定位结果不会发生跳变,并且结果更加准确。
本发明的第一方面,提供了一种载体低速旋转条件下的导航定位方法,包括以下步骤:
构建卡尔曼滤波模型并设置系统运行状态变量;
基于系统状态方程以及上一历元的状态估计值和状态估计值的均方误差阵,预测当前历元的状态估计值和状态估计值的均方误差阵;
根据所述当前历元的状态估计值和状态估计值的均方误差阵计算观测量残余和测量方差;
若所述观测量残余与所述测量方差的比值超出门限值且当前历元的滤波深度超过阈值,根据所述观测量残余与所述测量方差的比值调整所述测量方差,并根据调整后的所述测量方差计算卡尔曼滤波增益,根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵;若所述观测量残余与所述测量方差的比值未超出门限值和/或当前历元的滤波深度未超过阈值,根据所述测量方差计算卡尔曼滤波增益,根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵。
根据本发明的实施例,至少具有如下技术效果:
相较于传统的卡尔曼滤波算法直接剔除某一超限的观测量,可能导致定位结果发生跳变;本方法通过利用观测量残余与测量均方差的比值,调整超限观测量的方差,减小其滤波增益,达到弱化超限观测值的权重的目的,实现所有可见卫星均参与滤波,使得定位结果不会发生跳变,使得定位结果更加准确。
本发明的第二方面,提供了一种应用系统,所述应用系统中使用本发明第一方面所述的载体低速旋转条件下的导航定位方法。
根据本发明的实施例,至少具有如下技术效果:
相较于传统的卡尔曼滤波算法直接剔除某一超限的观测量,可能导致定位结果发生跳变;本系统通过利用观测量残余与测量均方差的比值,调整超限观测量的方差,减小其滤波增益,达到弱化超限观测值的权重的目的,实现所有可见卫星均参与滤波,使得定位结果不会发生跳变,使得定位结果更加准确。
本发明的附加方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解,其中:
图1为本发明实施例提供的一种载体低速旋转条件下的导航定位方法的流程示意图;
图2为本发明实施例提供的导航定位的卡尔曼算法流程框图;
图3为本发明实施例提供的实验结果示意图;
图4为本发明实施例提供的实验结果示意图;
图5为本发明实施例提供的实验结果示意图;
图6为本发明实施例提供的实验结果示意图。
具体实施方式
下面详细描述本发明的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,仅用于解释本发明,而不能理解为对本发明的限制。
第一实施例;
参照图1和图2,本发明的一个实施例,提供了一种载体低速旋转条件下的导航定位方法,本方法根据卫星的测量残余来设置其权重,即当某测量值表现为较大的测量残余时,通过调整它的测量噪声方差值,减小它的权重,实现所有可见卫星以适当的权重参与滤波运算。针对低速旋转条件下的载体,定位可用率提升大约10%,方法包括以下步骤:
步骤S100、构建卡尔曼滤波模型并设置系统运行状态变量。
步骤S100中的系统运行状态变量包括常系数状态转移矩阵A、测量关系矩阵C、过程噪声矩阵Q、状态向量x、观测向量y以及测量噪声R,具体的:
对于低速旋转的载体系统,其运行状态一般由载体的三个坐标分量上的位置、速度、加速度和接收机的钟差、频差所组成的一个11维状态向量x来描述,如下:
x=[x vx ax y vy ay z vz az δtu δfu] (1)
常系数状态转移矩阵A为:
式(2)中,TS表示状态方程的差分步长,状态方程用于描述系统状态如何随时间变化。
测量关系矩阵C:
假设某时刻某卫星位置为(xs,ys,zs),用户位置为(x,y,z),则星地距离D为
该卫星的测量关系矩阵为:
过程噪声矩阵Q为:
式(5)中,Qp、Qv、Qa、Qt、Qf分别表示位置、速度、加速度、钟差、频差的噪声。
步骤S200、基于系统状态方程以及上一历元的状态估计值和状态估计值的均方误差阵,预测当前历元的状态估计值和状态估计值的均方误差阵。
其中,Pk-1表示上一历元的状态估计值的均方误差阵。
步骤S300、根据当前历元的状态估计值和状态估计值的均方误差阵计算观测量残余和测量方差。
计算公式如下:
其中yk表示当前历元的观测值,R表示测量噪声。
步骤S400、若观测量残余与测量方差的比值超出门限值且当前历元的滤波深度超过阈值,则根据观测量残余与测量方差的比值调整测量方差,并进入步骤S500;否则,直接进入步骤S500。
作为一种可选的实施方式,按照以下公式调整测量方差:
作为一种可选的实施方式,经过实验发现,门限值设为5;阈值设为20时,得到的效果最佳,可参见后续实施例的实验结果。
步骤S500、根据测量方差计算卡尔曼滤波增益,根据卡尔曼滤波增益校正当前历元的状态估计值和状态估计值的均方误差阵。
校正公式如下:
其中Kk表示卡尔曼滤波增益,I表示单位矩阵。
步骤S600、当前历元的滤波处理完成,滤波深度加1,并跳转至步骤S200进行下一历元的滤波处理。
上述步骤S100至S600中以低速旋转的用户模型为例,与传统的卡尔曼滤波算法相比,定位结果可用率(三维定位误差小于30m)从89%提升到99%,三维测速误差(1σ)从0.19m/s提升到0.11m/s。针对静态低速旋转场景下,由于可见卫星组合发生变化引起的定位误差大的问题,本方法实施例提高了定位结果的正确率。
相较于传统的卡尔曼滤波算法直接剔除某一超限的观测量,可能导致定位结果发生跳变;本方法通过利用观测量残余与测量均方差的比值,调整超限观测量的方差,减小其滤波增益,达到弱化超限观测值的权重的目的,实现所有可见卫星均参与滤波,使得定位结果不会发生跳变,并且结果更加准确。
第二实施例;
如图3至图6,提供了一组转速为4秒每转的场景下,传统的卡尔曼滤波算法与第一实施例所述方法的定位测速的实验结果。
如图3所示,图中横坐标代表东向测速误差,纵坐标代表北向测速误差,单位为m/s。矩形方框表示传统方法测速误差,圆圈框表示本方法测速误差,由图可知,传统方法下,东向测速误差分布在-100m/s~250m/s,北向测速误差分布在±300m/s,本方法得到的测速误差分布在0附近,相较传统方法,本方法优化了测速误差大于1m/s时刻的测速结果,本方法的测速精度明显优于传统方法。
如图4所示,图中横坐标代表周内秒,纵坐标代表三维测速误差,单位为m/s。虚线表示传统方法下的测速误差,实线表示本方法测速误差。由图可知,在周内秒4.02100×105往后的时间段,传统方法下的三维测速误差超出1m/s,本方法得到的测速误差在0.1m/s以内,测速精度明显得到优化。
如图5所示,图中横坐标代表东向定位误差,纵坐标代表北向定位误差,单位为m。矩形方框表示传统方法定位误差,圆圈框表示本方法定位误差,由图可知,传统方法下,东向定位误差分布在-250m~200m,北向定位误差分布在-400m~300m,本方法得到的定位误差分布在±30m,相较传统方法,本方法优化了定位误差大于30m/s时刻的定位结果,本方法的定位精度明显优于传统方法。
如图6所示,图中横坐标代表周内秒,纵坐标代表三维定位误差,单位为m。虚线表示传统方法下的定位误差,实线表示本方法定位误差。由图可知,在周内秒4.02100×105往后的时间段,传统方法下的三维定位误差超出30m,本方法得到的定位误差小于10m,在周内秒4.01700×105~4.02100×105时间段,本方法得到的定位误差比传统方法下的定位误差明显较小,差值约2~10m。本方法较传统方法,定位精度更优。
第三实施例;
提供了一种应用系统,本应用系统中使用第一实施例所述的载体低速旋转条件下的导航定位方法。
例如上述实施例的方法可应用于气象探空火箭,用来收集大气参数。气象探空火箭垂直发射,转速低于0.25r/s。
由于本系统实施例与第一实施例是属于同一个发明构思,因此第一实施例的相应内容同样适应于本系统实施例,本领域技术人员在清楚第一实施例的基础上,可以不付出创造性劳动就能实现本系统实施例的技术方案,因此本系统实施例不再细述。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不一定指的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施例,本领域的普通技术人员可以理解:在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型,本发明的范围由权利要求及其等同物限定。
Claims (5)
1.一种载体低速旋转条件下的导航定位方法,其特征在于,包括以下步骤:
构建卡尔曼滤波模型并设置系统运行状态变量;
基于系统状态方程以及上一历元的状态估计值和状态估计值的均方误差阵,预测当前历元的状态估计值和状态估计值的均方误差阵;
根据所述当前历元的状态估计值和状态估计值的均方误差阵计算观测量残余和测量方差;
若所述观测量残余与所述测量方差的比值超出门限值且当前历元的滤波深度超过阈值,根据所述观测量残余与所述测量方差的比值调整所述测量方差,并根据调整后的所述测量方差计算卡尔曼滤波增益,根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵;若所述观测量残余与所述测量方差的比值未超出门限值和/或当前历元的滤波深度未超过阈值,根据所述测量方差计算卡尔曼滤波增益,根据所述卡尔曼滤波增益校正所述当前历元的状态估计值和状态估计值的均方误差阵。
4.根据权利要求1所述的载体低速旋转条件下的导航定位方法,其特征在于,所述门限值设为5,所述阈值设为20。
5.一种应用系统,其特征在于,所述应用系统中应用有权利要求1至4任一项所述的载体低速旋转条件下的导航定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110398195.XA CN113109848B (zh) | 2021-04-14 | 2021-04-14 | 一种载体低速旋转条件下的导航定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110398195.XA CN113109848B (zh) | 2021-04-14 | 2021-04-14 | 一种载体低速旋转条件下的导航定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113109848A true CN113109848A (zh) | 2021-07-13 |
CN113109848B CN113109848B (zh) | 2023-03-14 |
Family
ID=76716581
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110398195.XA Active CN113109848B (zh) | 2021-04-14 | 2021-04-14 | 一种载体低速旋转条件下的导航定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113109848B (zh) |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102508278A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于观测噪声方差阵估计的自适应滤波方法 |
JP2014215822A (ja) * | 2013-04-25 | 2014-11-17 | 日本電信電話株式会社 | 状態推定装置、方法、及びプログラム |
CN105510942A (zh) * | 2015-12-27 | 2016-04-20 | 哈尔滨米米米业科技有限公司 | 一种基于卡尔曼滤波的gps单点定位系统 |
CN107132562A (zh) * | 2016-02-26 | 2017-09-05 | 大唐半导体设计有限公司 | 一种实现卡尔曼滤波定位的方法和装置 |
CN107193028A (zh) * | 2017-03-29 | 2017-09-22 | 中国航空无线电电子研究所 | 基于GNSS的Kalman相对定位方法 |
CN109425876A (zh) * | 2017-08-22 | 2019-03-05 | 北京自动化控制设备研究所 | 一种提高定位精度的改进卡尔曼滤波方法 |
CN110109162A (zh) * | 2019-03-26 | 2019-08-09 | 西安开阳微电子有限公司 | 一种gnss接收机自适应的卡尔曼滤波定位解算方法 |
CN110455287A (zh) * | 2019-07-24 | 2019-11-15 | 南京理工大学 | 自适应无迹卡尔曼粒子滤波方法 |
CN111623703A (zh) * | 2020-07-28 | 2020-09-04 | 湖南联智科技股份有限公司 | 一种基于新型卡尔曼滤波的北斗变形监测实时处理方法 |
CN112013836A (zh) * | 2020-08-14 | 2020-12-01 | 北京航空航天大学 | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 |
-
2021
- 2021-04-14 CN CN202110398195.XA patent/CN113109848B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102508278A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于观测噪声方差阵估计的自适应滤波方法 |
JP2014215822A (ja) * | 2013-04-25 | 2014-11-17 | 日本電信電話株式会社 | 状態推定装置、方法、及びプログラム |
CN105510942A (zh) * | 2015-12-27 | 2016-04-20 | 哈尔滨米米米业科技有限公司 | 一种基于卡尔曼滤波的gps单点定位系统 |
CN107132562A (zh) * | 2016-02-26 | 2017-09-05 | 大唐半导体设计有限公司 | 一种实现卡尔曼滤波定位的方法和装置 |
CN107193028A (zh) * | 2017-03-29 | 2017-09-22 | 中国航空无线电电子研究所 | 基于GNSS的Kalman相对定位方法 |
CN109425876A (zh) * | 2017-08-22 | 2019-03-05 | 北京自动化控制设备研究所 | 一种提高定位精度的改进卡尔曼滤波方法 |
CN110109162A (zh) * | 2019-03-26 | 2019-08-09 | 西安开阳微电子有限公司 | 一种gnss接收机自适应的卡尔曼滤波定位解算方法 |
CN110455287A (zh) * | 2019-07-24 | 2019-11-15 | 南京理工大学 | 自适应无迹卡尔曼粒子滤波方法 |
CN111623703A (zh) * | 2020-07-28 | 2020-09-04 | 湖南联智科技股份有限公司 | 一种基于新型卡尔曼滤波的北斗变形监测实时处理方法 |
CN112013836A (zh) * | 2020-08-14 | 2020-12-01 | 北京航空航天大学 | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 |
Also Published As
Publication number | Publication date |
---|---|
CN113109848B (zh) | 2023-03-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
EP3688495B1 (en) | System for determining a physical metric such as position | |
EP2108923A2 (en) | Mobile terminal having a hybrid navigation system, method for determining a location thereof, and computer-readable recording medium recording the method | |
US20020158796A1 (en) | Integrated GPS and IGS system and method | |
JP3448976B2 (ja) | 車両用現在位置検出装置 | |
WO1998048295A1 (en) | Increased jamming immunity by optimizing processing gain for gps/ins systems | |
JP2001183439A (ja) | 統計的フィルタの使用に基づいた汎用位置決めシステム | |
WO1994009382A1 (en) | Track filter bias estimation | |
US7414581B2 (en) | Method for improved signal to noise ratio estimation | |
CN111380521B (zh) | 一种gnss/mems惯性组合芯片定位算法中的多路径滤波方法 | |
CN110988950A (zh) | 导航装置、航行辅助信息的生成方法及存储介质 | |
JP2011127939A (ja) | 移動体位置推定追尾装置、移動体位置推定追尾方法、及び移動体位置推定追尾プログラム | |
CN115616623A (zh) | 一种基于当前模型卡尔曼滤波的卫星导航授时方法和装置 | |
CN113794497B (zh) | 一种具有抗干扰定位功能的移动卫星通信天线终端 | |
CN113109848B (zh) | 一种载体低速旋转条件下的导航定位方法 | |
Choi et al. | An adaptive tracking estimator for robust vehicular localization in shadowing areas | |
CN106886037B (zh) | 适用于弱gnss信号条件的pos数据纠偏方法 | |
RU2617373C1 (ru) | Способ оптимальной привязки к подвижной наземной цели и прогноза её параметров на основе модифицированной, инвариантной к рельефу подстилающей поверхности угломестной процедуры расчёта дальности | |
WO2002014895A3 (en) | System, method, and software for non-iterative position estimation using range measurements | |
JP2006132949A (ja) | 測位装置、測位方法および測位プログラム | |
WO2017131548A1 (en) | Methods and apparatus for estimating motion parameters of gnss receiver | |
CN112099060B (zh) | 一种基于环路的自适应载波频率跟踪方法及装置 | |
CN112904382B (zh) | 一种城市峡谷环境下激光里程计辅助的快速优化选星方法 | |
CN111123323B (zh) | 一种提高便携设备定位精度的方法 | |
JP2002286829A (ja) | 絶対位置推定装置 | |
CN113706854B (zh) | 一种智能车联网中的车辆协作定位方法 |
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 |