CN106291645B - 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 - Google Patents

适于高维gnss/ins深耦合的容积卡尔曼滤波方法 Download PDF

Info

Publication number
CN106291645B
CN106291645B CN201610575270.4A CN201610575270A CN106291645B CN 106291645 B CN106291645 B CN 106291645B CN 201610575270 A CN201610575270 A CN 201610575270A CN 106291645 B CN106291645 B CN 106291645B
Authority
CN
China
Prior art keywords
ins
gnss
moment
error
matrix
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
CN201610575270.4A
Other languages
English (en)
Other versions
CN106291645A (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.)
Southeast University
Original Assignee
Southeast University
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 Southeast University filed Critical Southeast University
Priority to CN201610575270.4A priority Critical patent/CN106291645B/zh
Publication of CN106291645A publication Critical patent/CN106291645A/zh
Priority to PCT/CN2017/080383 priority patent/WO2018014602A1/zh
Priority to US16/094,474 priority patent/US10890667B2/en
Application granted granted Critical
Publication of CN106291645B publication Critical patent/CN106291645B/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
    • 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/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/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering

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)
  • Gyroscopes (AREA)

Abstract

本发明公开了一种适于高维GNSS/INS深耦合的容积卡尔曼滤波方法，包括：S1、构造高维GNSS/INS深耦合滤波模型；S2、对构造的滤波模型采用标准容积规则产生初始化容积点；S3、采用新型容积点更新规则进行CKF滤波。本发明适于高维GNSS/INS深耦合滤波，且精度高，稳定性高。

Description

