阅读说明：本技术 一种适用于弹道导弹ins/cns/gnss组合导航系统的最优数据融合方法 (Optimal data fusion method suitable for ballistic missile INS/CNS/GNSS combined navigation system ) 是由 陈熙源 柳笛 刘晓 石春凤 马振 于 2019-09-24 设计创作，主要内容包括：本发明公开了一种适用于弹道导弹INS/CNS/GNSS组合导航系统的最优数据融合方法,该方法包括以下步骤：构造INS/CNS/GNSS组合导航系统模型；在广义高阶CKF的时间更新阶段和量测更新阶段分别引入自适应渐消因子和最大相关熵准则进行INS/CNS子系统和INS/GNSS子系统的局部状态估计；根据最小方差原理和容积准则融合INS/CNS子系统和INS/GNSS子系统的局部估计得到全局最优状态估计。本发明可以同时抑制过程建模误差和非高斯量测噪声对状态估计的影响,提高弹道导弹INS/CNS/GNSS组合导航的自适应性和鲁棒性,获得全局最优的状态估计值。(The invention discloses an optimal data fusion method suitable for a ballistic missile INS/CNS/GNSS combined navigation system, which comprises the following steps: constructing an INS/CNS/GNSS integrated navigation system model; respectively introducing a self-adaptive fading factor and a maximum correlation entropy criterion to carry out local state estimation of the INS/CNS subsystem and the INS/GNSS subsystem in a time updating stage and a measurement updating stage of the generalized high-order CKF; and according to the minimum variance principle and the volume criterion, local estimation of the INS/CNS subsystem and the INS/GNSS subsystem is fused to obtain global optimal state estimation. The method can simultaneously inhibit the influence of process modeling errors and non-Gaussian measurement noise on state estimation, improve the adaptivity and robustness of ballistic missile INS/CNS/GNSS combined navigation, and obtain the globally optimal state estimation value.)

1. An optimal data fusion method suitable for a ballistic missile INS/CNS/GNSS combined navigation system is characterized by comprising the following steps: the method comprises the following steps:

s1: constructing an INS/CNS/GNSS combined navigation system filtering model;

s2: respectively introducing a self-adaptive fading factor and a maximum correlation entropy criterion to carry out local state estimation of the INS/CNS subsystem and the INS/GNSS subsystem in a time updating stage and a measurement updating stage of the generalized high-order CKF;

s3: and according to the minimum variance principle and the volume criterion, local estimation of the INS/CNS subsystem and the INS/GNSS subsystem is fused to obtain global optimal state estimation.

2. The optimal data fusion method applicable to ballistic missile INS/CNS/GNSS combined navigation system according to claim 1, wherein: the constructing of the INS/CNS/GNSS integrated navigation system filtering model in step S1 includes the following steps:

s1-1: setting the state vector of the INS/CNS/GNSS integrated navigation system as x ═ phi [ [ phi ] ]_x，φ_y，φ_z，δv_x，δv_y，δv_z，x，y，z，ε_x，ε_y，ε_z，Δ_x，Δ_y，Δ_z]^TWherein phi_x，φ_y，φ_zRepresenting the angular misalignment of the attitude, δ v, under the inertial system of the emission point_x，δv_y，δv_zRepresenting the velocity error in the inertial system of the emission point, x, y, z representing the position error in the inertial system of the emission point, epsilon_x，ε_y，ε_zRepresenting the gyro constant drift, Delta, in a projectile coordinate system_x，Δ_y，Δ_zRepresenting the constant bias of the accelerometer in a missile body coordinate system, wherein T is a transposition symbol;

s1-2: establishing a state equation of the system according to a 15-dimensional state vector x of the INS/CNS/GNSS combined navigation system:

x_k＝f(x_k-1)+v_k-1

wherein f (-) is a nonlinear system function, x_k-1And x_kState vectors, v, representing time k-1 and k, respectively_k-1Representing process noise, v_k-1Has a covariance of

S1-3: respectively establishing measurement equations of an INS/GNSS subsystem and an INS/CNS subsystem:

