Navigation data fusion method based on nonlinear observability

Abstract

The invention discloses a navigation data fusion method based on nonlinear observability, which comprises the following steps of: analyzing a plurality of data fusion weights by utilizing an observability theory, fusing a pseudo matrix of a Lie derivative and a volume Kalman filter, and then integrating a pseudo observation matrix and a pseudo state transition matrix into a Cramer-Rao lower bound, so as to obtain the navigation data fusion method based on nonlinear observability. And calculating the observability based on the lower bound to comprehensively consider the process noise and the observation noise of the system, thereby perfecting the analysis of the nonlinear observability, and dynamically adjusting the weight of each data in the fusion process by evaluating the observability of each data to realize the optimal fusion effect.

Description

Technical Field

[0001] The invention relates to the field of satellite navigation technology, in particular to a navigation data fusion method based on nonlinear observability. Background Art

[0002] The Global Satellite Navigation System (GNSS) is an air-based radio navigation and positioning system that can provide users with all-weather three-dimensional coordinates, speed and time information at any location on the Earth's surface or in near-Earth space. It includes one or more satellite constellations and their augmentation systems required to support specific tasks. The four major satellite navigation system suppliers announced by the International Committee on Global Navigation Satellite Systems include the United States' Global Positioning System GPS, Russia's GLONASS, the European Union's Galileo Satellite Navigation System (GALILEO) and China's BeiDou Satellite Navigation System (BDS).

[0003] The inertial navigation system (INS) is a navigation parameter calculation system that uses gyroscopes and accelerometers as sensitive devices. The system establishes a navigation coordinate system based on the output of the gyroscope and calculates the speed and position of the carrier in the navigation coordinate system based on the output of the accelerometer. INS is an autonomous navigation system that does not rely on external information and does not radiate energy to the outside. Its working environment includes not only air and ground, but also underwater. Its basic working principle is based on Newton's laws of mechanics. By measuring the acceleration of the carrier in the inertial reference system, integrating it over time, and transforming it into the navigation coordinate system, it is possible to obtain information such as speed, yaw angle and position in the navigation coordinate system.

[0004] Nonlinear observability refers to the degree to which the state variables of a system can be accurately inferred through observation data (i.e., output signals) when dealing with nonlinear dynamic systems. It measures the ability to recover the true value of a specific state component in the state space of the system from external observation data given the system model and observation sequence. In nonlinear systems, the accuracy of nonlinear observability is affected by the nonlinear properties of the system dynamics and observation models, and the observation process is complex, which may cause problems such as insufficient local information or singularity.

[0005] At present, a single data source is no longer sufficient as an information collection tool in industrial and military applications to meet actual needs. In the fields of target tracking, control system state estimation, etc., research on multiple data sources has flourished. How to effectively fuse multiple data to enhance their value in tracking, positioning, and state estimation has become an issue worthy of in-depth discussion. With the deepening of research, the concept of observability has become increasingly prominent in multi-source data fusion. The current research methods commonly used include dimensional expansion of measurement information, that is, under the premise of keeping the dimension of the observation vector unchanged, data fusion is achieved through minimum mean square error estimation and weighted observation data. Summary of the invention

[0006] The technical problem to be solved by the present invention is to provide a navigation data fusion method based on nonlinear observability, which uses observability theory to analyze the fusion weights of multiple data, and dynamically adjusts its weight in the fusion process by evaluating the observability of each data to achieve the optimal fusion effect.

[0007] The technical solution of the present invention is:

[0008] A navigation data fusion method based on nonlinear observability specifically comprises the following steps:

[0009] (1) Define the nonlinear function model, calculate the state equation and observation equation of the nonlinear system respectively, then define the observation space of the nonlinear observability, define the observability distribution based on the distribution of the observation space, then define the discriminant matrix of the nonlinear observability, and complete the calculation of the observability; define the nonlinear system model with process noise and observation noise, and calculate the Cramer-Rao lower bound of the nonlinear system at the kth moment through the Fisher information matrix;

[0010] (2) Using Lie derivatives to construct pseudo-state transfer matrices and pseudo-observation matrices of nonlinear systems by taking partial derivatives multiple times, using cubature Kalman filtering to obtain pseudo-state transfer matrices and pseudo-observation matrices under nonlinear filtering, and using condition numbers to fuse the above two pseudo-state transfer matrices and pseudo-observation matrices to obtain fused pseudo-state transfer matrices and pseudo-observation matrices;

[0011] (3) Add the pseudo observation matrix and the pseudo state transfer matrix to the Cramer-Rao lower bound, define the observability calculation matrix based on the Cramer-Rao lower bound fusion, and then obtain the observability of the nonlinear system based on the observability calculation matrix;

[0012] (4) Based on the observability of the nonlinear system, the observability of each data item is evaluated and its weight in the fusion process is dynamically adjusted to achieve the optimal fusion effect.

[0013] The nonlinear function model is shown in the following formula (1):