适于高维GNSS/INS深耦合的容积卡尔曼滤波方法
技术领域
本发明涉及滤波技术，尤其涉及一种适于高维GNSS/INS深耦合的容积卡尔曼滤波方法。
背景技术
非线性滤波是组合导航和目标跟踪领域中常用信息融合技术。不同于目标跟踪模型中显著的非线性，组合导航中的非线性误差一般较弱，然而受滤波器结构、传感器采样率、量测质量以及滤波更新率等因素影响其非线性表现出较强的时变特性。特别是当***初始误差较大、量测噪声较大以及***载体大机动的情况下，基于卡尔曼滤波(KF)的非线性状态估计常常难以稳定工作。扩展卡尔曼滤波器(EKF)在工程领域应用较为广泛，其基于雅克比矩阵解决***的非线性问题，状态估计精度可达到泰勒级数展开的一阶水平，在载体静止或者低动态情况下获得了良好的效果，但是对强非线性***估计精度较差。以无迹卡尔曼滤波(UKF)为代表的采样点逼近滤波方法一经提出就获得迅速应用，其以多点采样后雅克比矩阵的期望值代替EKF中单点的雅克比矩阵，对非线性引起的不确定性具有更好的平滑效果。容积卡尔曼滤波(CKF)较UKF在高维***状态估计中具有更好的精度和稳定性，虽然其均值精度在非线性的高斯噪声模型可以达到泰勒级数三阶，其方差的估计精度只在泰勒级数一阶水平。UKF采用改进的UT采样方法处理高维滤波中采样点的non-localsampling问题，提高了方差估计的精度，由于CKF采用对称采样策略其无法采用中心点权值捕获方差的高阶信息。
状态误差方差估计的精度问题和一致性问题在高维的组合导航***中更为突出。比如工程上GNSS/INS松组合一般采用15维，紧组合一般采用17或者更高，而深耦合中由于需要确保INS输出量对GNSS跟踪环路的辅助效果，需要更多的惯性器件状态误差，同时为了消除辅助信息与量测值的相关性误差，需要增加码跟踪环路和载波跟踪换路的状态误差量到组合滤波器的状态量中。以常规的4颗可见卫星为例其增加的跟踪环路的状态量维数为12维，因此现有的非线性滤波方法难以适用高维GNSS/INS深耦合状态估计问题。
发明内容
发明目的：本发明针对现有技术存在的问题，提供一种适于高维GNSS/INS深耦合的容积卡尔曼滤波方法。
技术方案：本发明所述的适于高维GNSS/INS深耦合的容积卡尔曼滤波方法，包括：
S1、构造高维GNSS/INS深耦合滤波模型；
S2、对构造的滤波模型采用标准容积规则产生初始化容积点；
S3、采用新型容积点更新规则进行CKF滤波。
进一步的，所述步骤S1包括：
S11、分别设置INS和GNSS子***的INS状态量为xI、GNSS状态量为xG，其中，
xI＝[δP δV ψ ka ba kω bω]
xG＝[bc dc δρdll δφpll δfpll]
式中，[δP δV ψ ka ba kω bω]分别依次为3维位置误差、速度误差、姿态误差、加速度计系数误差、加速度计系数零偏、陀螺仪系数误差与常值漂移；[bc dc δρdll δφpll δfpll]分别依次为接收机时钟偏置、时钟漂移、码跟踪环路的鉴相伪距误差、载波跟踪环路的相位误差和载波跟踪环路的频率误差；
S12、根据状态量xI、xG建立GNSS/INS深耦合的***模型为：
式中，FN(t)为9维惯导基本***矩阵和Ο3×9，FM(t)＝[Ο12×12]，Οi×j为i行j列的零矩阵，Ca和Cω分别为加速度计和陀螺的系数误差矩阵，为t时刻的姿态矩阵，T1、T2为环路滤波器的参数， 分别为xI和xG的微分项，G为***噪声驱动矩阵，WI和WG为INS和GNSS以及跟踪环路的***噪声，Tr为相关时间，Kpll为载波环增益，Ii×j为i行j列的单位阵，为卫星与接收机的径向位置矢量矩阵，W为INS解算位置误差引起的卫星与接收机径向速度误差，为导航坐标系到地心地固坐标系的转换矩阵，Kdll为码环增益。
S13、根据GNSS/INS深耦合的***模型建立GNSS/INS深耦合的量测模型为：
δρ＝ρI-ρG＝δρI-(bc+νρ+δρdll)
式中，δρ、分别为卫星通道的伪距、伪距率观测量，ρI、分别为INS预测的伪距、伪距率，ρG、分别为GNSS量测伪距、伪距率，bc、dc分别为接收机时钟偏置和漂移，νρ、分别为伪距、伪距率观测噪声，δρdll、分别为跟踪环路的伪距误差和伪距率误差，ν为量测噪声矩阵，即有 fL1为GPS L1载波频率，c为光速。
进一步的，所述步骤S2包括：
S21、k-1时刻采用高精度非线性滤波方法计算得到k-1时刻的状态后验分布
其中， 表示k-1时刻状态后验分布，表示由k-1时刻的量测推测出的k-1时刻的状态分布，Pk‐1|k‐1表示的协方差，x～N(μ,σ2)表示x服从均值为μ方差为σ2的高斯分布；
S22、采用标准容积规则产生***状态方程f(x)预测过程所需要的容积点；
其中，
式中，x为GNSS/INS深耦合滤波模型的状态量，其维数为nx，为nx维单位方阵，表示由k-1时刻的量测推测出的k-1时刻的第i个容积点，ξi为CKF滤波中第i个标准容积点，i＝1,…,2nx，Sk-1|k-1满足Pk‐1|k‐1＝Sk-1|k-1(Sk-1|k-1)T。
进一步的，所述高精度非线性滤波方法包括：
设置度非线性滤波迭代的量测方程为
其中为k时刻第j次量测迭代的状态估计值，zk为k时刻的多卫星通道器鉴相器输出的量测值，h(·)为深耦合跟踪环路的量测方程，为当前迭代中后验估计值与先验估计值的误差，Kj为第j次迭代的卡尔曼增益，αj为加速收敛设置的迭代步长控制系数，满足0＜αj≤1，为Pk|k-1的逆，Pxz,k|k-1为预测过程中产生的互协方差，状态后验估计误差的协方差矩阵取最后一次迭代计算的结果。
进一步的，所述步骤S3包括：
S31、采用下列公式计算得到k时刻状态先验分布：
式中，表示k时刻状态先验估计，为其均值，Pk|k-1为其方差，表示由k-1时刻的量测和状态推测出的k时刻的状态估计，Pk|k-1表示的协方差，wi＝1/2nx，Qk-1为***噪声方差阵；
S32、采用下式计算得到预测过程的容积点误差矩阵并定义为先验PDF逼近过程Sigma点统计线性回归(SLR)的误差方差；
其中，为经***方程传播后的容积点；
S33、将经***方程传播后的容积点作为量测更新过程的容积点；
S34、采用CKF量测更新计算量测值的似然分布函数；
其中，
式中，其中表示k时刻量测似然估计，为其均值，Pzz,k|k-1为其方差，为由k-1时刻的状态预测到的k时刻的量测，h(x)为量测方程，wi＝1/2nx，Rk为量测噪声方差阵；
S35、计算状态量x的后验分布函数；
其中，
式中，为k时刻的状态量的后验估计，其均值和方差分别为和Pk|k，Kk＝Pxz,k|k-1(Pzz,k|k-1)‐1为卡尔曼增益矩阵，Pxz,k|k-1为状态量的后验估计值与量测似然估计值的互协方差；
S36、定义Sigma点逼近后验分布产生的误差为为CKF容积点SLR的权值，k时刻的先验分布的SLR应精确捕获状态的均值和协方差，且考虑***不确定性和噪声的影响，则有
式中，为预测过程容积点误差阵，Σ＝diag(w)表示采用w对角线元素构造该矩阵Σ，类似的后验分布的SLR中，容积点至少能精确匹配到状态的均值和方差即
式中，为更新后的容积点；
S37、设和均为对称正定矩阵，且存在则有 其中B为待求解的转换矩阵，为更新后的容积点误差阵；进而有B＝Lk+1Γ(Lk)-1，其中Γ为任意正交矩阵满足取Γ为单位阵时则得到B＝Lk+1(Lk)-1；
S38、基于后验的状态估计均值和更新后的容积点误差阵得到更新的容积点为0≤i≤2nx。
进一步的，所述的GNSS码跟踪鉴相伪距误差ρdll方程为
其中δρdll为码环环路滤波器的输出量，为ρdll的微分，Kdll为码环增益，δVaid为滤波输出校正后INS提供给码跟踪环路的速度辅助，Q为环路热噪声和干扰引起的***噪声。
进一步的，所述的载波环路误差方程为
其中，φpll为载波环路相位误差，fpll为载波环路频率误差，Kpll为环路增益，δfaid为滤波输出校正后INS提供给载波跟踪环路的多普勒频率误差辅助，T1、T2为环路滤波器的参数，即环路低通滤波器的传递函数为
有益效果：本发明与现有技术相比，其显著优点是：
(1)采用更高精度的非线性滤波方法初始化生成容积点；
(2)基于矩阵变换更新容积点改善了状态估计过程中预测过程方差的一致性，提高滤波器的稳定性；
(3)将跟踪环路的状态误差参数作为组合滤波器的状态参数消除滤波器量测与跟踪环路误差的相关性，提高INS辅助量校正的精度和稳定性；
(4)采用两种策略生成CKF的容积点，即初始化时采用耗时的、高精度的方法生成容积点，后续的容积点更新过程基于前一时刻状态估计误差的方差实现，提高了容积点的利用率。
附图说明
图1是本发明的一个实施例的***框图；
图2是高维GNSS/INS深耦合滤波模型的示意图。
具体实施方式
如图1所示，本实施例的适于高维GNSS/INS深耦合的容积卡尔曼滤波方法包括以下步骤：
S1、构造高维GNSS/INS深耦合滤波模型，如图2所示。
步骤S1具体包括：
S11、分别设置INS和GNSS子***的INS状态量为xI、GNSS状态量为xG，其中，
xI＝[δP δV ψ ka ba kω bω]
xG＝[bc dc δρdll δφpll δfpll]
式中，[δP δV ψ ka ba kω bω]分别依次为3维位置误差、速度误差、姿态误差、加速度计系数误差、加速度计系数零偏、陀螺仪系数误差与常值漂移；[bc dc δρdll δφpll δfpll]分别依次为接收机时钟偏置、时钟漂移、码跟踪环路的鉴相伪距误差、载波跟踪环路的相位误差和载波跟踪环路的频率误差。
其中，所述的GNSS码跟踪鉴相伪距误差ρdll方程为
其中δρdll为码环环路滤波器的输出量，为ρdll的微分，Kdll为码环增益，δVaid为滤波输出校正后INS提供给码跟踪环路的速度辅助，Q为环路热噪声和干扰引起的***噪声。
其中，所述的载波环路误差方程为
其中，φpll为载波环路相位误差，fpll为载波环路频率误差，Kpll为环路增益，δfaid为滤波输出校正后INS提供给载波跟踪环路的多普勒频率误差辅助，T1、T2为环路滤波器的参数，即环路低通滤波器的传递函数为
S12、根据状态量xI、xG建立GNSS/INS深耦合的***模型为：
式中，FN(t)为9维惯导基本***矩阵和Ο3×9，FM(t)＝[Ο12×12]，Οi×j为i行j列的零矩阵，Ca和Cω分别为加速度计和陀螺的系数误差矩阵，为t时刻的姿态矩阵，T1、T2为环路滤波器的参数， 分别为xI和xG的微分项，G为***噪声驱动矩阵，WI和WG为INS和GNSS以及跟踪环路的***噪声，Tr为相关时间，Kpll为载波环增益，Ii×j为i行j列的单位阵，为卫星与接收机的径向位置矢量矩阵，W为INS解算位置误差引起的卫星与接收机径向速度误差，为导航坐标系到地心地固坐标系的转换矩阵，Kdll为码环增益。
S13、根据GNSS/INS深耦合的***模型建立GNSS/INS深耦合的量测模型为：
δρ＝ρI-ρG＝δρI-(bc+νρ+δρdll)
式中，δρ、分别为卫星通道的伪距、伪距率观测量，ρI、分别为INS预测的伪距、伪距率，ρG、分别为GNSS量测伪距、伪距率，bc、dc分别为接收机时钟偏置和漂移，νρ、分别为伪距、伪距率观测噪声，δρdll、分别为跟踪环路的伪距误差和伪距率误差，ν为量测噪声矩阵，即有 fL1为GPS L1载波频率，c为光速。
S2、对构造的滤波模型采用标准容积规则产生初始化容积点。
步骤S2具体包括：
S21、k-1时刻采用高精度非线性滤波方法计算得到k-1时刻的状态后验分布
其中， 表示k-1时刻状态后验分布，表示由k-1时刻的量测推测出的k-1时刻的状态分布，Pk‐1|k‐1表示的协方差，x～N(μ,σ2)表示x服从均值为μ方差为σ2的高斯分布。
其中，所述高精度非线性滤波方法包括：设置度非线性滤波迭代的量测方程为其中为k时刻第j次量测迭代的状态估计值，zk为k时刻的多卫星通道器鉴相器输出的量测值，h(·)为深耦合跟踪环路的量测方程，为当前迭代中后验估计值与先验估计值的误差，Kj为第j次迭代的卡尔曼增益，αj为加速收敛设置的迭代步长控制系数，满足0＜αj≤1，为Pk|k-1的逆，Pxz,k|k-1为预测过程中产生的互协方差，状态后验估计误差的协方差矩阵取最后一次迭代计算的结果。
S22、采用标准容积规则产生***状态方程f(x)预测过程所需要的容积点；
其中，
式中，x为GNSS/INS深耦合滤波模型的状态量，其维数为nx，为nx维单位方阵，表示由k-1时刻的量测推测出的k-1时刻的第i个容积点，ξi为CKF滤波中第i个标准容积点，i＝1,…,2nx，Sk-1|k-1满足Pk‐1|k‐1＝Sk-1|k-1(Sk-1|k-1)T。
S3、采用新型容积点更新规则进行CKF滤波。
步骤S3包括：
S31、采用下列公式计算得到k时刻状态先验分布：
式中，表示k时刻状态先验估计，为其均值，Pk|k-1为其方差，表示由k-1时刻的量测和状态推测出的k时刻的状态估计，Pk|k-1表示的协方差，wi＝1/2nx，Qk-1为***噪声方差阵；
S32、采用下式计算得到预测过程的容积点误差矩阵并定义为先验PDF逼近过程Sigma点统计线性回归的误差方差；
其中，经***方程传播后的容积点；
S33、将经***方程传播后的容积点作为量测更新过程的容积点；
S34、采用CKF量测更新计算量测值的似然分布函数；
其中，
式中，其中zk-表示k时刻量测似然估计，为其均值，Pzz,k|k-1为其方差，为由k-1时刻的状态预测到的k时刻的量测，h(x)为量测方程，wi＝1/2nx，Rk为量测噪声方差阵；
S35、计算状态量x的后验分布函数；
其中，
式中，为k时刻的状态量的后验估计，其均值和方差分别为和Pk|k，Kk＝Pxz,k|k-1(Pzz,k|k-1)‐1为卡尔曼增益矩阵，Pxz,k|k-1为状态量的后验估计值与量测似然估计值的互协方差；
S36、定义Sigma点逼近后验分布产生的误差为 为CKF容积点SLR的权值，k时刻的先验分布的SLR应精确捕获状态的均值和协方差，且考虑***不确定性和噪声的影响，则有
式中，为预测过程容积点误差阵，Σ＝diag(w)表示采用w对角线元素构造该矩阵Σ，类似的后验分布的SLR中，容积点至少能精确匹配到状态的均值和方差即
式中，为更新后的容积点；
S37、设和均为对称正定矩阵，且存在则有 其中B为待求解的转换矩阵，为更新后的容积点误差阵；进而有B＝Lk+1Γ(Lk)-1，其中Γ为任意正交矩阵满足取Γ为单位阵时则得到B＝Lk+1(Lk)-1；
S38、基于后验的状态估计均值和更新后的容积点误差阵得到更新的容积点为
以上所揭露的仅为本发明一种较佳实施例而已，不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (6)

