CN112269201B - 一种gnss/ins紧耦合时间分散滤波方法 - Google Patents

一种gnss/ins紧耦合时间分散滤波方法 Download PDF

Info

Publication number
CN112269201B
CN112269201B CN202011145864.4A CN202011145864A CN112269201B CN 112269201 B CN112269201 B CN 112269201B CN 202011145864 A CN202011145864 A CN 202011145864A CN 112269201 B CN112269201 B CN 112269201B
Authority
CN
China
Prior art keywords
gnss
time
matrix
ins
imu
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
CN202011145864.4A
Other languages
English (en)
Other versions
CN112269201A (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 Heng Heng Science And Technology Research Institute Co ltd
Original Assignee
Beijing Heng Heng Science And Technology Research Institute Co ltd
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 Heng Heng Science And Technology Research Institute Co ltd filed Critical Beijing Heng Heng Science And Technology Research Institute Co ltd
Priority to CN202011145864.4A priority Critical patent/CN112269201B/zh
Publication of CN112269201A publication Critical patent/CN112269201A/zh
Application granted granted Critical
Publication of CN112269201B publication Critical patent/CN112269201B/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/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
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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/396—Determining accuracy or reliability of position or pseudorange measurements
    • 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
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

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

本发明涉及一种GNSS/INS紧耦合时间分散滤波方法，通过分散传统Kalman滤波器的计算过程减小组合时刻点紧耦合的耗时。传统紧耦合滤波器在组合时刻点集中进行时间更新和测量更新，耗时较大，甚至超过INS的解算周期，影响INS力学编排的实时性。本发明将滤波器过程分散在各个时间段内，在组合时刻点之前提前进行时间更新和增益阵计算。而在组合时刻点只进行状态变量和状态协方差阵的修正，大大减小了组合时刻点的计算量。该方法不损失滤波器的精度，有利于GNSS/INS紧耦合的工程应用。

Description

一种GNSS/INS紧耦合时间分散滤波方法
技术领域
本发明涉及一种GNSS/INS紧耦合时间分散滤波方法，属于组合导航技术领域。
背景技术
全球卫星导航系统(GNSS)能够为不同平台提供时空基准，具有全天候工作、连续性强、精度高、实时性强等特点。但是其突出的缺点是卫星信号在众所周知的频率上发射，发射功率不大，信噪比低，到达地面的信号弱，容易产生信号中断、多径效应等问题，且易受干扰或欺骗。
惯性导航(INS)既不依赖外部信息、又不产生能量辐射，以本身携带的加速度计、陀螺仪为敏感器，探测平台在惯性空间中的比力和角速度，并以此计算平台在导航系中的位置、姿态、速度等信息，是一种探测信息极为全面的导航系统。但是由于导航结果由积分得到，因而误差会长时积累。
GNSS/INS组合导航系统能够实现两种导航方式的优势互补，是一种较为常见的传感器融合方案。紧耦合使用GNSS的伪距、伪距率作为观测量，是一种层次较深的组合模式。紧耦合的耗时与GNSS卫星数有关，星数越多，耗时越大。在主频为800MHz的ARM A9平台上进行测试，8颗星耗时约10ms。该耗时限制了INS的解算频率不能超过100Hz，否则会造成导航结果输出延迟。而实际应用中IMU频率普遍在200Hz以上，因此需要解决紧耦合耗时问题。
目前，减小Kalman滤波器计算量的方法主要有并行卡尔曼滤波和解耦卡尔曼滤波两种。并行卡尔曼滤波通过近似手段将时间更新和测量更新分离，从而把串行的计算过程变为并行。解耦卡尔曼滤波则利用状态变量之间的弱相关性，将一个高维数的滤波器分解为多个低维数的滤波器。这两种方法均不适用于GNSS/INS紧耦合Kalman滤波器。
发明内容
本发明要解决的技术问题是：避免传统紧耦合Kalman滤波器因观测量维数多造成计算耗时较大，提供了一种GNSS/INS紧耦合时间分散滤波方法。该方法通过分解传统Kalman滤波器的计算过程，将与GNSS实时观测量无关的部分计算过程在组合时刻点之前执行，此时CPU的载荷较小。而在组合时刻点只执行GNSS观测量修正状态变量的过程，从而大大减小了组合时刻点的计算量。该方法不损失滤波器的精度，可以被应用于GNSS/INS组合导航装置中。
本发明目的通过以下技术方案予以实现：
一种GNSS/INS紧耦合时间分散滤波方法，其特征在于：将组合Kalman滤波器的过程分解为时间更新、增益阵计算和状态变量修正，分别在不同时间段内计算，避免集中计算引起的耗时较大问题；具体的分散滤波方法包括如下步骤：
步骤(1)、在组合时刻点之前2个IMU周期内进行Kalman滤波器的时间更新；
步骤(2)、在组合时刻点之前1个IMU周期内计算Kalman滤波器的观测矩阵H和增益矩阵K；(R阵)
步骤(3)、在组合时刻点，检查是否需要重构H阵和K阵，若需要则重新计算H阵和K阵；
步骤(4)、在组合时刻点，计算Kalman滤波器的观测向量Z；
步骤(5)、在组合时刻点，对时间更新后的状态变量和状态协方差阵进行修正。
上述GNSS/INS紧耦合时间分散滤波方法，所述步骤(1)中在组合时刻点之前2个IMU周期内进行Kalman滤波器时间更新的具体方法如下：
(1a)、上次组合后，以IMU周期累积系统矩阵，设紧耦合滤波器的连续状态方程为
其中，X为状态向量，包括INS的姿态、位置和速度误差，IMU的零偏和GNSS的钟差、钟漂；F为系统矩阵，包括INS的姿态和IMU的输出；G为噪声矩阵；W为驱动噪声，其协方差阵为q，
在每个IMU周期，完成INS力学编排后累积系统矩阵，
其中，Fs为累积的系统矩阵；Fi为第i次力学编排确定的系统矩阵；N为组合周期内INS力学编排的总次数；
(1b)、提前两个IMU周期对滤波器的连续状态方程进行离散化，为避免在下次组合时刻点的集中计算，提前两个IMU周期进行时间更新，完成第N-2次INS力学编排后，对组合时刻前两个的系统矩阵进行近似，即
FN-1＝FN-2,FN＝FN-2
上式假设了在组合前的两个IMU周期内载体保持姿态和IMU输出不变，最后得到完整的累积矩阵
设离散化后的滤波器状态方程为
Xk＝Φk,k-1Xk-1+Wk
式中，k代表系统的第k个采样时刻；Φk,k-1为一步状态转移矩阵；Wk为系统噪声，其协方差阵为Qk，
Φk,k-1的计算公式为
Φk,k-1＝I+Fs*TIMU
其中，I为单位矩阵，TIMU为IMU周期，
当连续噪声协方差阵q为对角阵时，则Q＝GqGT亦为对角阵，则离散噪声协方差阵的计算公式近似为
Qk＝Q*TTC
其中，Q＝GqGT；TTC为紧组合周期；
(1c)、进行滤波器的时间更新，时间更新的公式为
Xk,k-1＝Φk,k-1Xk-1
式中，Pk-1为k-1时刻状态Xk-1的协方差矩阵；Pk,k-1为k时刻预报状态Xk,k-1的协方差矩阵。
进行时间更新的同时，将累积系统矩阵Fs清零，以开始下一次组合；
上述GNSS/INS紧耦合时间分散滤波方法，所述步骤(2)中在组合时刻点之前1个IMU周期内计算Kalman滤波器观测矩阵H和增益矩阵K的具体方法如下：
(2a)、假设上次组合后GNSS可用卫星不变，根据星历和下次组合时刻提前计算GNSS卫星的位置；
(2b)、提前1个IMU周期计算观测矩阵Hk，H阵的表达形式与滤波器选取的观测量类型有关，观测量选为GNSS和INS的伪距和伪距率之差，此时，H阵由GNSS卫星与载体之间的方向矢量构成。方向矢量的表达式为
其中，rs为步骤(2a)中获得的卫星位置；ru为当前时刻载体的位置；
(2c)、提前1个IMU周期计算观测噪声矩阵Rk，获取H阵后，计算R阵；
(2d)、提前1个IMU周期计算观测噪声矩阵Kk，获取H和R阵后，即可按下式计算K阵，
上述GNSS/INS紧耦合时间分散滤波方法，所述步骤(3)中在组合时刻点检查是否需要重构H阵和K阵的具体方法如下：
(3a)、比较当前时刻与上次组合时刻的GNSS可用卫星，检查是否存在新上星或下星情况；
(3b)、若两个时刻可用卫星完全相同，则无需重构；否则，按步骤(2)方法重新计算H阵和K阵。
上述GNSS/INS紧耦合时间分散滤波方法，所述步骤(4)中在组合时刻点计算Kalman滤波器观测向量Z的具体方法如下：
(4a)、计算GNSS与INS的伪距之差，伪距差观测量为GNSS测量伪距与INS构造的伪距之差，具体表达式如下
Δρ＝(||rs-ru||-dts+Ion+Trp+Rot)-ρGNSS
其中，dts为卫星钟差；Ion为卫星电离层延迟修正项；Trp为卫星对流层延迟修正项；Rot为地球自转效应修正项；ρGNSS为GNSS伪距测量值；
(4b)、计算GNSS与INS的伪距率之差，伪距率差观测量为GNSS测量伪距率与INS构造的伪距率之差，具体表达式如下
其中，vs为卫星的速度；vu为当前时刻载体的速度；dfs为卫星钟漂；为GNSS伪距率测量值。
上述GNSS/INS紧耦合时间分散滤波方法，所述步骤(5)中在组合时刻点对时间更新后状态变量和状态协方差阵进行修正的具体方法如下：
(5a)、对状态变量进行修正。修正公式为
Xk＝Xk,k-1+Kk(Zk-HkXk,k-1)
(5b)、对状态协方差阵进行修正。修正公式为
Pk＝(I-KkHk)Pk,k-1。
本发明相比于现有技术具有如下有益效果：
(1)本发明提出的GNSS/INS紧耦合时间分散滤波方法，针对Kalman滤波器集中计算耗时大的问题，将部分滤波器过程分散在空闲的时间段内执行。在组合时刻点之前2个IMU周期进行时间更新，之前1个IMU周期进行观测阵和增益阵计算。而在组合时刻点只进行状态变量和状态协方差阵的修正；
(2)本发明提出的GNSS/INS紧耦合时间分散滤波方法，利用了GNSS接收机卫星信号跟踪稳定，可用卫星在一段时间内不易发生变化的特点，提前计算观测阵和增益阵。即便发生了卫星上下，也因为提前进行了时间更新而减小了组合时刻点的计算量；
(3)本发明提出的GNSS/INS紧耦合时间分散滤波方法耗时少，可以应用在GNSS卫星数多或IMU频率高的场景。
附图说明
图1为本发明的GNSS/INS紧耦合时间分散滤波方法处理流程图；
图2为本发明实施例中GNSS/MEMS紧耦合装置工作流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的实施方式作进一步详细描述。
本发明涉及一种GNSS/INS紧耦合时间分散滤波方法，通过分散传统Kalman滤波器的计算过程减小组合时刻点紧耦合的耗时。传统紧耦合滤波器在组合时刻点集中进行时间更新和测量更新，耗时较大，甚至超过INS的解算周期，影响INS力学编排的实时性。本发明将滤波器过程分散在各个时间段内，在组合时刻点之前提前进行时间更新和增益阵计算。而在组合时刻点只进行状态变量和状态协方差阵的修正，大大减小了组合时刻点的计算量。
本发明采用GNSS/INS紧耦合装置，本实施例中INS类型选为MEMS器件。如图1所示为本发明GNSS/INS紧耦合时间分散滤波方法处理流程图，本发明GNSS/INS紧耦合时间分散滤波方法包括如下步骤：
一种GNSS/INS紧耦合时间分散滤波方法，其特征在于：将组合Kalman滤波器的过程分解为时间更新、增益阵计算和状态变量修正，分别在不同时间段内计算，避免集中计算引起的耗时较大问题；具体的分散滤波方法包括如下步骤：
(1)、提前2个IMU周期进行时间更新
在组合时刻点之前2个IMU周期内进行Kalman滤波器时间更新的具体方法如下：
(1a)、上次组合后，以IMU周期累积系统矩阵，设紧耦合滤波器的连续状态方程为
其中，X为状态向量，包括INS的姿态、位置和速度误差，IMU的零偏和GNSS的钟差、钟漂；F为系统矩阵，包括INS的姿态和IMU的输出；G为噪声矩阵；W为驱动噪声，其协方差阵为q，
在每个IMU周期，完成INS力学编排后累积系统矩阵，
其中，Fs为累积的系统矩阵；Fi为第i次力学编排确定的系统矩阵；N为组合周期内INS力学编排的总次数；
(1b)、提前两个IMU周期对滤波器的连续状态方程进行离散化，为避免在下次组合时刻点的集中计算，提前两个IMU周期进行时间更新，完成第N-2次INS力学编排后，对组合时刻前两个的系统矩阵进行近似，即
FN-1＝FN-2,FN＝FN-2
上式假设了在组合前的两个IMU周期内载体保持姿态和IMU输出不变，最后得到完整的累积矩阵
设离散化后的滤波器状态方程为
Xk＝Φk,k-1Xk-1+Wk
式中，k代表系统的第k个采样时刻；Φk,k-1为一步状态转移矩阵；Wk为系统噪声，其协方差阵为Qk，
Φk,k-1的计算公式为
Φk,k-1＝I+Fs*TIMU
其中，I为单位矩阵，TIMU为IMU周期，
当连续噪声协方差阵q为对角阵时，则Q＝GqGT亦为对角阵，则离散噪声协方差阵的计算公式近似为
Qk＝Q*TTC
其中，Q＝GqGT；TTC为紧组合周期；
(1c)、进行滤波器的时间更新，时间更新的公式为
Xk,k-1＝Φk,k-1Xk-1
式中，Pk-1为k-1时刻状态Xk-1的协方差矩阵；Pk,k-1为k时刻预报状态Xk,k-1的协方差矩阵。
进行时间更新的同时，将累积系统矩阵Fs清零，以开始下一次组合。
(2)、提前1个IMU周期计算H阵和K阵
在组合时刻点之前1个IMU周期内计算Kalman滤波器观测矩阵H和增益矩阵K的具体方法如下：
(2a)、假设上次组合后GNSS可用卫星不变，根据星历和下次组合时刻提前计算GNSS卫星的位置；
(2b)、提前1个IMU周期计算观测矩阵Hk，H阵的表达形式与滤波器选取的观测量类型有关,，观测量选为GNSS和INS的伪距和伪距率之差，此时，H阵由GNSS卫星与载体之间的方向矢量构成。方向矢量的表达式为
其中，rs为步骤(2a)中获得的卫星位置；ru为当前时刻载体的位置；
(2c)、提前1个IMU周期计算观测噪声矩阵Rk，获取H阵后，计算R阵；
(2d)、提前1个IMU周期计算观测噪声矩阵Kk，获取H和R阵后，即可按下式计算K阵，
(3)、检查是否需要重构H阵和K阵
在组合时刻点检查是否需要重构H阵和K阵的具体方法如下：
(3a)、比较当前时刻与上次组合时刻的GNSS可用卫星，检查是否存在新上星或下星情况；
(3b)、若两个时刻可用卫星完全相同，则无需重构；否则，按步骤(2)方法重新计算H阵和K阵。
(4)、计算观测向量Z
在组合时刻点计算Kalman滤波器观测向量Z的具体方法如下：
(4a)、计算GNSS与INS的伪距之差，伪距差观测量为GNSS测量伪距与INS构造的伪距之差，具体表达式如下
Δρ＝(||rs-ru||-dts+Ion+Trp+Rot)-ρGNSS
其中，dts为卫星钟差；Ion为卫星电离层延迟修正项；Trp为卫星对流层延迟修正项；Rot为地球自转效应修正项；ρGNSS为GNSS伪距测量值；
(4b)、计算GNSS与INS的伪距率之差，伪距率差观测量为GNSS测量伪距率与INS构造的伪距率之差，具体表达式如下
其中，vs为卫星的速度；vu为当前时刻载体的速度；dfs为卫星钟漂；为GNSS伪距率测量值。
(5)、修正时间更新后的状态向量和协方差阵
组合时刻点对时间更新后状态变量和状态协方差阵进行修正的具体方法如下：
(5a)、对状态变量进行修正。修正公式为
Xk＝Xk,k-1+Kk(Zk-HkXk,k-1)
(5b)、对状态协方差阵进行修正。修正公式为
Pk＝(I-KkHk)Pk,k-1。
实施例：
下面以GNSS/MEMS紧耦合装置为例结合附图对本发明的具体实施方式展开说明，对于采用其他类型惯导的紧耦合装置，本发明提供的方法仍然适用。
GNSS/MEMS紧耦合装置工作流程如图2所示。GNSS输出伪距、伪距率数据的频率为1Hz，MEMS输出IMU数据的频率为200Hz，紧耦合频率为1Hz。初始对准模块101为INS力学编排模块102提供初始的位置、速度和姿态，INS力学编排模块102利用采集到的IMU数据103实时计算INS的位置、速度和姿态。在组合时刻点tTC前2个IMU周期即10ms执行时间更新模块104，tTC前1个IMU周期即5ms执行计算观测阵和增益阵模块105。在组合时刻点即GNSS测量数据有效时，是否需要重构模块106检查当前GNSS可用卫星与上一次组合时的可用卫星是否相同，若不相同则重构观测阵和增益阵。计算观测向量Z模块107利用GNSS测量数据108以及卫星和INS的位置、速度构造伪距和伪距率之差。修正状态量模块109对状态变量和状态协方差阵进行测量更新。
以上所述，仅为本发明一个具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。

Claims (5)

1.一种GNSS/INS紧耦合时间分散滤波方法，其特征在于：将组合Kalman滤波器的过程分解为时间更新、增益阵计算和状态变量修正，分别在不同时间段内计算，避免集中计算引起的耗时较大问题；具体的分散滤波方法包括如下步骤：
步骤(1)、在组合时刻点之前2个IMU周期内进行Kalman滤波器的时间更新；
步骤(2)、在组合时刻点之前1个IMU周期内计算Kalman滤波器的观测矩阵H和增益矩阵K；
步骤(3)、在组合时刻点，检查是否需要重构H阵和K阵，若需要则重新计算H阵和K阵；
步骤(4)、在组合时刻点，计算Kalman滤波器的观测向量Z；
步骤(5)、在组合时刻点，对时间更新后的状态变量和状态协方差阵进行修正；
所述步骤(3)中在组合时刻点检查是否需要重构H阵和K阵的具体方法如下：
(3a)、比较当前时刻与上次组合时刻的GNSS可用卫星，检查是否存在新上星或下星情况；
(3b)、若两个时刻可用卫星完全相同，则无需重构；否则，按步骤(2)方法重新计算H阵和K阵。
2.根据权利要求1所述的一种GNSS/INS紧耦合时间分散滤波方法，其特征在于：所述步骤(1)中在组合时刻点之前2个IMU周期内进行Kalman滤波器时间更新的具体方法如下：
(1a)、上次组合后，以IMU周期累积系统矩阵，设紧耦合滤波器的连续状态方程为
其中，X为状态向量，包括INS的姿态、位置和速度误差，IMU的零偏和GNSS的钟差、钟漂；F为系统矩阵，包括INS的姿态和IMU的输出；G为噪声矩阵；W为驱动噪声，其协方差阵为q，
在每个IMU周期，完成INS力学编排后累积系统矩阵，
其中，Fs为累积的系统矩阵；Fi为第i次力学编排确定的系统矩阵；N为组合周期内INS力学编排的总次数；
(1b)、提前两个IMU周期对滤波器的连续状态方程进行离散化，为避免在下次组合时刻点的集中计算，提前两个IMU周期进行时间更新，完成第N-2次INS力学编排后，对组合时刻前两个的系统矩阵进行近似，即
FN-1＝FN-2,FN＝FN-2
上式假设了在组合前的两个IMU周期内载体保持姿态和IMU输出不变，最后得到完整的累积矩阵
设离散化后的滤波器状态方程为
Xk＝φk,k-1Xk-1+Wk
式中，k代表系统的第k个采样时刻；φk,k-1为一步状态转移矩阵；Wk为系统噪声，其协方差阵为Qk，
Φk,k-1的计算公式为
Φk,k-1＝I+Fs*TIMU
其中，I为单位矩阵，TIMU为IMU周期，
当连续噪声协方差阵q为对角阵时，则Q＝GqGT亦为对角阵，则离散噪声协方差阵的计算公式近似为
Qk＝Q*TTC
其中，Q＝GqGT；TTC为紧组合周期；
(1c)、进行滤波器的时间更新，时间更新的公式为
Xk,k-1＝Φk,k-1Xk-1
式中，Pk-1为k-1时刻状态Xk-1的协方差矩阵；Pk,k-1为k时刻预报状态Xk,k-1的协方差矩阵，
进行时间更新的同时，将累积系统矩阵Fs清零，以开始下一次组合。
3.根据权利要求1所述的一种GNSS/INS紧耦合时间分散滤波方法，其特征在于：所述步骤(2)中在组合时刻点之前1个IMU周期内计算Kalman滤波器观测矩阵H和增益矩阵K的具体方法如下：
(2a)、假设上次组合后GNSS可用卫星不变，根据星历和下次组合时刻提前计算GNSS卫星的位置；
(2b)、提前1个IMU周期计算观测矩阵Hk，H阵的表达形式与滤波器选取的观测量类型有关，观测量选为GNSS和INS的伪距和伪距率之差，此时，H阵由GNSS卫星与载体之间的方向矢量构成，方向矢量的表达式为
其中，rs为步骤(2a)中获得的卫星位置；ru为当前时刻载体的位置；
(2c)、提前1个IMU周期计算观测噪声矩阵Rk，获取H阵后，计算R阵；
(2d)、提前1个IMU周期计算观测噪声矩阵Kk，获取H和R阵后，即可按下式计算K阵，
4.根据权利要求1所述的一种GNSS/INS紧耦合时间分散滤波方法，其特征在于：所述步骤(4)中在组合时刻点计算Kalman滤波器观测向量Z的具体方法如下：
(4a)、计算GNSS与INS的伪距之差,伪距差观测量为GNSS测量伪距与INS构造的伪距之差，具体表达式如下
Δρ＝(||rs-ru||-dts+Ion+Trp+Rot)-ρGNSS
其中，dts为卫星钟差；Ion为卫星电离层延迟修正项；Trp为卫星对流层延迟修正项；Rot为地球自转效应修正项；ρGNSS为GNSS伪距测量值；
(4b)、计算GNSS与INS的伪距率之差,伪距率差观测量为GNSS测量伪距率与INS构造的伪距率之差，具体表达式如下
其中，vs为卫星的速度；vu为当前时刻载体的速度；dfs为卫星钟漂；为GNSS伪距率测量值。
5.根据权利要求1所述的一种GNSS/INS紧耦合时间分散滤波方法，其特征在于：所述步骤(5)中在组合时刻点对时间更新后状态变量和状态协方差阵进行修正的具体方法如下：
(5a)、对状态变量进行修正，修正公式为
Xk＝Xk,k-1+Kk(Zk-HkXk,k-1)
(5b)、对状态协方差阵进行修正，修正公式为
Pk＝(I-KkHk)Pk,k-1。
CN202011145864.4A 2020-10-23 2020-10-23 一种gnss/ins紧耦合时间分散滤波方法 Active CN112269201B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011145864.4A CN112269201B (zh) 2020-10-23 2020-10-23 一种gnss/ins紧耦合时间分散滤波方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011145864.4A CN112269201B (zh) 2020-10-23 2020-10-23 一种gnss/ins紧耦合时间分散滤波方法

Publications (2)

Publication Number Publication Date
CN112269201A CN112269201A (zh) 2021-01-26
CN112269201B true CN112269201B (zh) 2024-04-16

Family

ID=74341674

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011145864.4A Active CN112269201B (zh) 2020-10-23 2020-10-23 一种gnss/ins紧耦合时间分散滤波方法

Country Status (1)

Country Link
CN (1) CN112269201B (zh)

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102998685A (zh) * 2011-09-15 2013-03-27 北京自动化控制设备研究所 一种惯性/卫星导航系统伪距/伪距率紧组合方法
CN104181572A (zh) * 2014-05-22 2014-12-03 南京理工大学 一种弹载惯性/卫星紧组合导航方法
CN104316947A (zh) * 2014-08-26 2015-01-28 南京航空航天大学 Gnss/ins超紧组合导航装置及相对导航系统
CN105954783A (zh) * 2016-04-26 2016-09-21 武汉大学 一种改进gnss/ins实时紧组合导航实时性能的方法
WO2018014602A1 (zh) * 2016-07-19 2018-01-25 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
WO2018059532A1 (zh) * 2016-09-30 2018-04-05 华为技术有限公司 观测时滞系统的组合导航数据解算方法、装置及导航设备
JP2018060326A (ja) * 2016-10-04 2018-04-12 株式会社豊田中央研究所 トラッキング装置及びプログラム
CN108535755A (zh) * 2018-01-17 2018-09-14 南昌大学 基于mems的gnss/imu车载实时组合导航方法
CN109443355A (zh) * 2018-12-25 2019-03-08 中北大学 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法
CN109631892A (zh) * 2019-01-22 2019-04-16 武汉大学 一种imu数据中断的组合导航数据处理方法
CN109931957A (zh) * 2019-03-24 2019-06-25 北京工业大学 基于lgmkf的sins捷联惯性导航系统自对准方法
CN109945860A (zh) * 2019-05-07 2019-06-28 深圳市联和安业科技有限公司 一种基于卫星紧组合的ins和dr惯性导航方法与系统
CN111156987A (zh) * 2019-12-18 2020-05-15 东南大学 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN111175795A (zh) * 2020-01-03 2020-05-19 暨南大学 Gnss/ins组合导航系统的两步抗差滤波方法及系统

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102998685A (zh) * 2011-09-15 2013-03-27 北京自动化控制设备研究所 一种惯性/卫星导航系统伪距/伪距率紧组合方法
CN104181572A (zh) * 2014-05-22 2014-12-03 南京理工大学 一种弹载惯性/卫星紧组合导航方法
CN104316947A (zh) * 2014-08-26 2015-01-28 南京航空航天大学 Gnss/ins超紧组合导航装置及相对导航系统
CN105954783A (zh) * 2016-04-26 2016-09-21 武汉大学 一种改进gnss/ins实时紧组合导航实时性能的方法
WO2018014602A1 (zh) * 2016-07-19 2018-01-25 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
WO2018059532A1 (zh) * 2016-09-30 2018-04-05 华为技术有限公司 观测时滞系统的组合导航数据解算方法、装置及导航设备
JP2018060326A (ja) * 2016-10-04 2018-04-12 株式会社豊田中央研究所 トラッキング装置及びプログラム
CN108535755A (zh) * 2018-01-17 2018-09-14 南昌大学 基于mems的gnss/imu车载实时组合导航方法
CN109443355A (zh) * 2018-12-25 2019-03-08 中北大学 基于自适应高斯pf的视觉-惯性紧耦合组合导航方法
CN109631892A (zh) * 2019-01-22 2019-04-16 武汉大学 一种imu数据中断的组合导航数据处理方法
CN109931957A (zh) * 2019-03-24 2019-06-25 北京工业大学 基于lgmkf的sins捷联惯性导航系统自对准方法
CN109945860A (zh) * 2019-05-07 2019-06-28 深圳市联和安业科技有限公司 一种基于卫星紧组合的ins和dr惯性导航方法与系统
CN111156987A (zh) * 2019-12-18 2020-05-15 东南大学 基于残差补偿多速率ckf的惯性/天文组合导航方法
CN111175795A (zh) * 2020-01-03 2020-05-19 暨南大学 Gnss/ins组合导航系统的两步抗差滤波方法及系统

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
GNSS/INS紧组合算法实时性改进与嵌入式验证;章红平;常乐;汪宏晨;牛小骥;高周正;;东南大学学报(自然科学版)(04);全文 *
GNSS/INS组合导航滤波算法及可靠性分析;蒋晨;;测绘学报;20201015(10);全文 *
自适应Kalman滤波在光纤陀螺SINS/GNSS紧组合导航中的应用;王巍;向政;王国栋;;红外与激光工程(03);全文 *

Also Published As

Publication number Publication date
CN112269201A (zh) 2021-01-26

Similar Documents

Publication Publication Date Title
CN108535755B (zh) 基于mems的gnss/imu车载实时组合导航方法
JP5270184B2 (ja) 衛星航法／推測航法統合測位装置
US6859727B2 (en) Attitude change kalman filter measurement apparatus and method
US9488480B2 (en) Method and apparatus for improved navigation of a moving platform
US6088653A (en) Attitude determination method and system
US8560234B2 (en) System and method of navigation based on state estimation using a stepped filter
CN113359170B (zh) 一种惯导辅助北斗单频动对动高精度相对定位方法
CN112378400A (zh) 一种双天线gnss辅助的捷联惯导组合导航方法
CN108120994B (zh) 一种基于星载gnss的geo卫星实时定轨方法
CN109459776B (zh) 基于gnss信号非连续跟踪的gnss/ins深组合导航方法
CA3003298A1 (en) Gnss and inertial navigation system utilizing relative yaw as an observable for an ins filter
CN110308467B (zh) 一种基于Zynq-7020的超紧耦合微系统及方法
CA2359910A1 (en) A deeply-integrated adaptive ins/gps navigator with extended-range code tracking
CN108344415A (zh) 一种组合导航信息融合方法
CN111044075B (zh) 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法
KR100443550B1 (ko) 오차보정시스템을 구비하는 관성측정유닛-지피에스통합시스템과 미지정수 검색범위 축소방법 및 사이클 슬립검출방법, 및 그를 이용한 항체 위치, 속도,자세측정방법
CN113203418A (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
EP2193336B1 (en) A positioning system and method
JP2009222438A (ja) 移動体用測位装置
JP2008039691A (ja) 搬送波位相式測位装置
Pan et al. A variational Bayesian-based robust adaptive filtering for precise point positioning using undifferenced and uncombined observations
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
Han et al. Land vehicle navigation with the integration of GPS and reduced INS: performance improvement with velocity aiding
CN110988932B (zh) 一种提高星载gps接收机实时钟差解算精度的方法
CN112269201B (zh) 一种gnss/ins紧耦合时间分散滤波方法

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