[0014] f(x)=[f1(x1,x2…,x n ), …, f n (x1,x2…,x n )] T (1);

[0015] In formula (1), the components of the nonlinear function f(x) are all related to the corresponding variables x = (x1, x2…, x n ) T It is a functional relationship, that is, each state of the space has a certain direction. From the perspective of state space distribution, it is a field composed of vectors.

[0016] There is a scalar function h(x k ) and the given vector field f(x k ), according to the relevant definition of Lie derivative, the nonlinear system model is obtained as shown in the following formula (2):

[0017]

[0018] In formula (2), f(x k-1 ) and h(x k ) are the state equation and observation equation of the nonlinear system respectively;

[0019] h(x k ) along f(x k-1 The Lie derivatives of each order of ) are shown in the following formula (3):

[0020]

[0021] From formula (3), we can get the following formula (4):

[0022]

[0023] Where n is the state dimension;

[0024] The observation space of nonlinear observability is defined as

[0025] Based on the distribution of observation space, the observability distribution is defined as dR1(x)=span{dR(x),R∈R1}, where span represents the entire space covered by a series of linear combinations of all vectors in the observation space R1. The observation space R1 is the smallest linear space containing state variables and observation variables, and is closed with respect to Lie derivatives.

[0026] The rank condition for observability is: dimdR1(x0) = n, which means that the full rank matrix is ​​locally observable at point x0. In nonlinear systems, the observability distribution dR1(x0) is a linear space spanned by the Lie derivatives of all observation functions h(x). These Lie derivatives describe the rate of change of the observation function along the direction of the state function f(x).

[0027] Based on the above analysis, the nonlinear observable discriminant matrix θ is defined as formula (5):

[0028]

[0029] The calculation formula of observability δ(x) is as follows (6):

[0030]

[0031] In formula (6), σ min (θ) and σ max (θ) refer to the minimum singular value and the maximum singular value in the discriminant matrix θ, respectively.

[0032] The specific steps of defining a nonlinear system model with process noise and observation noise and calculating the Cramer-Rao lower bound of the nonlinear system at the kth moment through the Fisher information matrix are as follows:

[0033] Define a nonlinear system model with process noise and observation noise, as shown in the following equation (7):

[0034]

[0035] In formula (7), x k ∈R1 n represents the column vector of the system state variables of the nonlinear system at the kth moment; z k ∈R1 p is the column vector of the system observation variables of the nonlinear system at time k; f(x k-1 ) is the state equation, h(x k ) is the observation equation; w k-1 represents the process noise at time step k-1 in the state equation of the nonlinear system model; v k Represents the observation noise at time step k in the observation equation of the nonlinear system model;

[0036] Q k-1 is the covariance matrix of the process noise, which describes the statistical characteristics of the noise in the system state transition process at time step k; n k is the covariance matrix of the observation noise, which describes the statistical characteristics of the noise in the observation data at time step k; the Fisher information matrix ι at time k kSee the following formula (8):

[0037]

[0038] The parameters in formula (8) The value of is shown in the following formula (9):

[0039]

[0040] According to the formula and The parameters in equation (9) can be simplified to equation (10):

[0041]

[0042] Substituting the Fisher information matrix into the equation (11) we can get:

[0043]

[0044] in, ι k is the Cramer-Rao lower bound of the nonlinear system at the kth time, and its initial value is is the inverse of the state covariance matrix at the initial moment, reflecting the measure of the uncertainty of the state variables at time step 0; x k represents the state vector at time step k; f(x k ) represents the state transition equation of the nonlinear system, describing the transition from the current state c k Move to next state x k+1 ;h(x k ) represents the observation equation, which maps the system state to a measurable output; E{·} is the mathematical expectation; D is the variance; Represents the lower triangular part of the matrix;

[0045] Δ represents the upper triangular part of the matrix; p represents the probability density function, p(x k |x k-1 ) represents the state variable x k Given the state variable x at the previous moment k-1 The probability density function of the case describes the state variable x at the current moment k How does it depend on the state variable x at the previous moment? k-1 ; c1 and c2 represent constant coefficients.

[0046] The pseudo-state transfer matrix Φ of the nonlinear system is constructed by using Lie derivatives to obtain partial derivatives multiple times. k1 With the pseudo observation matrix H k1 , see the following formula (12) for details:

[0047]

[0048] In formula (12), the pseudo-state transfer matrix Φ k1 is through the Lie derivative Calculated, the pseudo observation matrix H k1 is through the Lie derivative Calculated;

[0049] The given integral function of the cubature Kalman filter is shown in the following equation (13):

[0050]

[0051] In formula (13), i(f) is the integral to be calculated, R n is the integration region, f(x) is a nonlinear function model, and the Spherical-Radial basic principle is adopted. s is a vector representing a point in the integration region. In three-dimensional space, vector s is represented by three-dimensional coordinates (s x ,s y ,s z ) represents; r is the radial coordinate, which represents the distance from the origin. In three-dimensional space, the radial coordinate r represents the distance from the origin to point s;