1.一种适于高维GNSS/INS深耦合的容积卡尔曼滤波方法，其特征在于该方法包括：
S1、构造高维GNSS/INS深耦合滤波模型；
S2、对构造的滤波模型采用标准容积规则产生初始化容积点；
S3、采用新型容积点更新规则进行CKF滤波；具体包括以及下步骤：
S31、采用下列公式计算得到k时刻状态先验分布：
式中，表示k时刻状态先验估计，为其均值，Pk|k-1为其方差，表示由k-1时刻的量测和状态推测出的k时刻的状态估计，Pk|k-1表示的协方差，wi＝1/2nx，Qk-1为***噪声方差阵；
S32、采用下式计算得到预测过程的容积点误差矩阵并定义Ξk-＝Pk|k-1-Qk‐1为先验PDF逼近过程Sigma点统计线性回归的误差方差；
其中，为经***方程传播后的容积点；
S33、将经***方程传播后的容积点作为量测更新过程的容积点；
S34、采用CKF量测更新计算量测值的似然分布函数；
其中，
式中，其中表示k时刻量测似然估计，为其均值，Pzz,k|k-1为其方差，为由k-1时刻的状态预测到的k时刻的量测，h(x)为量测方程，wi＝1/2nx，Rk为量测噪声方差阵；
S35、计算状态量x的后验分布函数；
其中，
式中，为k时刻的状态量的后验估计，其均值和方差分别为和Pk|k，Kk＝Pxz,k|k-1(Pzz,k|k-1)‐1为卡尔曼增益矩阵，Pxz,k|k-1为状态量的后验估计值与量测似然估计值的互协方差；
S36、定义Sigma点逼近后验分布产生的误差为为CKF容积点SLR的权值，k时刻的先验分布的SLR应精确捕获状态的均值和协方差，且考虑***不确定性和噪声的影响，则有
式中，为预测过程容积点误差阵，Σ＝diag(w)表示采用w对角线元素构造该矩阵Σ，类似的后验分布的SLR中，容积点至少能精确匹配到状态的均值和方差即
式中，为更新后的容积点；
S37、设和均为对称正定矩阵，且存在则有 其中B为待求解的转换矩阵，为更新后的容积点误差阵；进而有B＝Lk+1Γ(Lk)-1，其中Γ为任意正交矩阵满足取Γ为单位阵时则得到B＝Lk+1(Lk)-1；
S38、基于后验的状态估计均值和更新后的容积点误差阵得到更新的容积点为0≤i≤2nx。
2.根据权利要求1所述的高维GNSS/INS深耦合的容积卡尔曼滤波方法，其特征在于：所述步骤S1包括：
S11、分别设置INS和GNSS子***的INS状态量为xI、GNSS状态量为xG，其中，
xI＝[δP δV ψ ka ba kω bω]
xG＝[bc dc δρdll δφpll δfpll]
式中，[δP δV ψ ka ba kω bω]分别依次为3维位置误差、速度误差、姿态误差、加速度计系数误差、加速度计系数零偏、陀螺仪系数误差与常值漂移；[bc dc δρdll δφpll δfpll]分别依次为接收机时钟偏置、时钟漂移、码跟踪环路的鉴相伪距误差、载波跟踪环路的相位误差和载波跟踪环路的频率误差；
S12、根据状态量xI、xG建立GNSS/INS深耦合的***模型为：
式中，FN(t)为9维惯导基本***矩阵和Ο3×9，FM(t)＝[Ο12×12]，Οi×j为i行j列的零矩阵，Ca和Cω分别为加速度计和陀螺的系数误差矩阵，为t时刻的姿态矩阵，T1、T2为环路滤波器的参数， 分别为xI和xG的微分项，G为***噪声驱动矩阵，WI和WG为INS和GNSS以及跟踪环路的***噪声，Tr为相关时间，Kpll为载波环增益，Ii×j为i行j列的单位阵，为卫星与接收机的径向位置矢量矩阵，W为INS解算位置误差引起的卫星与接收机径向速度误差，为导航坐标系到地心地固坐标系的转换矩阵，Kdll为码环增益；
S13、根据GNSS/INS深耦合的***模型建立GNSS/INS深耦合的量测模型为：
δρ＝ρI-ρG＝δρI-(bc+νρ+δρdll)
式中，δρ、分别为卫星通道的伪距、伪距率观测量，ρI、分别为INS预测的伪距、伪距率，ρG、分别为GNSS量测伪距、伪距率，bc、dc分别为接收机时钟偏置和漂移，νρ、分别为伪距、伪距率观测噪声，δρdll、分别为跟踪环路的伪距误差和伪距率误差，ν为量测噪声矩阵，即有 fL1为GPS L1载波频率，c为光速。
3.根据权利要求1所述的高维GNSS/INS深耦合的容积卡尔曼滤波方法，其特征在于：所述步骤S2包括：
S21、k-1时刻采用高精度非线性滤波方法计算得到k-1时刻的状态后验分布
其中， 表示k-1时刻状态后验分布，表示由k-1时刻的量测推测出的k-1时刻的状态分布，Pk‐1|k‐1表示的协方差，x～N(μ,σ2)表示x服从均值为μ方差为σ2的高斯分布；
S22、采用标准容积规则产生***状态方程f(x)预测过程所需要的容积点；
其中，
式中，x为GNSS/INS深耦合滤波模型的状态量，其维数为nx，为nx维单位方阵，表示由k-1时刻的量测推测出的k-1时刻的第i个容积点，ξi为CKF滤波中第i个标准容积点，i＝1,…,2nx，Sk-1|k-1满足Pk‐1|k‐1＝Sk-1|k-1(Sk-1|k-1)T。
4.根据权利要求3所述的高维GNSS/INS深耦合的容积卡尔曼滤波方法，其特征在于：所述高精度非线性滤波方法包括：
设置度非线性滤波迭代的量测方程为
其中为k时刻第j次量测迭代的状态估计值，zk为k时刻的多卫星通道器鉴相器输出的量测值，h(·)为深耦合跟踪环路的量测方程，为当前迭代中后验估计值与先验估计值的误差，Kj为第j次迭代的卡尔曼增益，αj为加速收敛设置的迭代步长控制系数，满足0＜αj≤1，为Pk|k-1的逆，Pxz,k|k-1为预测过程中产生的互协方差，状态后验估计误差的协方差矩阵取最后一次迭代计算的结果。
5.根据权利要求2所述的高维GNSS/INS深耦合的容积卡尔曼滤波方法，其特征在于：所述的GNSS码跟踪鉴相伪距误差ρdll方程为
其中δρdll为码环环路滤波器的输出量，为ρdll的微分，Kdll为码环增益，δVaid为滤波输出校正后INS提供给码跟踪环路的速度辅助，Q为环路热噪声和干扰引起的***噪声。
6.根据权利要求2所述的高维GNSS/INS深耦合的容积卡尔曼滤波方法，其特征在于所述的载波环路误差方程为
其中，φpll为载波环路相位误差，fpll为载波环路频率误差，Kpll为环路增益，δfaid为滤波输出校正后INS提供给载波跟踪环路的多普勒频率误差辅助，T1、T2为环路滤波器的参数，即环路低通滤波器的传递函数为
CN201610575270.4A 2016-07-19 2016-07-19 适于高维gnss/ins深耦合的容积卡尔曼滤波方法 Active CN106291645B (zh)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN201610575270.4A CN106291645B (zh) 2016-07-19 2016-07-19 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
PCT/CN2017/080383 WO2018014602A1 (zh) 2016-07-19 2017-04-13 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
US16/094,474 US10890667B2 (en) 2016-07-19 2017-04-13 Cubature Kalman filtering method suitable for high-dimensional GNSS/INS deep coupling

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201610575270.4A CN106291645B (zh) 2016-07-19 2016-07-19 适于高维gnss/ins深耦合的容积卡尔曼滤波方法