in an INS/GNSS subsystem, taking the difference between the positions and the speed output by the INS and the GNSS as measurement information, and establishing a measurement equation of the subsystem:

z_1，k＝H_1，kx_k+ω_1，k

wherein z is_1，kRepresentation of INS/GNSSMeasurement vector of system k time, H_1，kMeasurement matrix, ω, representing INS/GNSS subsystem k time_1，kRepresents the measurement noise at the moment k of the INS/GNSS subsystem, the variance of which is

In an INS/CNS subsystem, the difference value of the attitude angles output by the INS and the CNS is used as measurement information, and a measurement equation of the subsystem is established:

z_2，k＝H_2，kx_k+ω_2，k

wherein z is_2，kThe measurement vector, H, representing the INS/CNS subsystem at time k_2，kMeasurement matrix, ω, representing INS/CNS subsystem k time_2，kRepresents the measured noise at the moment k of the INS/CNS subsystem with a variance of

3. The optimal data fusion method applicable to the ballistic missile INS/CNS/GNSS combined navigation system according to claim 2, wherein the optimal data fusion method comprises the following steps: the step S2 specifically includes the following steps:

s2-1: since the INS/CNS subsystem and the INS/GNSS subsystem use the same filtering process for local state estimation, only the filtering process of the INS/GNSS subsystem will be specifically described herein to avoid repetition, and therefore, z in step S1-3 will be described_1，k、z_2，kIs uniformly written as z_k，H_1，kAnd H_2，kWrite uniformly as H_k，ω_1，kAnd ω_2，kWrite uniformly to omega_k，R_1，kAnd R_2，kIs written uniformly as R_kSetting the value of the kernel width gamma according to the local state estimation requirement of the INS/GNSS subsystem, and setting the initial state vector, the state error covariance and the fading factor as

s2-2: according to the formula

s2-3: predicting state vectors at time k

S2-4: according toAnd S_k|k-1Generating new volume points

S2-5: measurement equations and from INS/GNSS subsystemsIn step S2-3

wherein, I represents an identity matrix,

in the equation

S2-6: updating the measurement noise variance:

s2-7: computing a cross-covariance matrix P between state information and metrology information_xz，k|k-1，

s2-8: order to

S2-9：

if it is notComputing

s2-10: using an adaptive fading factor tau_kUpdate predicted state prediction covariance:

4. The optimal data fusion method applicable to the ballistic missile INS/CNS/GNSS combined navigation system according to claim 3, wherein; the step S3 includes the steps of:

s3-1: after the INS/GNSS subsystem and the INS/CNS subsystem are respectively executed with filtering processes, corresponding local posterior state estimation can be obtained

S3-2: obtaining global optimal state estimation according to the minimum variance principle and the volume criterion:

5. The optimal data fusion method applicable to ballistic missile INS/CNS/GNSS combined navigation system according to claim 4, wherein: p in the step S3-2₁₂And P₂₁The volume is obtained by approximation through a volume criterion, and specifically comprises the following steps:

wherein the content of the first and second substances,respectively representing the propagation volume points obtained by the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2-2

Technical Field

The invention relates to a data fusion method, in particular to an optimal data fusion method suitable for a ballistic missile INS/CNS/GNSS combined navigation system.

Background

Traditional EKF, UKF based federated multi-sensor data fusion methods use the upper bound of process noise covariance to eliminate the correlation between local state estimates rather than the process noise covariance itself, so the resulting global state estimate is suboptimal. Furthermore, this type of method requires that the nonlinear system model must achieve sufficient accuracy. However, in the process of ballistic missile flight, on one hand, the missile-borne INS/CNS/GNSS integrated navigation system is easily affected by a complex environment, which causes measurement information of the INS/CNS/GNSS integrated navigation system to be affected by non-gaussian noise, and in this case, if the conventional non-linear filtering method based on gaussian assumption is continuously used, the navigation accuracy of the integrated navigation system will be seriously reduced; on the other hand, because the real dynamic system model is very complex, the established system process model can only be a theoretical approximation of the real model, so that a modeling error exists, and the modeling error can also cause the reduction of the navigation precision of the integrated navigation system.