[0052] The above equation (13) can be simplified into equation (14):

[0053]

[0054] Formula (14) represents the integration of f(x) in a multidimensional space; a i b j f(r i y j ) represents a product of polynomials, where a i and b j is the coefficient, r i y j Represents the radial coordinate r i and vector y j The product of

[0055] When the s×r sampling points adopt the third-order Spherical-Radial basic criterion, r=1, s=2n, the integral is expressed as the following formula (15):

[0056]

[0057] In formula (15), τ j is a volume point, and The weight is ω m=1 / m. When the third-order Spherical-Radial principle is adopted, the number of volume points is twice the state dimension, that is, m=2n, where n is the state dimension.

[0058] The time-updated volume calculation point is as follows (16):

[0059]

[0060] In formula (16), S k-1 =chol(P k-1 ), chol means to perform Lesky decomposition on the matrix and solve the upper and lower triangular matrices of the matrix;

[0061] The propagation of volume points is as follows (17):

[0062]

[0063] The state prediction is as follows (18):

[0064]

[0065] In formula (17) and formula (18), x m,k-1 Represents the state prediction value at time step k-1; is the state prediction value at time step k, which is the state prediction value x at the previous moment m,k-1 Obtained through nonlinear function transformation; ω m is the weight, which indicates the importance of mode m; f is a nonlinear function;

[0066] The covariance prediction is as follows (19):

[0067]

[0068] The cross-covariance prediction is as follows (20):

[0069]

[0070] The parameter x in formula (20) m,k|k-1 、z m,k|k-1 , The calculation of is shown in the following formula (21):

[0071]

[0072] Then the pseudo observation matrix based on cubature Kalman filtering is obtained as follows (22):

[0073]

[0074] The pseudo-state transfer matrix under nonlinear filtering is as follows (23):

[0075]

[0076] In formula (23), Φ k2 represents the pseudo-state transfer matrix under nonlinear filtering, N k is the pseudo-state transfer matrix under nonlinear filtering, which represents the pseudo-state transfer matrix at time step k; is the pseudo-observation matrix under nonlinear filtering, which represents the pseudo-observation matrix at time step k-1;

[0077] Then, the above two pseudo-state transfer matrices and pseudo-observation matrices are fused respectively through the condition number;

[0078] The pseudo-state transfer matrix after fusion and the pseudo observation matrix See the following formula (24) for details:

[0079]

[0080] In formula (24), α1, α2, β1, β2 are fusion coefficients;

[0081] When constructing the observability calculation matrix, it is necessary to invert the pseudo-state transfer matrix and the pseudo-observation matrix, and use the condition number of the matrix to fuse them so that the error of the observability inversion is minimized;

[0082] Define a matrix C as an n×n reversible matrix, δC is also an n×n reversible matrix, and the inversion error of matrix C is expressed as and Therefore, through the following formula get:

[0083] Among them, δC represents the perturbation of matrix C, which indicates the change of matrix C; cond(C) represents the condition number of matrix C, which indicates the stability of matrix C. The larger the condition number cond(C), the more unstable the matrix C is, and vice versa;

[0084] When the matrix is ​​ill-conditioned, it will cause a large error in the next step of calculation. That is, when constructing a new pseudo-observation matrix, it is necessary to make the ill-conditionedness of the pseudo-observation matrix as small as possible to reduce the impact of the ill-conditionedness of the matrix on the iterative calculation of observability.

[0085] Therefore, the optimal fusion coefficients are required: α1, α2, β1, β2 and cond(Φ1 * ): First, define i = 1, and then calculate the H constructed by Lie derivatives k1 ,Φ k1 H constructed with cubature Kalman filter k2 ,Φk2 ; Then set the initial values ​​of α1, α2, β1, β2 and calculate the condition number at this time and Assume that the fused condition number is the minimum condition number that satisfies the condition; then recursively calculate the condition number at the i-th moment Then compare the time i The condition number of the moment i+1 is greater than the condition number of the moment i. If the condition number of the moment i+1 is less than the condition number of the moment i, then the moment i+1 is the minimum condition number after fusion. Then record the α1, α2, β1, and β2 corresponding to the moment i+1. If the condition number of the moment i+1 is greater than the condition number of the moment i, swap the value of the moment i+1 with the condition number of the moment i, and then recursively calculate the condition number of the moment i. and Repeat the above steps until the condition number of the two fusion matrices at time i+1 is smaller than that at time i. After the comparison is completed, α1, α2, β1, and β2 corresponding to the moment with the smallest condition number are the optimal fusion coefficients.

[0086] The specific steps of adding the pseudo observation matrix and the pseudo state transfer matrix to the Cramer-Rao lower bound, defining the observability calculation matrix based on the Cramer-Rao lower bound fusion, and then obtaining the observability of the nonlinear system based on the observability calculation matrix are as follows:

[0087] The fused pseudo-state transfer matrix and the pseudo observation matrix Substituting into the Fisher information matrix, we get the following equation (25):

[0088]

[0089] In formula (25):

[0090]

[0091] The observability calculation matrix after Cramer-Rao lower bound fusion is defined as the following formula (26):

[0092]

[0093] In formula (25), is the inverse of the state covariance matrix; is the observation matrix; is the covariance matrix of the observation noise;

[0094] Then the observability calculation matrix of the established nonlinear system is processed to obtain the observability of the nonlinear system;

[0095] The global observable expression of the nonlinear system is as follows (27):

[0096]

[0097] In formula (27), ψ k is the local observability of the nonlinear system; Trace(ψ k ) represents the trace of the local observable degree of the nonlinear system, which indicates the size and change of the local observable degree;

[0098] Local component observability δ i The calculation of is shown in the following formula (28):

[0099]

[0100] The observability of each item of data specifically includes navigation data of a global satellite navigation system and navigation data of an inertial navigation system.

[0101] Advantages of the present invention:

[0102] The present invention uses observability theory to analyze the fusion weights of multiple data, and then integrates the pseudo-matrix of Lie derivative and cubature Kalman filter, and then integrates the pseudo-observation matrix and the pseudo-state transfer matrix into the Cramer-Rao lower bound. The observability is calculated based on the lower bound to comprehensively consider the process noise and observation noise of the system, thereby improving the analysis of nonlinear observability. By evaluating the observability of each data, its weight in the fusion process is dynamically adjusted to achieve the optimal fusion effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0103] Figure 1 It is a flow chart of solving the optimal fusion coefficient of the present invention. DETAILED DESCRIPTION

[0104] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0105] A navigation data fusion method based on nonlinear observability is characterized by comprising the following steps:

[0106] S11. Define a nonlinear function model, as shown in the following formula (1):

[0107] f(x)=[f1(x1,x2…,x n ), …, f n (x1,x2…,x n )] T (1);

[0108] In formula (1), the components of the nonlinear function f(x) are all related to the corresponding variables x = (x1, x2…, x n ) T It is a functional relationship, that is, each state of the space has a certain direction. From the perspective of state space distribution, it is a field composed of vectors.

[0109] There is a scalar function h(x k ) and the given vector field f(x k ), according to the relevant definition of Lie derivative, the nonlinear system model is obtained as shown in the following formula (2):

[0110]

[0111] In formula (2), f(x k-1 ) and h(x k ) are the state equation and observation equation of the nonlinear system respectively;

[0112] h(x k ) along f(x k-1 The Lie derivatives of each order of ) are shown in the following formula (3):

[0113]

[0114] From formula (3), we can get the following formula (4):

[0115]

[0116] Where n is the state dimension;

[0117] S12. Define the observation space of nonlinear observability as

[0118] Based on the distribution of observation space, the observability distribution is defined as dR1(x)=span{dR(x),R∈R1}, where span represents the entire space covered by a series of linear combinations of all vectors in the observation space R1. The observation space R1 is the smallest linear space containing state variables and observation variables, and is closed with respect to Lie derivatives.

[0119] The rank condition for observability is: dim dR1(x0) = n, which means that the full rank matrix is ​​locally observable at point x0. In nonlinear systems, the observability distribution dR1(x0) is a linear space spanned by the Lie derivatives of all observation functions h(x). These Lie derivatives describe the rate of change of the observation function along the direction of the state function f(x).

[0120] S13. Define the nonlinear observable discriminant matrix θ as formula (5):

[0121]

[0122] The calculation formula of observability δ(x) is as follows (6):

[0123]

[0124] In formula (6), σ min (θ) and σ max (θ) refers to the minimum singular value and the maximum singular value in the discriminant matrix θ respectively.

[0125] S14. Define a nonlinear system model with process noise and observation noise, as shown in the following formula (7):

[0126]

[0127] In formula (7), x k ∈R1 n represents the column vector of the system state variables of the nonlinear system at the kth moment; z k ∈R1 p is the column vector of the system observation variables of the nonlinear system at time k; f(x k-1 ) is the state equation, h(x k ) is the observation equation; w k-1 represents the process noise at time step k-1 in the state equation of the nonlinear system model; v k Represents the observation noise at time step k in the observation equation of the nonlinear system model;

[0128] Q k-1 is the covariance matrix of the process noise, which describes the statistical characteristics of the noise in the system state transition process at time step k; R k is the covariance matrix of the observation noise, which describes the statistical characteristics of the noise in the observation data at time step k; the Fisher information matrix ι at time k k See the following formula (8):

[0129]

[0130] The parameters in formula (8) The value of is shown in the following formula (9):

[0131]

[0132] According to the formula and The parameters in equation (9) can be simplified to equation (10):

[0133]

[0134] Substituting the Fisher information matrix into the equation (11) we can get:

[0135]

[0136] in, ι k is the Cramer-Rao lower bound of the nonlinear system at the kth time, and its initial value is is the inverse of the state covariance matrix at the initial moment, reflecting the measure of the uncertainty of the state variables at time step 0; x k represents the state vector at time step k; f(x k ) represents the state transition equation of the nonlinear system, describing the transition from the current state x k Move to next state x k+1 ;h(x k ) represents the observation equation, which maps the system state to a measurable output; E{·} is the mathematical expectation; D is the variance; Represents the lower triangular part of the matrix;

[0137] Δ represents the upper triangular part of the matrix; p represents the probability density function, p(x k |x k-1 ) represents the state variable x k Given the state variable x at the previous moment k-1 The probability density function of the case describes the state variable x at the current moment k How does it depend on the state variable x at the previous moment? k-1 ; c1 and c2 represent constant coefficients;

[0138] S21. Use Lie derivatives to construct the pseudo-state transfer matrix Φ of nonlinear systems by taking partial derivatives multiple times k1 With the pseudo observation matrix H k1 , see the following formula (12) for details:

[0139]

[0140] In formula (12), the pseudo-state transfer matrix Φ k1 is through the Lie derivative Calculated, the pseudo observation matrix H k1 is through the Lie derivative Calculated;

[0141] S22, the given integral function of the cubature Kalman filter is shown in the following formula (13):

[0142]

[0143] In formula (13), i(f) is the integral to be calculated, Rn is the integration region, f(x) is a nonlinear function model, and the Spherical-Radial basic principle is adopted. s is a vector representing a point in the integration region. In three-dimensional space, vector s is represented by three-dimensional coordinates (s x ,s y ,s z ) represents; r is the radial coordinate, which represents the distance from the origin. In three-dimensional space, the radial coordinate r represents the distance from the origin to point s;

[0144] The above equation (13) can be simplified into equation (14):

[0145]

[0146] Formula (14) represents the integration of f(x) in a multidimensional space; a i b j f(r i y j ) represents a product of polynomials, where a i and b j is the coefficient, r i y j Represents the radial coordinate r i and vector y j The product of

[0147] When the s×r sampling points adopt the third-order Spherical-Radial basic criterion, r=1, s=2n, the integral is expressed as the following formula (15):

[0148]

[0149] In formula (15), τ j is a volume point, and The weight is ω m =1 / m. When the third-order Spherical-Radial principle is adopted, the number of volume points is twice the state dimension, that is, m=2n, where n is the state dimension.

[0150] The time-updated volume calculation point is as follows (16):

[0151]

[0152] In formula (16), S k-1 =chol(P k-1 ), chol means to perform Lesky decomposition on the matrix and solve the upper and lower triangular matrices of the matrix;

[0153] The propagation of volume points is as follows (17):

[0154]

[0155] The state prediction is as follows (18):

[0156]

[0157] In formula (17) and formula (18), x m,k-1 Represents the state prediction value at time step k-1; is the state prediction value at time step k, which is the state prediction value x at the previous moment m,k-1 Obtained through nonlinear function transformation; ω m is the weight, which indicates the importance of mode m; f is a nonlinear function;

[0158] The covariance prediction is as follows (19):

[0159]

[0160] The cross-covariance prediction is as follows (20):

[0161]

[0162] The parameter x in formula (20) m,k|k-1 、z m,k|k-1 , The calculation of is shown in the following formula (21):

[0163]

[0164] Then the pseudo observation matrix based on cubature Kalman filtering is obtained as follows (22):

[0165]

[0166] The pseudo-state transfer matrix under nonlinear filtering is as follows (23):

[0167]

[0168] In formula (23), Φ k2 represents the pseudo-state transfer matrix under nonlinear filtering, N k is the pseudo-state transfer matrix under nonlinear filtering, which represents the pseudo-state transfer matrix at time step k; is the pseudo-observation matrix under nonlinear filtering, which represents the pseudo-observation matrix at time step k-1;

[0169] S23, respectively performing matrix fusion on the above two pseudo state transfer matrices and pseudo observation matrices through condition numbers;

[0170] The pseudo-state transfer matrix after fusion and the pseudo observation matrix See the following formula (24) for details:

[0171]

[0172] In formula (24), α1, α2, β1, β2 are fusion coefficients;

[0173] S24. When constructing the observability calculation matrix, it is necessary to find the inverse matrix of the pseudo-state transfer matrix and the pseudo-observation matrix, and use the condition number of the matrix to fuse them so that the error of the observability inversion is minimized;

[0174] Define a matrix C as an n×n reversible matrix, δC is also an n×n reversible matrix, and the inversion error of matrix C is expressed as and Therefore, through the following formula get:

[0175] Among them, δC represents the perturbation of matrix C, which indicates the change of matrix C; cond(C) represents the condition number of matrix C, which indicates the stability of matrix C. The larger the condition number cond(C), the more unstable the matrix C is, and vice versa;