Publications (2)

Publication Number Publication Date
CN106291645A CN106291645A (zh) 2017-01-04
CN106291645B true CN106291645B (zh) 2018-08-21

Family

ID=57651636

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201610575270.4A Active CN106291645B (zh) 2016-07-19 2016-07-19 适于高维gnss/ins深耦合的容积卡尔曼滤波方法

Country Status (3)

Country Link
US (1) US10890667B2 (zh)
CN (1) CN106291645B (zh)
WO (1) WO2018014602A1 (zh)

Families Citing this family (79)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106291645B (zh) * 2016-07-19 2018-08-21 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN106871905B (zh) * 2017-03-02 2020-02-11 哈尔滨工业大学 一种非理想条件下高斯滤波替代框架组合导航方法
CN107607977B (zh) * 2017-08-22 2020-12-08 哈尔滨工程大学 一种基于最小偏度单形采样的自适应ukf组合导航方法
CN107707220A (zh) * 2017-08-31 2018-02-16 东南大学 一种应用在gnss/ins中的改进型ckf方法
CN107643534B (zh) * 2017-09-11 2019-07-12 东南大学 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法
EP3460398A1 (en) * 2017-09-22 2019-03-27 Trimble Inc. Methods, apparatuses, and computer programs for estimating the heading of an axis of a rigid body
CN108332751B (zh) * 2018-01-08 2020-11-20 北京邮电大学 一种多源融合定位方法、装置、电子设备及存储介质
CN109341690B (zh) * 2018-09-25 2022-03-22 江苏大学 一种鲁棒高效的组合导航自适应数据融合方法
CN109343013B (zh) * 2018-11-20 2023-06-16 北京电子工程总体研究所 一种基于重启机制的空间配准方法和***
CN109508445B (zh) * 2019-01-14 2023-05-05 哈尔滨工程大学 一种带有色量测噪声和变分贝叶斯自适应卡尔曼滤波的目标跟踪方法
CN109724599B (zh) * 2019-03-12 2023-08-01 哈尔滨工程大学 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN110309481B (zh) * 2019-04-24 2023-05-02 武汉大学 农田土壤反应动力学过程模型建模方法
CN110018501A (zh) * 2019-05-09 2019-07-16 哈尔滨工程大学 一种基于***间随机模型在线估计调整的多模精密单点定位方法
CN110189421B (zh) * 2019-05-10 2022-03-25 山东大学 一种北斗gnss/dr组合导航出租车计程计时***及其运行方法
CN110289989B (zh) * 2019-05-27 2022-10-25 东南大学 一种基于容积卡尔曼滤波算法的分布式状态估计方法
CN110146076B (zh) * 2019-06-06 2023-04-18 哈尔滨工业大学（威海） 一种无逆矩阵自适应滤波的sins/dvl组合定位方法
CN110531393B (zh) * 2019-07-27 2022-12-13 金华航大北斗应用技术有限公司 基于自适应卡尔曼滤波的弱信号跟踪方法
CN110458944B (zh) * 2019-08-08 2023-04-07 西安工业大学 一种基于双视角Kinect关节点融合的人体骨架重建方法
CN110457863B (zh) * 2019-08-23 2021-02-19 江南大学 基于椭球收缩滤波的风力发电机桨距子***参数估计方法
CN110579740B (zh) * 2019-09-17 2023-03-31 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110632629B (zh) * 2019-09-17 2023-12-08 成都国星通信有限公司 一种适用于高动态非平稳***的自适应卡尔曼滤波方法
CN110567455B (zh) * 2019-09-25 2023-01-03 哈尔滨工程大学 一种求积更新容积卡尔曼滤波的紧组合导航方法
CN114514412B (zh) * 2019-10-08 2024-06-04 株式会社电装 误差估计装置、误差估计方法、存储介质
CN110675435B (zh) * 2019-10-08 2022-08-02 上海智驾汽车科技有限公司 基于卡尔曼滤波和χ2检测平滑处理的车辆轨迹跟踪方法
CN110633761B (zh) * 2019-10-12 2022-12-13 东北大学 一种基于改进卡尔曼滤波的电力***虚假数据检测方法
CN112835075B (zh) * 2019-11-25 2024-04-02 大唐移动通信设备有限公司 正交频分复用载波相位的跟踪方法、装置及电子设备
CN110901699B (zh) * 2019-11-26 2023-06-23 南京信息工程大学 铁路平面无线调车定位方法、调车***及其工作方法
CN111090281B (zh) * 2019-11-27 2023-07-28 温州大学 基于改进粒子滤波算法估算机器人方位的方法和装置
CN111175795B (zh) * 2020-01-03 2023-05-26 暨南大学 Gnss/ins组合导航***的两步抗差滤波方法及***
CN111190207B (zh) * 2020-01-09 2023-08-08 郑州轻工业大学 基于pstcsdref算法的无人机ins bds组合导航方法
CN111189442B (zh) * 2020-01-11 2023-07-04 郑州轻工业大学 基于cepf的无人机多源导航信息状态预测方法
CN111288983B (zh) * 2020-03-02 2021-07-27 中国电子科技集团公司第五十四研究所 一种适于多源融合的室内狭长地带定位方法
CN111444663B (zh) * 2020-03-11 2023-05-12 西安电子科技大学 卡尔曼跟踪环路设计方法、卡尔曼跟踪环路、航天飞行器
CN111310110B (zh) * 2020-03-20 2023-05-05 郑州轻工业大学 一种高维耦合不确定***混合状态估计方法
CN111474960B (zh) * 2020-03-23 2023-03-31 中国人民解放军空军工程大学 基于控制量特征辅助的临空高超声速目标跟踪方法
CN111367160B (zh) * 2020-03-30 2021-07-06 武汉大学 一种gnss授时接收机精密时钟调控方法及***
CN111523208B (zh) * 2020-04-09 2023-08-01 淮阴工学院 一种对人体行走足底地面反应力的卡尔曼滤波方法
CN112051598B (zh) * 2020-06-24 2023-09-29 中铁第四勘察设计院集团有限公司 一种基于双重校正的车载gnss/ins组合导航方法
CN113884102B (zh) * 2020-07-04 2024-06-25 华为技术有限公司 传感器安装偏差角的标定方法、组合定位***和车辆
CN111896029A (zh) * 2020-07-29 2020-11-06 西安石油大学 一种基于组合算法的mems陀螺随机误差补偿方法
CN111948601B (zh) * 2020-07-31 2024-01-30 中国电子科技集团公司第四十一研究所 一种非高斯噪声条件下的单站纯角度目标定位与跟踪方法
CN111965683A (zh) * 2020-08-19 2020-11-20 上海海事大学 一种卫星定位和惯导定位的组合定位方法、装置及存储介质
CN111896008A (zh) * 2020-08-20 2020-11-06 哈尔滨工程大学 一种改进的鲁棒无迹卡尔曼滤波组合导航方法
CN112197767B (zh) * 2020-10-10 2022-12-23 江西洪都航空工业集团有限责任公司 一种在线改进滤波误差的滤波器设计方法
CN112269201B (zh) * 2020-10-23 2024-04-16 北京云恒科技研究院有限公司 一种gnss/ins紧耦合时间分散滤波方法
CN112632454A (zh) * 2020-12-17 2021-04-09 长光卫星技术有限公司 一种基于自适应卡尔曼滤波算法的mems陀螺滤波方法
CN112946711B (zh) * 2021-01-29 2022-11-25 中国人民解放军国防科技大学 一种gnss/ins组合导航***的导航方法
CN113204038B (zh) * 2021-04-16 2023-03-21 北方工业大学 基于时域与频域的卡尔曼平滑滤波方法及平滑滤波器
CN113189578B (zh) * 2021-04-20 2022-09-16 浙江大学 一种扩展目标跟踪方法
CN113155156A (zh) * 2021-04-27 2021-07-23 北京信息科技大学 运行信息的确定方法及装置、存储介质、电子装置
CN113188539A (zh) * 2021-04-27 2021-07-30 深圳亿嘉和科技研发有限公司 一种巡检机器人的组合定位方法
CN113326616A (zh) * 2021-05-31 2021-08-31 上海航天测控通信研究所 抗慢变量测粗差的容错卡尔曼滤波方法
CN113504012B (zh) * 2021-07-14 2023-12-22 国网福建省电力有限公司检修分公司 基于卡尔曼滤波算法的sf6压力状态判别方法及装置
CN113536227B (zh) * 2021-07-27 2023-10-24 中国电子科技集团公司第二十八研究所 一种基于Kalman协方差的机动目标快速跟踪方法
CN113721280B (zh) * 2021-07-30 2023-08-15 中国人民解放军国防科技大学 一种组合导航条件下实现定向驱离的方法
CN113670315B (zh) * 2021-08-25 2024-03-15 无锡北微传感科技有限公司 一种基于变分迭代卡尔曼滤波的李群重尾干扰噪声动态飞行器姿态估计方法
CN113916226B (zh) * 2021-10-09 2023-04-18 哈尔滨工业大学 一种基于最小方差的组合导航***抗扰滤波方法
CN113985451B (zh) * 2021-10-25 2022-11-15 湘潭大学 一种基于卡尔曼滤波跟踪环路的导航欺骗检测方法和装置
CN113848579B (zh) * 2021-11-29 2022-03-08 北京北斗华大科技有限公司 Ins辅助gnss定位的粗差剔除方法及***
CN114396941B (zh) * 2021-12-20 2023-12-19 东南大学 一种基于强跟踪Kalman滤波的级联式惯性/卫星深组合方法
CN114417912B (zh) * 2021-12-20 2024-04-12 中国人民解放军军事科学院国防科技创新研究院 一种野值噪声干扰下基于中心误差熵中心差分卡尔曼滤波的卫星姿态确定方法
CN114390431B (zh) * 2022-01-11 2024-04-26 上海则芯半导体科技有限公司 一种基于超宽带的两基站二维相对定位方法及装置
CN114444279B (zh) * 2022-01-14 2022-12-16 中国人民解放军国防科技大学 基于卫星低高精度数据关联参数联合估计的响应预测方法
CN114442557B (zh) * 2022-01-25 2023-05-12 西南交通大学 一种机床温度场快速辨识方法及***
CN114485650B (zh) * 2022-02-15 2023-09-19 上海海事大学 一种mems-ins辅助gnss矢量环路跟踪方法、装置、存储介质及设备
CN114662226B (zh) * 2022-02-24 2024-03-29 北京工业大学 一种基于时变卡尔曼滤波的滚动轴承剩余使用寿命预测方法
CN114895241B (zh) * 2022-05-09 2024-05-03 知微空间智能科技(苏州)有限公司 一种基于数据和模型联合驱动的弹性融合定位方法和装置
CN114858166B (zh) * 2022-07-11 2022-10-11 北京神导科技股份有限公司 基于最大相关熵卡尔曼滤波器的imu姿态解算方法
CN115235513B (zh) * 2022-09-15 2023-01-17 中国船舶重工集团公司第七0七研究所 一种基于伪距和伪距率的惯导校正方法
CN115496099B (zh) * 2022-09-20 2023-06-23 哈尔滨工业大学 一种机械臂传感器的滤波及高阶状态观测方法
CN115616641B (zh) * 2022-12-16 2023-03-17 南京航空航天大学 一种基于粒子滤波的城市峡谷中组合导航高精度定位方法
CN115906535B (zh) * 2023-01-06 2023-05-23 山东科技大学 野值影响下基于事件触发机制的谐波检测方法
CN116009032B (zh) * 2023-03-27 2023-05-26 太原理工大学 基于gnss/imu/里程表耦合的车辆导航干扰信号检测方法
CN116067370B (zh) * 2023-04-03 2023-06-27 广东智能无人***研究院(南沙) 一种imu姿态解算方法及设备、存储介质
CN116068586B (zh) * 2023-04-06 2023-06-16 武汉大学 Gnss授时接收机lqg时钟控制器参数设计方法
CN116552812B (zh) * 2023-04-12 2024-01-23 四川大学 一种电推进geo卫星自学习定轨方法
CN116839591B (zh) * 2023-07-12 2024-05-28 哈尔滨天枢问道技术有限公司 一种轨迹跟踪定位滤波***及救援无人机的融合导航方法
CN117255401B (zh) * 2023-10-16 2024-04-09 四川轻化工大学 一种基于fpga的多源相位自适应跟踪同步***
CN117516523A (zh) * 2023-12-04 2024-02-06 国家基础地理信息中心 一种基于空间状态和观测模型的gnss/ins紧组合导航方法

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6834234B2 (en) * 2000-11-22 2004-12-21 Trimble Navigation, Limited AINS land surveyor system with reprocessing, AINS-LSSRP
US7551133B2 (en) * 2007-04-05 2009-06-23 Sirf Technology, Inc. GPS navigation using interacting multiple model (IMM) estimator and probabilistic data association filter (PDAF)
WO2009043183A1 (en) * 2007-10-04 2009-04-09 University Technologies International System and method for intelligent tuning of kalman filters for ins/gps navigation applications
US9062978B2 (en) * 2011-05-31 2015-06-23 Massachusetts Institute Of Technology Tracking a body by nonlinear and non-Gaussian parametric filtering
US8825396B2 (en) * 2012-11-30 2014-09-02 Applanix Corporation Quasi tightly coupled GNSS-INS integration process
CN103217175B (zh) * 2013-04-10 2015-09-30 哈尔滨工程大学 一种自适应容积卡尔曼滤波方法
US8996311B1 (en) 2013-12-06 2015-03-31 Novatel Inc. Navigation system with rapid GNSS and inertial initialization
CN103927436A (zh) * 2014-04-04 2014-07-16 郑州牧业工程高等专科学校 一种自适应高阶容积卡尔曼滤波方法
CN105737823B (zh) * 2016-02-01 2018-09-21 东南大学 一种基于五阶ckf的gps/sins/cns组合导航方法
CN106291645B (zh) 2016-07-19 2018-08-21 东南大学 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
FR3058229B1 (fr) * 2016-10-27 2020-02-28 Airbus Helicopters Estimation, independante d'une mesure magnetique, de la vitesse et du cap d'un aeronef
EP3339908B1 (en) * 2016-12-23 2019-10-02 u-blox AG Distributed kalman filter architecture for carrier range ambiguity estimation
KR20200044420A (ko) * 2018-10-19 2020-04-29 삼성전자주식회사 위치 추정 방법 및 장치

