CN109357674A - 一种gnss和ins组合导航观测量引入方法 - Google Patents
一种gnss和ins组合导航观测量引入方法 Download PDFInfo
- Publication number
- CN109357674A CN109357674A CN201811496616.7A CN201811496616A CN109357674A CN 109357674 A CN109357674 A CN 109357674A CN 201811496616 A CN201811496616 A CN 201811496616A CN 109357674 A CN109357674 A CN 109357674A
- Authority
- CN
- China
- Prior art keywords
- matrix
- state
- moment
- observed quantity
- navigation
- 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.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- 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
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种GNSS/INS组合导航观测量引入方法。本发明通过调节Kalman滤波器观测误差方差阵R,从而实现观测量引入,能够进行不同GNSS精度下的GNSS/INS组合导航解算。
Description
技术领域
本发明涉及导弹组合导航领域,具体地涉及一种GNSS和INS组合导航观测量引入方法,基于调节Kalman滤波器观测误差方差阵R进行观测量引入方法,能够进行不同GNSS精度下的组合导航解算。
背景技术
提高精度和可靠性是导航***要解决的基本问题。目前,尚没有一种导航方式能够同时满足高精度与高可靠性的要求。如广泛使用的卫星导航技术,它虽然具有精度高、定位精度不随时间增长而积累从而能保持长期稳定、可以全天候全时段工作、覆盖区域广、近实时输出导航数据、终端成本低廉等特点,但也存在复杂地形(如城市峡谷、密林、室内和地下建筑等)条件下的信号可用性差、易受恶意或无意干扰以及信号欺骗影响等弱点,且由于受观测噪声的影响,卫星定位精度的短期稳定性也不是特别高。与此相对应,惯性导航虽然长期稳定度较差,但其不受外界影响、短期稳定度高的特点则恰恰可以弥补卫星导航技术的不足。卫星导航信息作为观测量引入Kalman滤波器,可以对惯性导航误差进行补偿,因此组合导航***可以满足高精度与高可靠性的要求。
发明内容
针对现有技术中的缺陷,本发明的目的是提供一种GNSS和INS组合导航观测量引入方法,基于调节观测误差方差阵R的观测量引入方法,可以有效实现观测量的合理引入,实现Kalman滤波器的最优解。
本发明是根据以下技术方案实现的:
一种GNSS和INS组合导航观测量引入方法,其特征在于,包括如下步骤:
步骤S1:建立组合导航方程:
式中,X为状态变量,各元素依次为东向、北向、天向3个失准角误差,东向、北向、天向3个速度误差,纬度、经度、高度3个位置误差,F为状态转移矩阵,W为***噪声,Z为***观测量,H为观测矩阵,V为观测噪声,表示X的微分;
步骤S2:将组合方程离散化:
式中:Xk-1、Xk分别表示k-1时刻、k时刻的状态变量数值,Zk为k时刻的***观测量数值,Φ为状态转移矩阵、Q为***噪声矩阵。状态转移矩阵Φ、***噪声矩阵Q分别由F、W离散化得到;
观测噪声方差阵R取值方法为:卫星导航信息有效时观测噪声方差阵R取R1,无效时R取R2;
R1=diag[Δp2Δp2Δp2Δv2Δv2Δv2],
R2=diag[∞∞∞∞∞∞],
式中:Δp、Δv分别表示卫星导航单向位置、速度均方差;∞表示正无穷;
步骤S3:建立Kalman滤波方程:
P(-)=ΦPk-1ΦT+Q,
K=P(-)HT[HPk-1HT+R]-1,
Xk=ΦXk-1+K[Zk-HΦXk-1],
Pk=[I-KH]P(-)[I-KH]T+KRKT,
式中:Pk-1、Pk分别表示k-1时刻、k时刻的P阵数值、P(-)为中间变量,K为增益矩阵,ΦT为矩阵Φ的转置,A-1表示矩阵A的逆矩阵,
R值影响K的取值:当卫星导航用时R取R1,将观测量对于状态量估计的影响引入;当卫星导航失锁时R取R2,将观测量对于状态量估计的影响屏蔽;
通过Kalman滤波,估计得到状态变量X的结果。
与现有技术相比,本发明具有如下的有益效果:当卫星导航正常工作时,可以将观测量对于状态量估计的影响引入;当卫星导航失锁,将观测量对于状态量估计的影响屏蔽。从而实现卫星导航短时间内失锁情况下,组合导航***的正常工作。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明的其它特征、目的和优点将会变得更明显:
图1为滤波工作流程图;
图2为组合导航仿真的高度误差结果示意图;
图3为第200~250s时卫星导航失锁、其他时刻卫星导航结果可用情况下的组合导航高度误差。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明,但不以任何形式限制本发明。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变化和改进。这些都属于本发明的保护范围。
现参考附图详细描述根据本发明实施例的观测量引入方法。
本发明通过如下步骤具体实现:
步骤S1:建立组合方程
式中,X为状态变量,各元素依次为东向、北向、天向3个失准角误差,东向、北向、天向3个速度误差,纬度、经度、高度3个位置误差,F为状态转移矩阵,W为***噪声,Z为***观测量,H为观测矩阵,V为观测噪声,表示X的微分。
步骤S2:将组合方程离散化
式中:Xk-1、Xk分别表示k-1时刻、k时刻的状态变量数值,Zk为k时刻的***观测量数值,Φ为状态转移矩阵、Q为***噪声矩阵。状态转移矩阵Φ、***噪声矩阵Q分别由F、W离散化得到;
观测噪声方差阵R取值方法为:
卫星导航信息有效时观测噪声方差阵R取R1,无效时R取R2;
R1=diag[Δp2Δp2Δp2Δv2Δv2Δv2],
R2=diag[∞∞∞∞∞∞],
式中:Δp、Δv分别表示卫星导航单向位置、速度均方差;∞表示正无穷,实际工程中应该取尽量大的数值。
步骤S3:建立Kalman滤波方程:
P(-)=ΦPk-1ΦT+Q,
K=P(-)HT[HPk-1HT+R]-1,
Xk=ΦXk-1+K[Zk-HΦXk-1],
Pk=[I-KH]P(-)[I-KH]T+KRKT,
式中:Pk-1、Pk分别表示k-1时刻、k时刻的P阵数值、P(-)为中间变量,K为增益矩阵,ΦT为矩阵Φ的转置,A-1表示矩阵A的逆矩阵。
R值影响K的取值:当卫星导航可用时R取R1,将观测量对于状态量估计的影响引入;当卫星导航失锁时R取R2,将观测量对于状态量估计的影响屏蔽;
通过Kalman滤波,即可以估计得到状态变量X的结果。
仿真实例
组合导航仿真的高度误差结果见图2、3所示。图2表示全程卫星导航结果可用时的组合导航高度误差,图3表示第200~250s时卫星导航失锁、其他时刻卫星导航结果可用情况下的组合导航高度误差。
由图2、3可见,当卫星导航可用时,观测量可以进入滤波器,估计得到惯性导航的误差,组合导航误差较小;当卫星导航失锁时,观测量不可以进入滤波器,期间组合导航误差相较卫星导航可用时误差变大。
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变化或修改,这并不影响本发明的实质内容。在不冲突的情况下,本申请的实施例和实施例中的特征可以任意相互组合。
Claims (1)
1.一种GNSS和INS组合导航观测量引入方法,其特征在于,包括如下步骤:
步骤S1:建立组合导航方程:
式中,X为状态变量,各元素依次为东向、北向、天向3个失准角误差,东向、北向、天向3个速度误差,纬度、经度、高度3个位置误差,F为状态转移矩阵,W为***噪声,Z为***观测量,H为观测矩阵,V为观测噪声,表示X的微分;
步骤S2:将组合方程离散化:
式中:Xk-1、Xk分别表示k-1时刻、k时刻的状态变量数值,Zk为k时刻的***观测量数值,Φ为状态转移矩阵、Q为***噪声矩阵,状态转移矩阵Φ、***噪声矩阵Q分别由F、W离散化得到;
观测噪声方差阵R取值方法为:卫星导航信息有效时观测噪声方差阵R取R1,无效时R取R2;
R1=diag[Δp2 Δp2 Δp2 Δv2 Δv2 Δv2],
R2=diag[∞ ∞ ∞ ∞ ∞ ∞],
式中:Δp、Δv分别表示卫星导航单向位置、速度均方差;∞表示正无穷;
步骤S3:建立Kalman滤波方程:
P(-)=ΦPk-1ΦT+Q,
K=P(-)HT[H Pk-1HT+R]-1,
Xk=ΦXk-1+K[Zk-HΦXk-1],
Pk=[I-KH]P(-)[I-KH]T+KRKT,
式中:P阵为状态误差方差阵,I为单位矩阵,Pk-1、Pk分别表示k-1时刻、k时刻的P阵数值、P(-)为中间变量,K为增益矩阵,ΦT为矩阵Φ的转置,A-1表示矩阵A的逆矩阵,
R值影响K的取值:当卫星导航用时R取R1,将观测量对于状态量估计的影响引入;当卫星导航失锁时R取R2,将观测量对于状态量估计的影响屏蔽;
通过Kalman滤波,估计得到状态变量X的结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811496616.7A CN109357674A (zh) | 2018-12-07 | 2018-12-07 | 一种gnss和ins组合导航观测量引入方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811496616.7A CN109357674A (zh) | 2018-12-07 | 2018-12-07 | 一种gnss和ins组合导航观测量引入方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109357674A true CN109357674A (zh) | 2019-02-19 |
Family
ID=65331775
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811496616.7A Pending CN109357674A (zh) | 2018-12-07 | 2018-12-07 | 一种gnss和ins组合导航观测量引入方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109357674A (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6449559B2 (en) * | 1998-11-20 | 2002-09-10 | American Gnc Corporation | Fully-coupled positioning process and system thereof |
CN1869630A (zh) * | 2006-04-19 | 2006-11-29 | 吉林大学 | 完备汽车运动状态测量*** |
CN102506857A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 |
CN103149580A (zh) * | 2013-02-04 | 2013-06-12 | 东南大学 | 一种基于stkf和wnn的gps/ins组合导航方法 |
CN103278837A (zh) * | 2013-05-17 | 2013-09-04 | 南京理工大学 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
CN106595705A (zh) * | 2016-11-22 | 2017-04-26 | 北京航天自动控制研究所 | 一种基于gps的飞行中惯性初始基准偏差估计方法 |
-
2018
- 2018-12-07 CN CN201811496616.7A patent/CN109357674A/zh active Pending
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6449559B2 (en) * | 1998-11-20 | 2002-09-10 | American Gnc Corporation | Fully-coupled positioning process and system thereof |
CN1869630A (zh) * | 2006-04-19 | 2006-11-29 | 吉林大学 | 完备汽车运动状态测量*** |
CN102506857A (zh) * | 2011-11-28 | 2012-06-20 | 北京航空航天大学 | 一种基于双imu/dgps组合的相对姿态测量实时动态滤波方法 |
CN103149580A (zh) * | 2013-02-04 | 2013-06-12 | 东南大学 | 一种基于stkf和wnn的gps/ins组合导航方法 |
CN103278837A (zh) * | 2013-05-17 | 2013-09-04 | 南京理工大学 | 基于自适应滤波的sins/gnss多级容错组合导航方法 |
CN106595705A (zh) * | 2016-11-22 | 2017-04-26 | 北京航天自动控制研究所 | 一种基于gps的飞行中惯性初始基准偏差估计方法 |
Non-Patent Citations (1)
Title |
---|
胡世明: "GPS/INS组合导航数据融合算法研究", 《中国优秀硕士学位论文全文数据库》 * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
CN101246024B (zh) | 一种外场快速标定微型多传感器组合导航***的方法 | |
CN104280746B (zh) | 一种惯性辅助gps的深组合半实物仿真方法 | |
CN104181572B (zh) | 一种弹载惯性/卫星紧组合导航方法 | |
CN103575299B (zh) | 利用外观测信息的双轴旋转惯导***对准及误差修正方法 | |
CN105043415B (zh) | 基于四元数模型的惯性系自对准方法 | |
CN103017755B (zh) | 一种水下导航姿态测量方法 | |
CN105607093B (zh) | 一种组合导航***及获取导航坐标的方法 | |
CN104698485B (zh) | 基于bd、gps及mems的组合导航***及导航方法 | |
CN102636798B (zh) | 基于环路状态自检测的sins/gps深组合导航方法 | |
CN105910602B (zh) | 一种组合导航方法 | |
CN102519485B (zh) | 一种引入陀螺信息的二位置捷联惯性导航***初始对准方法 | |
CN105606094A (zh) | 一种基于mems/gps组合***的信息条件匹配滤波估计方法 | |
CN105371844A (zh) | 一种基于惯性/天文互助的惯性导航***初始化方法 | |
CN103674021A (zh) | 基于捷联惯导与星敏感器的组合导航***及方法 | |
CN108151737A (zh) | 一种动态互观测关系条件下的无人机蜂群协同导航方法 | |
Choi et al. | A consumer tracking estimator for vehicles in GPS-free environments | |
CN116734887B (zh) | 基于速度误差修正模型的极地双惯导协同标定方法 | |
CN106979781A (zh) | 基于分布式惯性网络的高精度传递对准方法 | |
CN110058288A (zh) | 无人机ins/gnss组合导航***航向误差修正方法及*** | |
CN116481564B (zh) | 基于Psi角误差修正模型的极地双惯导协同标定方法 | |
CN116391138A (zh) | 定位方法、装置、设备、***以及存储介质 | |
CN108151765B (zh) | 一种在线实时估计补偿磁强计误差的定位测姿方法 | |
CN114779307B (zh) | 面向港区的uwb/ins/gnss的无缝定位方法 | |
CN109470272A (zh) | 一种imu测量基准的标定方法 |
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 | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190219 |