[0176] When the matrix is ​​ill-conditioned, it will cause a large error in the next step of calculation. That is, when constructing a new pseudo-observation matrix, it is necessary to make the ill-conditionedness of the pseudo-observation matrix as small as possible to reduce the impact of the ill-conditionedness of the matrix on the iterative calculation of observability.

[0177] See Figure 1 , solve the optimal fusion coefficients: α1, α2, β1, β2 and cond(Φ1 * ): First, define i = 1, and then calculate the H constructed by Lie derivatives k1 ,Φ k1 H constructed with cubature Kalman filter k2 ,Φ k2 ; Then set the initial values ​​of α1, α2, β1, and β2, and calculate the condition number at this time and Assume that the fused condition number is the minimum condition number that satisfies the condition; then recursively calculate the condition number at the i-th moment Then compare the time i The condition number of the moment i+1 is greater than the condition number of the moment i. If the condition number of the moment i+1 is less than the condition number of the moment i, then the moment i+1 is the minimum condition number after fusion. Then record the α1, α2, β1, and β2 corresponding to the moment i+1. If the condition number of the moment i+1 is greater than the condition number of the moment i, swap the value of the moment i+1 with the condition number of the moment i, and then recursively calculate the condition number of the moment i. and Repeat the above steps until the condition number of the two fusion matrices at time i+1 is smaller than that at time i. After the comparison is completed, the α1, α2, β1, and β2 corresponding to the time with the smallest condition number are the optimal fusion coefficients.

[0178] S31, the fused pseudo-state transfer matrix and the pseudo observation matrix Substituting into the Fisher information matrix, we get the following equation (25):

[0179]

[0180] In formula (25):

[0181]

[0182] The observability calculation matrix after Cramer-Rao lower bound fusion is defined as the following formula (26):

[0183]

[0184] In formula (25), is the inverse of the state covariance matrix; is the observation matrix; is the covariance matrix of the observation noise;

[0185] Then the observability calculation matrix of the established nonlinear system is processed to obtain the observability of the nonlinear system;

[0186] The global observable expression of the nonlinear system is as follows (27):

[0187]

[0188] In formula (27), ψ k is the local observability of the nonlinear system; Trace(ψ k ) represents the trace of the local observable degree of the nonlinear system, which indicates the size and change of the local observable degree;

[0189] Local component observability δ i The calculation of is shown in the following formula (28):

[0190]

[0191] S4. Evaluate the observability of each data (navigation data of the global satellite navigation system and navigation data of the inertial navigation system) based on the observability of the nonlinear system, and dynamically adjust its weight in the fusion process to achieve the best fusion effect;

[0192] The fused nonlinear system is shown in the following equation (29):

[0193]

[0194] In formula (29), N is the total number of data sources; i represents the i-th data source; ul represents the nonlinear system; z l,k represents the observation value of sensor l at time step k; represents the observation matrix, which is used to describe how the state of a nonlinear system is observed; h(x k ) is the output function of the nonlinear system state; v k This is the observation noise, and Trace represents the trace of the matrix.

[0195] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A navigation data fusion method based on nonlinear observability, characterized by: The specific steps include: (1) Define the nonlinear function model, calculate the state equation and observation equation of the nonlinear system respectively, then define the observation space of the nonlinear observability, define the observability distribution based on the distribution of the observation space, then define the discriminant matrix of the nonlinear observability, and complete the calculation of the observability; define the nonlinear system model with process noise and observation noise, and calculate the Cramer-Rao lower bound of the nonlinear system at the kth moment through the Fisher information matrix; (2) Using Lie derivatives to construct pseudo-state transfer matrices and pseudo-observation matrices of nonlinear systems by taking partial derivatives multiple times, using cubature Kalman filtering to obtain pseudo-state transfer matrices and pseudo-observation matrices under nonlinear filtering, and using condition numbers to fuse the above two pseudo-state transfer matrices and pseudo-observation matrices to obtain fused pseudo-state transfer matrices and pseudo-observation matrices; (3) Add the pseudo observation matrix and the pseudo state transfer matrix to the Cramer-Rao lower bound, define the observability calculation matrix based on the Cramer-Rao lower bound fusion, and then obtain the observability of the nonlinear system based on the observability calculation matrix; (4) Based on the observability of the nonlinear system, the observability of each data item is evaluated and its weight in the fusion process is dynamically adjusted to achieve the optimal fusion effect.