Also Published As

Publication number Publication date
WO2018014602A1 (zh) 2018-01-25
US20190129044A1 (en) 2019-05-02
US10890667B2 (en) 2021-01-12
CN106291645A (zh) 2017-01-04

Similar Documents

Publication Publication Date Title
CN106291645B (zh) 适于高维gnss/ins深耦合的容积卡尔曼滤波方法
CN109211276B (zh) 基于gpr与改进的srckf的sins初始对准方法
Chang et al. Initial alignment by attitude estimation for strapdown inertial navigation systems
Chang et al. Initial alignment for a Doppler velocity log-aided strapdown inertial navigation system with limited information
Davari et al. An asynchronous adaptive direct Kalman filter algorithm to improve underwater navigation system performance
CN109000642A (zh) 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN108226980A (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CN106772524B (zh) 一种基于秩滤波的农业机器人组合导航信息融合方法
CN107643534A (zh) 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法
Han et al. A novel initial alignment scheme for low-cost INS aided by GPS for land vehicle applications
CN108761512A (zh) 一种弹载bds/sins深组合自适应ckf滤波方法
CN109945895B (zh) 基于渐消平滑变结构滤波的惯性导航初始对准方法
Zhang et al. Singular value decomposition-based robust cubature Kalman filtering for an integrated GPS/SINS navigation system
US10379224B2 (en) Invariant particle filtering
Zhao et al. Adaptive two-stage Kalman filter for SINS/odometer integrated navigation systems
Gong et al. Application of unscented R–T–S smoothing on INS/GPS integration system post processing for airborne earth observation
Huang et al. A novel nonlinear algorithm for non-Gaussian noises and measurement information loss in underwater navigation
Wen et al. Odometer aided SINS in-motion alignment method based on backtracking scheme for large misalignment angles
Tsukerman et al. Analytic evaluation of fine alignment for velocity aided INS
KR101502721B1 (ko) 적응형 상호작용 다중모델 추정기를 이용한 정밀 위치정보 제공 방법 및 장치
Wang et al. An adaptive multiple backtracking UKF method based on Krein space theory for marine vehicles alignment process
Farrell et al. GNSS/INS Integration
CN111190207A (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
Wang et al. A robust backtracking CKF based on Krein space theory for in-motion alignment process
Avzayesh et al. Improved-Performance Vehicle’s State Estimator Under Uncertain Model Dynam

Legal Events

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