Therefore, a new technical solution is needed to solve the above problems.

Disclosure of Invention

The purpose of the invention is as follows: in order to overcome the defect of poor navigation accuracy of an integrated navigation system in the prior art, an optimal data fusion method suitable for a ballistic missile INS/CNS/GNSS integrated navigation system is provided, and global optimal state estimation can still be obtained under the condition that a process model contains uncertainty and measurement noise is non-Gaussian noise.

The technical scheme is as follows: the invention provides an optimal data fusion method suitable for a ballistic missile INS/CNS/GNSS combined navigation system, which comprises the following steps:

s1: constructing an INS/CNS/GNSS combined navigation system filtering model;

s2: respectively introducing a self-adaptive fading factor and a maximum correlation entropy criterion to carry out local state estimation of the INS/CNS subsystem and the INS/GNSS subsystem in a time updating stage and a measurement updating stage of the generalized high-order CKF;

s3: and according to the minimum variance principle and the volume criterion, local estimation of the INS/CNS subsystem and the INS/GNSS subsystem is fused to obtain global optimal state estimation.

Further, the constructing an INS/CNS/GNSS integrated navigation system filter model in step S1 includes the following steps:

s1-1: setting the state vector of the INS/CNS/GNSS integrated navigation system as x ═ phi [ [ phi ] ]_x，φ_y，φ_z，δv_x，δv_y，δv_z，x，y，z，ε_x，ε_y，ε_z，Δ_x，Δ_y，Δ_z]^TWherein phi_x，φ_y，φ_zRepresenting the angular misalignment of the attitude, δ v, under the inertial system of the emission point_x，δv_y，δv_zRepresenting the velocity error in the inertial system of the emission point, x, y, z representing the position error in the inertial system of the emission point, epsilon_x，ε_y，ε_zRepresenting the gyro constant drift, Delta, in a projectile coordinate system_x，Δ_y，Δ_zRepresenting the constant bias of the accelerometer in a missile body coordinate system, wherein T is a transposition symbol;

s1-2: establishing a state equation of the system according to a 15-dimensional state vector x of the INS/CNS/GNSS combined navigation system:

x_k＝f(x_k-1)+v_k-1

wherein f (-) is a nonlinear system function, x_k-1And x_kState vectors, v, representing time k-1 and k, respectively_k-1Representing process noise, v_k-1Has a covariance of

S1-3: respectively establishing measurement equations of an INS/GNSS subsystem and an INS/CNS subsystem:

in an INS/GNSS subsystem, taking the difference between the positions and the speed output by the INS and the GNSS as measurement information, and establishing a measurement equation of the subsystem:

z_1，k＝H_1，kx_k+ω_1，k

wherein z is_1，kMeasurement vector, H, representing INS/GNSS subsystem time k_1，kMeasurement matrix, ω, representing INS/GNSS subsystem k time_1，kRepresents the measurement noise at the moment k of the INS/GNSS subsystem, the variance of which is

In an INS/CNS subsystem, the difference value of the attitude angles output by the INS and the CNS is used as measurement information, and a measurement equation of the subsystem is established:

z_2，k＝H_2，kx_k+ω_2，k

wherein z is_2，kThe measurement vector, H, representing the INS/CNS subsystem at time k_2，kMeasurement matrix, ω, representing INS/CNS subsystem k time_2，kRepresents the measured noise at the moment k of the INS/CNS subsystem with a variance of

Further, the step S2 specifically includes the following steps:

s2-1: since the INS/CNS subsystem and the INS/GNSS subsystem use the same filtering process for local state estimation, only the filtering process of the INS/GNSS subsystem will be specifically described herein to avoid repetition. Therefore, z in step S1-3_1，k、z_2，kIs uniformly written as z_k，H_1，kAnd H_2，kWrite uniformly as H_k，ω_1，kAnd ω_2，kWrite uniformly to omega_k，R_1，kAnd R_2，kIs written uniformly as R_kSetting the value of the kernel width gamma according to the local state estimation requirement of the INS/GNSS subsystem, and setting the initial state vector, the state error covariance and the fading factor as