2. The navigation data fusion method based on nonlinear observability according to claim 1, characterized in that: The nonlinear function model is shown in the following formula (1): f(x)=[f1(x1,x2…,x n ), …,f n (x1,x2…,x n )] T (1); In formula (1), the components of the nonlinear function f(x) are all related to the corresponding variables x = (x1, x2…, x n ) T It is a functional relationship, that is, each state of the space has a certain direction. From the perspective of state space distribution, it is a field composed of vectors. There is a scalar function h(x k ) and the given vector field f(x k ), according to the relevant definition of Lie derivative, the nonlinear system model is obtained as shown in the following formula (2): In formula (2), f(x k-1 ) and h(x k ) are the state equation and observation equation of the nonlinear system respectively; h(x k ) along f(x k-1 The Lie derivatives of each order of ) are shown in the following formula (3): From formula (3), we can get the following formula (4): Where n is the state dimension; The observation space of nonlinear observability is defined as Based on the distribution of observation space, the observability distribution is defined as dR1(x)=span{dR(x),R∈R1}, where span represents the entire space covered by a series of linear combinations of all vectors in the observation space R1. The observation space R1 is the smallest linear space containing state variables and observation variables, and is closed with respect to Lie derivatives. The rank condition for observability is: dimdR1(x0) = n, which means that the full rank matrix is ​​locally observable at point x0. In nonlinear systems, the observability distribution dR1(x0) is a linear space spanned by the Lie derivatives of all observation functions h(x). These Lie derivatives describe the rate of change of the observation function along the direction of the state function f(x). Based on the above analysis, the nonlinear observable discriminant matrix θ is defined as formula (5): The calculation formula of observability δ(x) is as follows (6): In formula (6), σ min (θ) and σ max (θ) refer to the minimum singular value and the maximum singular value in the discriminant matrix θ, respectively.

3. The navigation data fusion method based on nonlinear observability according to claim 2, characterized in that: The specific steps of defining a nonlinear system model with process noise and observation noise and calculating the Cramer-Rao lower bound of the nonlinear system at the kth moment through the Fisher information matrix are as follows: Define a nonlinear system model with process noise and observation noise, as shown in the following equation (7): In formula (7), x k ∈R1 n represents the column vector of the system state variables of the nonlinear system at the kth moment; z k ∈R1 p is the column vector of the system observation variables of the nonlinear system at time k; f(x k-1 ) is the state equation, h(x k ) is the observation equation; w k-1 represents the process noise at time step k-1 in the state equation of the nonlinear system model; v k Represents the observation noise at time step k in the observation equation of the nonlinear system model; Q k-1 is the covariance matrix of the process noise, which describes the statistical characteristics of the noise in the system state transition process at time step k; R k is the covariance matrix of the observation noise, which describes the statistical characteristics of the noise in the observation data at time step k; the Fisher information matrix ι at time k k See the following formula (8): The parameters in formula (8) The value of is shown in the following formula (9): According to the formula and The parameters in equation (9) can be simplified to equation (10): Substituting the Fisher information matrix into the equation (11) we can get: in, ι k is the Cramer-Rao lower bound of the nonlinear system at the kth time, and its initial value is is the inverse of the state covariance matrix at the initial moment, reflecting the measure of the uncertainty of the state variables at time step 0; x k represents the state vector at time step k; f(x k ) represents the state transition equation of the nonlinear system, describing the transition from the current state x k Move to next state x k+1 ;h(x k ) represents the observation equation, which maps the system state to a measurable output; E{·} is the mathematical expectation; D is the variance; Represents the lower triangular part of the matrix; Δ represents the upper triangular part of the matrix; p represents the probability density function, p(x k |x k-1 ) represents the state variable x k Given the state variable x at the previous moment k-1 The probability density function of the case describes the state variable x at the current moment k How does it depend on the state variable x at the previous moment? k-1 ; c1 and c2 represent constant coefficients.

