CN110632634B - 一种适用于弹道导弹ins/cns/gnss组合导航系统的最优数据融合方法 - Google Patents

一种适用于弹道导弹ins/cns/gnss组合导航系统的最优数据融合方法 Download PDF

Info

Publication number
CN110632634B
CN110632634B CN201910907858.9A CN201910907858A CN110632634B CN 110632634 B CN110632634 B CN 110632634B CN 201910907858 A CN201910907858 A CN 201910907858A CN 110632634 B CN110632634 B CN 110632634B
Authority
CN
China
Prior art keywords
ins
subsystem
cns
gnss
representing
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
CN201910907858.9A
Other languages
English (en)
Other versions
CN110632634A (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 CN201910907858.9A priority Critical patent/CN110632634B/zh
Publication of CN110632634A publication Critical patent/CN110632634A/zh
Application granted granted Critical
Publication of CN110632634B publication Critical patent/CN110632634B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/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/35—Constructional details or hardware or software details of the signal processing chain
    • G01S19/37—Hardware or software details of the signal processing chain
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Signal Processing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Feedback Control In General (AREA)
  • Navigation (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明公开了一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法，该方法包括以下步骤：构造INS/CNS/GNSS组合导航系统模型；在广义高阶CKF的时间更新阶段和量测更新阶段分别引入自适应渐消因子和最大相关熵准则进行INS/CNS子系统和INS/GNSS子系统的局部状态估计；根据最小方差原理和容积准则融合INS/CNS子系统和INS/GNSS子系统的局部估计得到全局最优状态估计。本发明可以同时抑制过程建模误差和非高斯量测噪声对状态估计的影响，提高弹道导弹INS/CNS/GNSS组合导航的自适应性和鲁棒性，获得全局最优的状态估计值。

Description

一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据 融合方法
技术领域
本发明涉及一种数据融合方法，具体涉及一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法。
背景技术
传统的基于EKF、UKF的联邦多传感器数据融合方法使用过程噪声协方差的上界而不是过程噪声协方差本身来消除局部状态估计之间的相关性，因此得到的全局状态估是次优的。此外，该类方法要求非线性系统模型必须达到足够的精度。然而，在弹道导弹飞行的过程中，一方面，其弹载的INS/CNS/GNSS组合导航系统很容易受到复杂环境的影响，进而导致INS/CNS/GNSS组合导航系统的量测信息受到非高斯噪声的影响，在这种情况下如果继续使用传统的基于高斯假设的非线性滤波方法将会使组合导航系统的导航精度严重下降；另一方面，由于真实的动态系统模型非常复杂，我们建立的系统过程模型只能是真实模型的理论近似，因而存在建模误差，这种建模误差也会导致组合导航系统的导航精度的下降。
所以，需要一个新的技术方案来解决上述问题。
发明内容
发明目的：为了克服现有技术中存在的组合导航系统的导航精度差的不足，提供一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法，其实现在过程模型含有不确定性以及量测噪声为非高斯噪声的情况下依然可以获取全局最优的状态估计。
技术方案：本发明提供一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法，包括以下步骤：
S1：构造INS/CNS/GNSS组合导航系统滤波模型；
S2：在广义高阶CKF的时间更新阶段和量测更新阶段分别引入自适应渐消因子和最大相关熵准则进行INS/CNS子系统和INS/GNSS子系统的局部状态估计；
S3：根据最小方差原理和容积准则融合INS/CNS子系统和INS/GNSS子系统的局部估计得到全局最优状态估计。
进一步的，所述步骤S1中构造INS/CNS/GNSS组合导航系统滤波模型包括以下步骤：
S1-1：设置INS/CNS/GNSS组合导航系统的状态向量为x＝[φx，φy，φz，δvx，δvy，δvz，x，y，z，εx，εy，εz，Δx，Δy，Δz]T，其中φx，φy，φz表示发射点惯性系下姿态失准角，δvx，δvy，δvz表示发射点惯性系下的速度误差，x，y，z表示发射点惯性系下的位置误差，εx，εy，εz表示弹体坐标系下的陀螺常值漂移，Δx，Δy，Δz表示弹体坐标系下的加速度计常值偏置，T为转置符号；
S1-2：根据INS/CNS/GNSS组合导航系统的15维状态向量x建立系统的状态方程：
xk＝f(xk-1)+vk-1
其中，f(·)为非线性系统函数，xk-1和xk分别表示k-1时刻和k时刻的状态向量，vk-1表示过程噪声，vk-1的协方差为
Figure BDA0002212558140000021
S1-3：分别建立INS/GNSS子系统和INS/CNS子系统的量测方程：
在INS/GNSS子系统中，将INS与GNSS输出的位置的差值和速度的差值作为量测信息，建立该子系统的量测方程：
z1，k＝H1，kxk+ω1，k
其中，z1，k表示INS/GNSS子系统k时刻的量测向量，H1，k表示INS/GNSS子系统k时刻的测量矩阵，ω1，k表示INS/GNSS子系统k时刻的量测噪声，其方差是
Figure BDA0002212558140000022
在INS/CNS子系统中，将INS与CNS输出的姿态角的差值作为量测信息，建立该子系统的量测方程：
z2，k＝H2，kxk+ω2，k
其中，z2，k表示INS/CNS子系统k时刻的量测向量，H2，k表示INS/CNS子系统k时刻的测量矩阵，ω2，k表示INS/CNS子系统k时刻的量测噪声，其方差为
Figure BDA0002212558140000023
进一步的，所述步骤S2具体包括以下步骤：
S2-1：因为INS/CNS子系统和INS/GNSS子系统采用同样的滤波过程进行局部状态估计，为了避免重复，在此仅具体说明INS/GNSS子系统的滤波过程。因此，将步骤S1-3中的z1，k、z2，k统一写成zk，H1，k和H2，k统一写成Hk，ω1，k和ω2，k统一写成ωk，R1，k和R2，k统一写成Rk，根据INS/GNSS子系统的局部状态估计要求设置核宽度γ的值，设置初始的状态向量、状态误差协方差、渐消因子分别为
Figure BDA0002212558140000024
τk(0)＝1，S0|0＝chol(P0|0)，chol(·)表示乔列斯分解运算；
S2-2：根据公式
Figure BDA0002212558140000031
计算出传播容积点
Figure BDA0002212558140000032
其中i＝1，...，2n2+1，n表示状态向量的维数，
Figure BDA0002212558140000033
[·]i表示集合[·]的第i列；
S2-3：预测k时刻状态向量
Figure BDA0002212558140000034
误差协方差阵
Figure BDA0002212558140000035
和计算Sk|k-1＝chol(Pk|k-1)，其中
Figure BDA0002212558140000036
S2-4：根据
Figure BDA0002212558140000037
和Sk|k-1产生新的容积点
Figure BDA0002212558140000038
并预测k时刻的量测信息
Figure BDA0002212558140000039
S2-5：根据INS/GNSS子系统的量测方程和步骤S2-3中的
Figure BDA00022125581400000310
和Pk|k-1构建如下方程：
Figure BDA00022125581400000311
其中，I表示单位矩阵，
Figure BDA00022125581400000312
Mp，k|k-1＝chol(Pk|k-1)、Mr，k＝chol(Rk)和
Figure BDA00022125581400000313
在方程
Figure BDA00022125581400000314
的两边左乘
Figure BDA00022125581400000315
得到：
Figure BDA00022125581400000316
令
Figure BDA00022125581400000317
则上述方程可重写为：Dk＝BkXk+ek；
S2-6：对量测噪声方差进行更新：
Figure BDA0002212558140000041
其中，diag(·)表示矩阵的对角化运算，m为量测信息的维度，
Figure BDA0002212558140000042
Figure BDA0002212558140000043
di，k表示Dk的第i个元素，bi，k表示Bk的第i行元素；
S2-7：计算状态信息和量测信息之间的互协方差矩阵Pxz，k|k-1，
Figure BDA0002212558140000044
并令i＝0；
S2-8：令
Figure BDA0002212558140000045
Figure BDA0002212558140000046
并设定卡方分布
Figure BDA0002212558140000047
的值，其中θ表示卡方分布的自由度，α表示卡方分布的分位数，如果
Figure BDA0002212558140000048
则计算
Figure BDA0002212558140000049
Figure BDA00022125581400000410
返回步骤S2-2继续执行下一滤波周期；
S2-9：
如果
Figure BDA00022125581400000411
计算
Figure BDA00022125581400000412
其中，i＝(0，1，...)，i＝i+1，根据得到自适应渐消因子τk更新ak(i)的值，判断ak(i)是否满足
Figure BDA00022125581400000413
若不满足，则返回步骤S2-9继续执行；若满足，则得到最优的自适应渐消因子τk的值，然后继续执行后续步骤；
S2-10：使用自适应渐消因子τk更新预测的状态预测协方差：
Figure BDA00022125581400000414
计算卡尔曼增益：
Figure BDA00022125581400000415
估计后验状态和后验协方差矩阵：
Figure BDA00022125581400000416
返回步骤S2-2继续执行下一滤波周期。
进一步的，所述步骤S3包括以下步骤：
S3-1：在分别对INS/GNSS子系统和INS/CNS子系统执行完滤波过程后便可获得相应的局部后验状态估计
Figure BDA00022125581400000417
为了便于区分，令INS/GNSS子系统获得局部后验状态估计为
Figure BDA0002212558140000051
INS/CNS子系统获得局部后验状态估计为
Figure BDA0002212558140000052
53-2：根据最小方差原理和容积准则得到全局最优状态估计：
Figure BDA0002212558140000053
其中令β＝[β1，β2]T，
Figure BDA0002212558140000054
P11为INS/GNSS子系统执行局部状态估计得到的Pk|k，P22为INS/CNS子系统执行局部状态估计得到的Pk|k，P12表示
Figure BDA0002212558140000055
和
Figure BDA0002212558140000056
估计误差的互协方差矩阵，P21表示
Figure BDA0002212558140000057
和
Figure BDA0002212558140000058
估计误差的互协方差矩阵。
进一步的，P12和P21通过容积准则近似得到：
Figure BDA0002212558140000059
Figure BDA00022125581400000510
其中，
Figure BDA00022125581400000511
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2-2得到的传播容积点
Figure BDA00022125581400000512
和
Figure BDA00022125581400000513
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2-3得到的状态向量预测值，
Figure BDA00022125581400000514
和
Figure BDA00022125581400000515
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2-4得到的新的容积点Xi，k|k-1，
Figure BDA00022125581400000516
和
Figure BDA00022125581400000517
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2-4得到的量测信息预测值
Figure BDA00022125581400000518
Figure BDA00022125581400000519
和
Figure BDA00022125581400000520
分别表示INS/GNSS子系统与INS/CNS子系统执行局部状态估计过程得到的滤波增益
Figure BDA00022125581400000521
Figure BDA00022125581400000522
I表示n维的单位矩阵。
有益效果：本发明与现有技术相比，在广义高阶CKF的时间更新阶段和量测更新阶段分别引入自适应渐消因子和最大相关熵准则进行INS/GNSS子系统和INS/CNS子系统的局部状态估计，最后根据最小方差原理和容积准则融合INS/CNS子系统和INS/GNSS子系统的局部估计，使得弹道导弹INS/CNS/GNSS组合导航系统在系统模型存在过程建模误差和量测噪声为非高斯分布的情况下，该组合导航系统依然可以获得全局最优的状态估计，本发明可以同时抑制过程建模误差和非高斯量测噪声对状态估计的影响，提高弹道导弹INS/CNS/GNSS组合导航的自适应性和鲁棒性，从而保证了INS/CNS/GNSS组合导航系统的导航精度。
附图说明
图1为本发明方法的流程图。
具体实施方式
下面结合附图和具体实施例，进一步阐明本发明。
如图1所示，本发明提供一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法，包括以下步骤：
S1：构造INS/CNS/GNSS组合导航系统滤波模型：
S1-1：设置INS/CNS/GNSS组合导航系统的状态向量为：
x＝[φx，φy，φz，δvx，δvy，δvz，x，y，z，εx，εy，εz，Δx，Δy，Δz]T (1)
其中φx，φy，φz表示发射点惯性系下姿态失准角，δvx，δvy，δvz表示发射点惯性系下的速度误差，x，y，z表示发射点惯性系下的位置误差，εx，εy，εz表示弹体坐标系下的陀螺常值漂移，Δx，Δy，Δz表示弹体坐标系下的加速度计常值偏置，T为转置符号。
S1-2：根据INS/CNS/GNSS组合导航系统的15维状态向量x建立系统的状态方程：
xk＝f(xk-1)+vk-1 (2)
其中，f(·)为非线性系统函数，xk-1和xk分别表示k-1时刻和k时刻的状态向量，vk-1表示过程噪声，vk-1的协方差为
Figure BDA0002212558140000061
S1-3：分别建立INS/GNSS子系统和INS/CNS子系统的量测方程：
在INS/GNSS子系统中，将INS与GNSS输出的位置和速度分别做差，并将其作为量测信息，建立该子系统的量测方程：
z1，k＝H1，kxk+ω1，k (3)
其中，z1，k表示INS/GNSS子系统k时刻的量测向量，H1，k表示INS/GNSS子系统k时刻的测量矩阵，ω1，k表示INS/GNSS子系统k时刻的量测噪声，其方差是
Figure BDA0002212558140000062
在INS/CNS子系统中，该子系统的量测方程将INS与CNS输出的姿态角的差值作为量测信息，建立该子系统的量测方程：
z2，k＝H2，kxk+ω2，k (4)
其中，z2，k表示INS/CNS子系统k时刻的量测向量，H2，k表示INS/CNS子系统k时刻的测量矩阵，ω2，k表示INS/CNS子系统k时刻的量测噪声，其方差为
Figure BDA0002212558140000063
S2：在广义高阶CKF的时间更新阶段和量测更新阶段分别引入自适应渐消因子和最大相关熵准则进行INS/CNS子系统和INS/GNSS子系统的局部状态估计，其具体过程如下：
S2-1：因为INS/GNSS子系统和INS/CNS子系统分别通过局部滤波器1和局部滤波器2采用同样的滤波过程进行局部状态估计，为了避免重复，在此仅具体说明INS/GNSS子系统的滤波过程：，因此，本实施例将步骤S1中的z1，k、z2，k统一写成zk，H1，k和H2，k统一写成Hk，ω1，k和ω2，k统一写成ωk，R1，k和R2，k统一写成Rk。根据INS/GNSS子系统的局部状态估计要求设置核宽度γ的值，设置初始的状态向量、状态误差协方差、渐消因子分别为
Figure BDA0002212558140000071
τk(0)＝1，S0|0＝chol(P0|0)，chol(·)表示乔列斯分解运算。
S2-2：根据公式(5)计算出传播容积点
Figure BDA0002212558140000072
Figure BDA0002212558140000073
其中i＝1，...，2n2+1，n表示状态向量的维数，
Figure BDA0002212558140000074
[·]i表示集合[·]的第i列。
S2-3：预测k时刻状态向量和误差协方差阵：
Figure BDA0002212558140000075
Figure BDA0002212558140000076
其中
Figure BDA0002212558140000077
S2-4：根据
Figure BDA0002212558140000078
阳Sk|k-1产生新的容积点：
Figure BDA0002212558140000079
其中，Sk|k-1＝chol(Pk|k-1)。
S2-5：预测k时刻的量测信息：
Figure BDA00022125581400000710
S2-6：根据INS/GNSS子系统的量测方程和
Figure BDA00022125581400000711
和Pk|k-1构建如下方程：
Figure BDA00022125581400000712
其中，I表示单位矩阵，
Figure BDA0002212558140000081
Mp，k|k-1＝chol(Pk|k-1)、Mr，k＝chol(Rk)和
Figure BDA0002212558140000082
在方程
Figure BDA0002212558140000083
的两边左乘
Figure BDA0002212558140000084
得到：
Figure BDA0002212558140000085
令
Figure BDA0002212558140000086
则公式(11)可重写为：
Dk＝BkXk+ek (12)
S2-7：对量测噪声方差进行更新：
Figure BDA0002212558140000087
其中，diag(·)表示矩阵的对角化运算，m为量测信息的维度，
Figure BDA0002212558140000088
Figure BDA0002212558140000089
di，k表示Dk的第i个元素，bi，k表示Bk的第i行元素。
S2-8：计算状态信息和量测信息之间的互协方差矩阵Pxz，k|k-1：
Figure BDA00022125581400000810
令i＝0，
Figure BDA00022125581400000811
Figure BDA00022125581400000812
并设定卡方分布
Figure BDA00022125581400000813
的值，其中θ表示卡方分布的自由度，α表示卡方分布的分位数；
如果
Figure BDA00022125581400000814
则计算：
Figure BDA00022125581400000815
Figure BDA00022125581400000816
Figure BDA00022125581400000817
Figure BDA0002212558140000091
Sk|k＝chol(Pk|k) (19)
继续执行k＝k+1，并返回步骤S2-2中的公式(5)所在位置开始执行下一滤波周期；
如果
Figure BDA0002212558140000092
计算：
Figure BDA0002212558140000093
根据得到自适应渐消因子τk更新ak(i)的值，判断ak(i)是否满足
Figure BDA0002212558140000094
若不满足，i＝i+1，则返回公式(20)更新渐消因子τk的值；若满足，则得到最优的自适应渐消因子τk的值，然后继续执行下列步骤。
S2-9：使用得到的自适应渐消因子τk更新预测的状态预测协方差：
Figure BDA0002212558140000095
计算卡尔曼增益：
Figure BDA0002212558140000096
S2-10：估计后验状态和后验协方差矩阵：
Figure BDA0002212558140000097
Figure BDA0002212558140000098
Sk|k＝chol(Pk|k) (25)
执行k＝k+1，并返回步骤S2-2中的公式(5)所在位置开始执行下一滤波周期。
S3：根据最小方差原理和容积准则融合INS/GNSS子系统和INS/CNS子系统的局部估计得到全局最优状态估计：
S3-1：在分别对INS/GNSS子系统和INS/CNS子系统执行完以上滤波过程后便可获得相应的局部后验状态估计
Figure BDA0002212558140000099
为了便于区分，令INS/GNSS子系统获得局部后验状态估计为
Figure BDA00022125581400000910
INS/CNS子系统获得局部后验状态估计为
Figure BDA00022125581400000911
S3-2：根据最小方差原理和容积准则得到全局最优状态估计：
Figure BDA00022125581400000912
其中令β＝[β1，β2]T，
Figure BDA0002212558140000101
P11为INS/GNSS子系统执行局部状态估计得到的Pk|k，P22为INS/CNS子系统执行局部状态估计得到的Pk|k，P12表示
Figure BDA0002212558140000102
和
Figure BDA0002212558140000103
估计误差的互协方差矩阵，P21表示
Figure BDA0002212558140000104
和
Figure BDA0002212558140000105
估计误差的互协方差矩阵，P12和P21通过容积准则近似得到：
Figure BDA0002212558140000106
Figure BDA0002212558140000107
其中，
Figure BDA0002212558140000108
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2的过程中得到的传播容积点
Figure BDA0002212558140000109
Figure BDA00022125581400001010
和
Figure BDA00022125581400001011
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2的过程中得到的状态向量预测值，
Figure BDA00022125581400001012
和
Figure BDA00022125581400001013
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2的过程中得到的新的容积点Xi，k|k-1，
Figure BDA00022125581400001014
和
Figure BDA00022125581400001015
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2的过程中得到的量测信息预测值
Figure BDA00022125581400001016
Figure BDA00022125581400001017
和
Figure BDA00022125581400001018
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2的过程中得到的滤波增益
Figure BDA00022125581400001019
Figure BDA00022125581400001020
I表示n维的单位矩阵。

Claims (3)

1.一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法，其特征在于：包括以下步骤：
S1：构造INS/CNS/GNSS组合导航系统滤波模型；
S2：在广义高阶CKF的时间更新阶段和量测更新阶段分别引入自适应渐消因子和最大相关熵准则进行INS/CNS子系统和INS/GNSS子系统的局部状态估计；
S3：根据最小方差原理和容积准则融合INS/CNS子系统和INS/GNSS子系统的局部估计得到全局最优状态估计；
所述步骤S1中构造INS/CNS/GNSS组合导航系统滤波模型包括以下步骤：
S1-1：设置INS/CNS/GNSS组合导航系统的状态向量为x＝[φx,φy,φz,δvx,δvy,δvz,x,y,z,εx,εy,εz,Δx,Δy,Δz]T，其中φx,φy,φz表示发射点惯性系下姿态失准角，δvx,δvy,δvz表示发射点惯性系下的速度误差，x,y,z表示发射点惯性系下的位置误差，εx,εy,εz表示弹体坐标系下的陀螺常值漂移，Δx,Δy,Δz表示弹体坐标系下的加速度计常值偏置，T为转置符号；
S1-2：根据INS/CNS/GNSS组合导航系统的15维状态向量x建立系统的状态方程：
xk＝f(xk-1)+vk-1
其中，f(·)为非线性系统函数，xk-1和xk分别表示k-1时刻和k时刻的状态向量，vk-1表示过程噪声，vk-1的协方差为
Figure FDA0003706885430000011
S1-3：分别建立INS/GNSS子系统和INS/CNS子系统的量测方程：
在INS/GNSS子系统中，将INS与GNSS输出的位置的差值和速度的差值作为量测信息，建立该子系统的量测方程：
z1,k＝H1,kxk+ω1,k
其中，z1,k表示INS/GNSS子系统k时刻的量测向量，H1,k表示INS/GNSS子系统k时刻的测量矩阵，ω1,k表示INS/GNSS子系统k时刻的量测噪声，其方差是
Figure FDA0003706885430000012
在INS/CNS子系统中，将INS与CNS输出的姿态角的差值作为量测信息，建立该子系统的量测方程：
z2,k＝H2,kxk+ω2,k
其中，z2,k表示INS/CNS子系统k时刻的量测向量，H2,k表示INS/CNS子系统k时刻的测量矩阵，ω2,k表示INS/CNS子系统k时刻的量测噪声，其方差为
Figure FDA0003706885430000021
所述步骤S2具体包括以下步骤：
S2-1：因为INS/CNS子系统和INS/GNSS子系统采用同样的滤波过程进行局部状态估计，为了避免重复，在此仅具体说明INS/GNSS子系统的滤波过程，因此，将步骤S1-3中的z1,k、z2,k统一写成zk，H1,k和H2,k统一写成Hk，ω1,k和ω2,k统一写成ωk，R1,k和R2,k统一写成Rk，根据INS/GNSS子系统的局部状态估计要求设置核宽度γ的值，设置初始的状态向量、状态误差协方差、渐消因子分别为
Figure FDA0003706885430000022
τk(0)＝1，S00＝chol(P00)，chol(·)表示乔列斯分解运算；
S2-2：根据公式
Figure FDA0003706885430000023
计算出传播容积点
Figure FDA0003706885430000024
其中i＝1,...,2n2+1，n表示状态向量的维数，
Figure FDA0003706885430000025
[·]i表示集合[·]的第i列；
S2-3：预测k时刻状态向量
Figure FDA0003706885430000026
误差协方差阵
Figure FDA0003706885430000027
和计算Sk|k-1＝chol(Pk|k-1)，其中
Figure FDA0003706885430000028
S2-4：根据
Figure FDA0003706885430000029
和Sk|k-1产生新的容积点
Figure FDA00037068854300000210
并预测k时刻的量测信息
Figure FDA00037068854300000211
S2-5：根据INS/GNSS子系统的量测方程和步骤S2-3中的
Figure FDA00037068854300000212
和Pk|k-1构建如下方程：
Figure FDA00037068854300000213
其中，I表示单位矩阵，
Figure FDA0003706885430000031
Mp,k|k-1＝chol(Pk|k-1)、Mr,k＝chol(Rk)和
Figure FDA0003706885430000032
在方程
Figure FDA0003706885430000033
的两边左乘
Figure FDA0003706885430000034
得到：
Figure FDA0003706885430000035
令
Figure FDA0003706885430000036
则上述方程可重写为：Dk＝BkXk+ek；
S2-6：对量测噪声方差进行更新：
Figure FDA0003706885430000037
其中，diag(·)表示矩阵的对角化运算，m为量测信息的维度，
Figure FDA0003706885430000038
Figure FDA0003706885430000039
di,k表示Dk的第i个元素，bi,k表示Bk的第i行元素；
S2-7：计算状态信息和量测信息之间的互协方差矩阵Pxz,k|k-1，
Figure FDA00037068854300000310
并令i＝0；
S2-8：令
Figure FDA00037068854300000311
Figure FDA00037068854300000312
并设定卡方分布
Figure FDA00037068854300000313
的值，其中θ表示卡方分布的自由度，α表示卡方分布的分位数，如果
Figure FDA00037068854300000314
则计算
Figure FDA00037068854300000315
Figure FDA00037068854300000316
k＝k+1，返回步骤S2-2继续执行下一滤波周期；
S2-9：
如果
Figure FDA0003706885430000041
计算
Figure FDA0003706885430000042
其中，i＝(0,1,…)，i＝i+1，根据得到自适应渐消因子τk更新ak(i)的值，判断ak(i)是否满足
Figure FDA0003706885430000043
若不满足，则返回步骤S2-9继续执行；若满足，则得到最优的自适应渐消因子τk的值，然后继续执行后续步骤；
S2-10：使用自适应渐消因子τk更新预测的状态预测协方差：
Figure FDA0003706885430000044
计算卡尔曼增益：
Figure FDA0003706885430000045
估计后验状态和后验协方差矩阵：
Figure FDA0003706885430000046
返回步骤S2-2继续执行下一滤波周期。
2.根据权利要求1所述的一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法，其特征在于：所述步骤S3包括以下步骤：
S3-1：在分别对INS/GNSS子系统和INS/CNS子系统执行完滤波过程后便可获得相应的局部后验状态估计
Figure FDA0003706885430000047
为了便于区分，令INS/GNSS子系统获得局部后验状态估计为
Figure FDA0003706885430000048
INS/CNS子系统获得局部后验状态估计为
Figure FDA0003706885430000049
S3-2：根据最小方差原理和容积准则得到全局最优状态估计：
Figure FDA00037068854300000410
其中令β＝[β1,β2]T，
Figure FDA00037068854300000411
P11为INS/GNSS子系统执行局部状态估计得到的Pk|k，P22为INS/CNS子系统执行局部状态估计得到的Pk|k，P12表示
Figure FDA00037068854300000412
和
Figure FDA00037068854300000413
估计误差的互协方差矩阵，P21表示
Figure FDA00037068854300000414
和
Figure FDA00037068854300000415
估计误差的互协方差矩阵。
3.根据权利要求2所述的一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法，其特征在于：所述步骤S3-2中P12和P21通过容积准则近似得到，具体为：
Figure FDA00037068854300000416
Figure FDA00037068854300000417
其中，
Figure FDA00037068854300000418
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2-2得到的传播容积点
Figure FDA0003706885430000051
和
Figure FDA0003706885430000052
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2-3得到的状态向量预测值，
Figure FDA0003706885430000053
和
Figure FDA0003706885430000054
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2-4得到的新的容积点Xi,k|k-1，
Figure FDA0003706885430000055
和
Figure FDA0003706885430000056
分别表示INS/GNSS子系统与INS/CNS子系统执行步骤S2-4得到的量测信息预测值
Figure FDA0003706885430000057
和
Figure FDA0003706885430000058
分别表示INS/GNSS子系统与INS/CNS子系统执行局部状态估计过程得到的滤波增益
Figure FDA0003706885430000059
Figure FDA00037068854300000510
I表示n维的单位矩阵。
CN201910907858.9A 2019-09-24 2019-09-24 一种适用于弹道导弹ins/cns/gnss组合导航系统的最优数据融合方法 Active CN110632634B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910907858.9A CN110632634B (zh) 2019-09-24 2019-09-24 一种适用于弹道导弹ins/cns/gnss组合导航系统的最优数据融合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910907858.9A CN110632634B (zh) 2019-09-24 2019-09-24 一种适用于弹道导弹ins/cns/gnss组合导航系统的最优数据融合方法

Publications (2)

Publication Number Publication Date
CN110632634A CN110632634A (zh) 2019-12-31
CN110632634B true CN110632634B (zh) 2022-11-01

Family

ID=68973638

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910907858.9A Active CN110632634B (zh) 2019-09-24 2019-09-24 一种适用于弹道导弹ins/cns/gnss组合导航系统的最优数据融合方法

Country Status (1)

Country Link
CN (1) CN110632634B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113432608B (zh) * 2021-02-03 2024-06-07 东南大学 用于ins/cns组合导航系统的基于最大相关熵的广义高阶ckf方法
CN113447019B (zh) * 2021-08-05 2023-01-13 齐鲁工业大学 Ins/cns组合导航方法、系统、存储介质及设备
CN113776527B (zh) * 2021-09-13 2023-04-07 中国民用航空飞行学院 一种民航飞机全时空的组合导航系统和导航方法
CN116608863B (zh) * 2023-07-17 2023-09-22 齐鲁工业大学(山东省科学院) 基于Huber滤波更新框架的组合导航数据融合方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103528587B (zh) * 2013-10-15 2016-09-28 西北工业大学 自主组合导航系统
CN103927436A (zh) * 2014-04-04 2014-07-16 郑州牧业工程高等专科学校 一种自适应高阶容积卡尔曼滤波方法
CN106813664A (zh) * 2017-03-06 2017-06-09 四川咖范网络科技有限公司 一种车载导航方法和装置
CN107707220A (zh) * 2017-08-31 2018-02-16 东南大学 一种应用在gnss/ins中的改进型ckf方法
CN109000642A (zh) * 2018-05-25 2018-12-14 哈尔滨工程大学 一种改进的强跟踪容积卡尔曼滤波组合导航方法
CN109447122B (zh) * 2018-09-28 2021-07-13 浙江大学 一种分布式融合结构中的强跟踪渐消因子计算方法
CN109459019B (zh) * 2018-12-21 2021-09-10 哈尔滨工程大学 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法

Also Published As

Publication number Publication date
CN110632634A (zh) 2019-12-31

Similar Documents

Publication Publication Date Title
CN110632634B (zh) 一种适用于弹道导弹ins/cns/gnss组合导航系统的最优数据融合方法
CN109211276B (zh) 基于gpr与改进的srckf的sins初始对准方法
CN110398257B (zh) Gps辅助的sins系统快速动基座初始对准方法
CN103235328B (zh) 一种gnss与mems组合导航的方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN108827310A (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN108761512A (zh) 一种弹载bds/sins深组合自适应ckf滤波方法
CN107479076B (zh) 一种动基座下联合滤波初始对准方法
CN110044385B (zh) 一种大失准角情况下的快速传递对准方法
CN110440830A (zh) 动基座下车载捷联惯导系统自对准方法
CN111707292B (zh) 一种自适应滤波的快速传递对准方法
CN113587926B (zh) 一种航天器空间自主交会对接相对导航方法
CN108827288A (zh) 一种基于对偶四元数的降维捷联惯性导航系统初始对准方法及系统
CN112013849A (zh) 一种水面船自主定位方法及系统
CN113916226B (zh) 一种基于最小方差的组合导航系统抗扰滤波方法
CN111190207A (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
CN117570975A (zh) 一种改进混合熵容积卡尔曼滤波的惯性导航姿态解算方法
Shen et al. Research on high-precision measurement systems of modern aircraft
CN108871312B (zh) 一种重力梯度仪及星敏感器的联合定姿方法
CN114018262B (zh) 一种改进的衍生容积卡尔曼滤波组合导航方法
CN112197767B (zh) 一种在线改进滤波误差的滤波器设计方法
CN115839726A (zh) 磁传感器和角速度传感器联合标定的方法、系统及介质
CN110006455A (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