τ_k(0)＝1，S_0|0＝chol(P_0|0) Chol (·) denotes the cholesky decomposition operation;

s2-2: according to the formula

Calculating propagation volume points

Where i 1., 2n²+1, n represents the dimension of the state vector,

[·]_ia set of representations [ ·]The ith column;

s2-3: predicting state vectors at time k

Error covariance matrix

And calculating S_k|k-1＝chol(P_k|k-1) Wherein

S2-4: according to

And S_k|k-1Generating new volume pointsAnd predict the measurement information at time k

S2-5: according to the INS/GNSS subsystem' S measurement equation and in step S2-3

And P_k|k-1The following equation was constructed:

wherein, I represents an identity matrix,

M_p，k|k-1＝chol(P_k|k-1)、M_r，k＝chol(R_k) And

in the equation

Is multiplied by

Obtaining:

order to

The above equation can be rewritten as: d_k＝B_kX_k+e_k；

S2-6: updating the measurement noise variance:

wherein diag (-) denotes the diagonalization of the matrix, m is the dimension of the measurement information,

d_i，krepresents D_kThe ith element of (b)_i，kIs represented by B_kRow i element of (1);

s2-7: computing a cross-covariance matrix P between state information and metrology information_xz，k|k-1，And let i equal to 0;

s2-8: order to

And setting chi-square distribution

Where θ represents the degree of freedom of the chi-squared distribution and α represents the quantile of the chi-squared distribution if

Then calculate

Returning to the step S2-2 to continue executing the next filtering cycle;

S2-9：

if it is not

Computing

Wherein, i ═ i +1, i ═ i · is obtained according to the adaptive fading factor τ_kUpdate a_k(i) A value of (a) is judged_k(i) Whether or not to satisfy

If not, returning to the step S2-9 to continue execution; if so, obtaining the optimal adaptive fading factor tau_kThen continuing to perform subsequent steps;

s2-10: using an adaptive fading factor tau_kUpdating predicted state prediction covariance：

Calculating a Kalman gain:

estimating a posterior state and a posterior covariance matrix:

returning to step S2-2 continues to execute the next filtering cycle.

Further, the step S3 includes the following steps:

s3-1: after the INS/GNSS subsystem and the INS/CNS subsystem are respectively executed with filtering processes, corresponding local posterior state estimation can be obtained

For the convenience of differentiation, the INS/GNSS subsystem is made to obtain a local posterior state estimate as

The INS/CNS subsystem obtains a local posterior state estimate of

53-2: obtaining global optimal state estimation according to the minimum variance principle and the volume criterion:

wherein let beta be [ beta ]₁，β₂]^T，

P₁₁P derived from performing local state estimation for INS/GNSS subsystems_k|k，P₂₂P derived from performing local state estimation for INS/CNS subsystems_k|k，P₁₂To represent

And

cross covariance matrix of estimation errors, P₂₁To represent

And

a cross-covariance matrix of the errors is estimated.

Further, P₁₂And P₂₁Approximated by the volume criterion:

wherein the content of the first and second substances,

respectively representing the propagation volume points obtained by the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2-2

And

respectively representing the predicted values of the state vectors obtained by the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2-3,and

represents the new volume point X obtained by the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2-4 respectively_i，k|k-1，And

respectively indicating the predicted values of the measurement information obtained by the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2-4

And

respectively representing the filter gains obtained by the INS/GNSS subsystem and the INS/CNS subsystem executing the local state estimation process

I denotes an n-dimensional identity matrix.