4. The navigation data fusion method based on nonlinear observability according to claim 3, characterized in that: The pseudo-state transfer matrix Φ of the nonlinear system is constructed by using Lie derivatives to obtain partial derivatives multiple times. k1 With the pseudo observation matrix H k1 , see the following formula (12) for details: In formula (12), the pseudo-state transfer matrix Φ k1 is through the Lie derivative Calculated, the pseudo observation matrix H k1 is through the Lie derivative Calculated; The given integral function of the cubature Kalman filter is shown in the following equation (13): In formula (13), i(f) is the integral to be calculated, R n is the integration region, f(x) is a nonlinear function model, and the Spherical-Radial basic principle is adopted. s is a vector representing a point in the integration region. In three-dimensional space, vector s is represented by three-dimensional coordinates (s x ,s y ,s z ) represents; r is the radial coordinate, which represents the distance from the origin. In three-dimensional space, the radial coordinate r represents the distance from the origin to point s; The above equation (13) can be simplified into equation (14): Formula (14) represents the integration of f(x) in a multidimensional space; a i b j f(r i y j ) represents a product of polynomials, where a i and b j is the coefficient, r i y j Represents the radial coordinate r i and vector y j The product of When the s×r sampling points adopt the third-order Spherical-Radial basic criterion, r=1, s=2n, the integral is expressed as the following formula (15): In formula (15), τ j is a volume point, and The weight is ω m =1 / m. When the third-order Spherical-Radial principle is adopted, the number of volume points is twice the state dimension, that is, m=2n, where n is the state dimension. The time-updated volume calculation point is as follows (16): In formula (16), S k-1 =chol(P k-1 ), chol means to perform Lesky decomposition on the matrix and solve the upper and lower triangular matrices of the matrix; The propagation of volume points is as follows (17): The state prediction is as follows (18): In formula (17) and formula (18), x m,k-1 Represents the state prediction value at time step k-1; is the state prediction value at time step k, which is the state prediction value x at the previous moment m,k-1 Obtained through nonlinear function transformation; ω m is the weight, which indicates the importance of mode m; f is a nonlinear function; The covariance prediction is as follows (19): The cross-covariance prediction is as follows (20): The parameter x in formula (20) m,k|k-1 、z m,k|k-1 , The calculation of is shown in the following formula (21): Then the pseudo observation matrix based on cubature Kalman filtering is obtained as follows (22): The pseudo-state transfer matrix under nonlinear filtering is as follows (23): In formula (23), Φ k2 represents the pseudo-state transfer matrix under nonlinear filtering, N k is the pseudo-state transfer matrix under nonlinear filtering, which represents the pseudo-state transfer matrix at time step k; is the pseudo-observation matrix under nonlinear filtering, which represents the pseudo-observation matrix at time step k-1; Then, the above two pseudo-state transfer matrices and pseudo-observation matrices are fused respectively through the condition number; The pseudo-state transfer matrix after fusion and the pseudo observation matrix See the following formula (24) for details: In formula (24), α1, α2, β1, β2 are fusion coefficients; When constructing the observability calculation matrix, it is necessary to invert the pseudo-state transfer matrix and the pseudo-observation matrix, and use the condition number of the matrix to fuse them so that the error of the observability inversion is minimized; Define a matrix C as an n×n reversible matrix, δC is also an n×n reversible matrix, and the inversion error of matrix C is expressed as and Therefore, through the following formula get: Among them, δC represents the perturbation of matrix C, which indicates the change of matrix C; cond(C) represents the condition number of matrix C, which indicates the stability of matrix C. The larger the condition number cond(C), the more unstable the matrix C is, and vice versa; When the matrix is ​​ill-conditioned, it will cause a large error in the next step of calculation. That is, when constructing a new pseudo-observation matrix, it is necessary to make the ill-conditionedness of the pseudo-observation matrix as small as possible to reduce the impact of the ill-conditionedness of the matrix on the iterative calculation of observability. Therefore, the optimal fusion coefficients are required: α1, α2, β1, β2 and cond(Φ1 * ): First, define i = 1, and then calculate the H constructed by Lie derivatives k1 ,Φ k1 H constructed with cubature Kalman filter k2 ,Φ k2 ; Then set the initial values ​​of α1, α2, β1, β2 and calculate the condition number at this time and Assume that the fused condition number is the minimum condition number that satisfies the condition; then recursively calculate the condition number at the i-th moment Then compare the time i The condition number of the moment i+1 is greater than the condition number of the moment i. If the condition number of the moment i+1 is less than the condition number of the moment i, then the moment i+1 is the minimum condition number after fusion. Then, record the α1, α2, β1, and β2 corresponding to the moment i+1. If the condition number of the moment i+1 is greater than the condition number of the moment i, swap the value of the moment i+1 with the condition number of the moment i, and then recursively calculate the condition number of the moment i. and Repeat the above steps until the condition number of the two fusion matrices at time i+1 is found to be smaller than that at time i. After the comparison is completed, α1, α2, β1, and β2 corresponding to the moment with the smallest condition number are the optimal fusion coefficients.

5. The navigation data fusion method based on nonlinear observability according to claim 4, characterized in that: The specific steps of adding the pseudo observation matrix and the pseudo state transfer matrix to the Cramer-Rao lower bound, defining the observability calculation matrix based on the Cramer-Rao lower bound fusion, and then obtaining the observability of the nonlinear system based on the observability calculation matrix are as follows: The fused pseudo-state transfer matrix and the pseudo observation matrix Substituting into the Fisher information matrix, we get the following equation (25): In formula (25): The observability calculation matrix after Cramer-Rao lower bound fusion is defined as the following formula (26): In formula (25), is the inverse of the state covariance matrix; is the observation matrix; is the covariance matrix of the observation noise; Then the observability calculation matrix of the established nonlinear system is processed to obtain the observability of the nonlinear system; The global observable expression of the nonlinear system is as follows (27): In formula (27), ψ k is the local observability of the nonlinear system; Trace(ψ k ) represents the trace of the local observable degree of the nonlinear system, which indicates the size and change of the local observable degree; Local component observability δ i The calculation of is shown in the following formula (28):

6. The navigation data fusion method based on nonlinear observability according to claim 5, characterized in that: The observability of each item of data specifically includes navigation data of a global satellite navigation system and navigation data of an inertial navigation system.

Images