Has the advantages that: compared with the prior art, the invention respectively introduces the self-adaptive fading factor and the maximum correlation entropy criterion to carry out the local state estimation of the INS/GNSS subsystem and the INS/CNS subsystem in the time updating stage and the measurement updating stage of the generalized high-order CKF, and finally fuses the local estimation of the INS/CNS subsystem and the INS/GNSS subsystem according to the minimum variance principle and the volume criterion, under the condition that the ballistic missile INS/CNS/GNSS combined navigation system has process modeling error and measurement noise is in non-Gaussian distribution in a system model, the integrated navigation system can still obtain the global optimal state estimation, the invention can simultaneously inhibit the influence of process modeling errors and non-Gaussian measurement noise on the state estimation, improve the adaptivity and robustness of ballistic missile INS/CNS/GNSS integrated navigation, therefore, the navigation precision of the INS/CNS/GNSS combined navigation system is ensured.

Drawings

FIG. 1 is a flow chart of the method of the present invention.

Detailed Description

The invention is further elucidated with reference to the drawings and the embodiments.

As shown in fig. 1, the present invention provides an optimal data fusion method suitable for a ballistic missile INS/CNS/GNSS integrated navigation system, comprising the following steps:

s1: constructing an INS/CNS/GNSS combined navigation system filtering model:

s1-1: setting the state vector of the INS/CNS/GNSS integrated navigation system as follows:

x＝[φ_x，φ_y，φ_z，δv_x，δv_y，δv_z，x，y，z，ε_x，ε_y，ε_z，Δ_x，Δ_y，Δ_z]^T (1)

wherein phi_x，φ_y，φ_zRepresenting the angular misalignment of the attitude, δ v, under the inertial system of the emission point_x，δv_y，δv_zRepresenting the velocity error in the inertial system of the emission point, x, y, z representing the position error in the inertial system of the emission point, epsilon_x，ε_y，ε_zRepresenting the gyro constant drift, Delta, in a projectile coordinate system_x，Δ_y，Δ_zAnd T is a transposed symbol and represents the constant bias of the accelerometer in the missile coordinate system.

S1-2: establishing a state equation of the system according to a 15-dimensional state vector x of the INS/CNS/GNSS combined navigation system:

x_k＝f(x_k-1)+v_k-1 (2)

wherein f (-) is a nonlinear system function, x_k-1And x_kState vectors, v, representing time k-1 and k, respectively_k-1Representing process noise, v_k-1Has a covariance of

S1-3: respectively establishing measurement equations of an INS/GNSS subsystem and an INS/CNS subsystem:

in an INS/GNSS subsystem, the INS and the GNSS output position and speed are respectively differenced and used as measurement information to establish a measurement equation of the subsystem:

z_1，k＝H_1，kx_k+ω_1，k (3)

wherein z is_1，kMeasurement vector, H, representing INS/GNSS subsystem time k_1，kMeasurement matrix, ω, representing INS/GNSS subsystem k time_1，kRepresents the measurement noise at the moment k of the INS/GNSS subsystem, the variance of which is

In an INS/CNS subsystem, a measurement equation of the subsystem takes the difference value of the attitude angles output by the INS and the CNS as measurement information, and the measurement equation of the subsystem is established as follows:

z_2，k＝H_2，kx_k+ω_2，k (4)

wherein z is_2，kThe measurement vector, H, representing the INS/CNS subsystem at time k_2，kMeasurement matrix, ω, representing INS/CNS subsystem k time_2，kRepresents the measured noise at the moment k of the INS/CNS subsystem with a variance of

S2: respectively introducing a self-adaptive fading factor and a maximum correlation entropy criterion to carry out local state estimation of an INS/CNS subsystem and an INS/GNSS subsystem in a time updating stage and a measurement updating stage of the generalized high-order CKF, wherein the specific process is as follows:

s2-1: because the INS/GNSS subsystem and the INS/CNS subsystem perform local state estimation through the local filter 1 and the local filter 2 respectively by using the same filtering process, in order to avoid repetition, only the filtering process of the INS/GNSS subsystem is specifically described here: therefore, the present embodiment converts z in step S1_1，k、z_2，kIs uniformly written as z_k，H_1，kAnd H_2，kWrite uniformly as H_k，ω_1，kAnd ω_2，kWrite uniformly to omega_k，R_1，kAnd R_2，kIs written uniformly as R_k. According to INS/GNSSThe local state estimation of the system requires setting the value of kernel width gamma, and setting the initial state vector, state error covariance and fading factor as

τ_k(0)＝1，S_0|0＝chol(P_0|0) Chol (·) denotes the choles decomposition operation.

S2-2: calculating a propagation volume point according to the formula (5)

Where i 1., 2n²+1, n represents the dimension of the state vector,[·]_ia set of representations [ ·]Column i.

S2-3: predicting a state vector and an error covariance matrix at the k moment:

wherein

S2-4: according to

Yang S_k|k-1Generate new volume points:

wherein S is_k|k-1＝chol(P_k|k-1)。

S2-5: prediction of measurement information at time k:

s2-6: measurement equations and from INS/GNSS subsystemsAnd P_k|k-1The following equation was constructed:

wherein, I represents an identity matrix,M_p，k|k-1＝chol(P_k|k-1)、M_r，k＝chol(R_k) And

in the equation

Is multiplied by

Obtaining:

order to

Equation (11) can be rewritten as:

D_k＝B_kX_k+e_k (12)

s2-7: updating the measurement noise variance:

wherein diag (-) denotes the diagonalization of the matrix, m is the dimension of the measurement information,

d_i，krepresents D_kThe ith element of (b)_i，kIs represented by B_kRow i element of (1).

S2-8: computing a cross-covariance matrix P between state information and metrology information_xz，k|k-1：

Let i be 0 and/or n be 0,

and setting chi-square distribution

Where θ represents the degree of freedom of the chi-squared distribution and α represents the quantile of the chi-squared distribution;

if it is not

Then calculate:

S_k|k＝chol(P_k|k) (19)

continuing to execute k-k +1, and returning to the position of the formula (5) in the step S2-2 to start executing the next filtering cycle;

if it is not

And (3) calculating:

according to the obtained adaptive fading factor tau_kUpdate a_k(i) A value of (a) is judged_k(i) Whether or not to satisfy

If the value is not satisfied, i is equal to i +1, the formula (20) is returned to update the extinction factor τ_kA value of (d); if so, obtaining the optimal adaptive fading factor tau_kThen the following steps are continued.

S2-9: using the obtained adaptive fading factor tau_kUpdate predicted state prediction covariance:

calculating a Kalman gain:

s2-10: estimating a posterior state and a posterior covariance matrix:

S_k|k＝chol(P_k|k) (25)

k is executed as k +1, and the execution of the next filtering cycle is started returning to the position of the formula (5) in step S2-2.

S3: and (3) fusing local estimation of the INS/GNSS subsystem and the INS/CNS subsystem according to a minimum variance principle and a volume criterion to obtain a global optimal state estimation:

s3-1: after the INS/GNSS subsystem and the INS/CNS subsystem are respectively executed with the filtering process, corresponding local posterior state estimation can be obtained

For the convenience of differentiation, the INS/GNSS subsystem is made to obtain a local posterior state estimate as

The INS/CNS subsystem obtains a local posterior state estimate of

S3-2: obtaining global optimal state estimation according to the minimum variance principle and the volume criterion:

wherein let beta be [ beta ]₁，β₂]^T，

P₁₁P derived from performing local state estimation for INS/GNSS subsystems_k|k，P₂₂P derived from performing local state estimation for INS/CNS subsystems_k|k，P₁₂To represent

Andcross covariance matrix of estimation errors, P₂₁To represent

Andcross covariance matrix of estimation errors, P₁₂And P₂₁Approximated by the volume criterion:

wherein the content of the first and second substances,respectively representing the propagation volume points obtained during the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2

And

respectively representing the predicted values of the state vectors obtained by the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2,

and

respectively representing INS/GNSS sub-elementsThe system and the INS/CNS subsystem perform the new volume point X obtained during step S2_i，k|k-1，

And

respectively representing the predicted values of the measurement information obtained by the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2

Andrespectively representing the filter gains obtained during the INS/GNSS subsystem and the INS/CNS subsystem executing the step S2

I denotes an n-dimensional identity matrix.