A combined navigation data fusion method based on a robust cubature point update framework

Abstract

The present invention relates to the field of integrated navigation and information fusion, and specifically discloses a combined navigation data fusion method based on a robust cubature point update framework. The method constructs a robust cubature point update framework based on the maximum likelihood criterion and by approximating the likelihood function without regenerating new cubature points, improves the robustness of the data fusion method using maximum likelihood estimation, and proposes to approximate the likelihood function using instantiated points and directly update the posterior cubature points. The above cubature point update strategy is incorporated into the Gaussian filtering framework to achieve robust data fusion for combined navigation based on the new cubature point update framework, improving the combined navigation data fusion accuracy in the case of process uncertainty in the system model and measurement information loss.

Description

Technical Field

[0001] The present invention relates to the field of integrated navigation and information fusion, and particularly to an integrated navigation data fusion method based on a robust cubature point update framework. Background Art

[0002] In an integrated navigation system, the Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Cubature Kalman Filter (CKF) are often used for data fusion. Among them, the EKF uses the Taylor series expansion to simply locally linearize the nonlinear system equation, and there are serious model description errors in the linearized system; the UKF uses a set of selected sigma points to approximate the probability distribution of the state, overcoming the errors caused by the local linearization of the EKF algorithm. However, since the central point weights of the unscented transform may be negative, it leads to the numerical instability of the UKF algorithm; compared with the UKF, the CKF has better numerical stability, and the CKF algorithm is derived based on the third-order cubature criterion, which can only guarantee the third-order approximation accuracy, so it is not suitable for application scenarios with high precision requirements; the high-order CKF algorithm has higher estimation accuracy compared to the traditional CKF algorithm. In practical applications, the integrated navigation system model is usually a theoretical approximation of the real system model. Especially in some integrated navigation systems with high real-time requirements, such a theoretical approximation model will inevitably have process uncertainties. In addition, due to the influence of the external environment, the integrated navigation system usually has the loss of measurement information. Both the process uncertainty and the loss of measurement information in the integrated navigation system will cause a serious decline in the performance of the integrated navigation data fusion algorithm, resulting in a decline in the navigation accuracy of the integrated navigation system and even the unavailability of navigation information. Therefore, how to achieve high-precision fusion of the integrated navigation system data in the above situations is an urgent problem to be solved. Summary of the Invention

[0003] Object of the Invention: In order to overcome the deficiencies of the prior art, the present invention provides an integrated navigation data fusion method based on a robust cubature point update framework, which overcomes the problem of the decline in the navigation accuracy of the integrated navigation system in the case of the integrated navigation system containing uncertainties and the loss of measurement information.

[0004] Technical Solution: The integrated navigation data fusion method based on a robust cubature point update framework of the present invention includes the following steps:

[0005] (1) Construct the conditional probability density of the measurement using the statistical information of the innovation vector, and then introduce an estimation window with a fixed length memory into the maximum likelihood criterion to estimate the covariance matrix of the process noise;

[0006] (2) Approximate the likelihood function using the predictive volume point error matrix of the likelihood function and the posterior volume point error matrix obtained through the linear transformation of the model prediction residuals, and directly use it to update the posterior volume points, thereby constructing a new volume point update framework;

[0007] (3) Incorporate the covariance matrix of the process noise estimated in step (1) and the volume point update strategy described in step (2) into the high-order cubature Kalman filter framework, and a combined navigation data fusion method with high robustness and accuracy can be obtained.

[0008] Preferably, the specific operation method of step (1) is as follows:

[0009] First, express the conditional probability density function of the measurement z k as:

[0010]

[0011] where k represents the time, Q represents the covariance matrix of the process noise, z represents the measurement, represents the predicted value of the measurement at time k, represents the innovation vector at time k, represents the measurement prediction error covariance matrix, and m represents the dimension of the measurement.

[0012] According to algebraic operations, calculate the likelihood function of the measurement vector (z k-N+1 , z k-N+2 , …, z k ) through Equation (44).

[0013]

[0014] where N represents the size of the fixed-length memory estimation window.

[0015] Finally, according to the maximum likelihood criterion, the covariance matrix Q of the process noise can be obtained by solving the following optimization problem

[0016]

[0017] Preferably, the solution process of the optimization problem described in Equation (45) is as follows:

[0018] Take the logarithm operation on both sides of Equation (44) and ignore the constant term, and Equation (45) can be further rewritten as:

[0019]

[0020] To facilitate the solution of the optimization problem described in Equation (46), define the following cost function.

[0021]

[0022] Taking the partial derivatives of the above cost function with respect to the elements of Q and setting them equal to 0, the following maximum likelihood equation can be obtained:

[0023]

[0024] where \(i, l = 1, 2, \cdots, n\), \(n\) represents the dimension of the state variables, \(tr(\cdot)\) represents the trace operation of a matrix, and \(Q_{ij}\) il represents the element in the \(i\)-th row and \(j\)-th column of Q.

[0025] By performing a Taylor expansion on the estimated value of the integrated navigation system state variables at time \(k - 1\), the prediction error of the state variables at time \(k\) can be expressed as:

[0026]

[0027] where represents the second-order and higher-order terms in the Taylor expansion coefficients, represents the estimated error of the integrated navigation system state variables at time \(k - 1\), and \(w_k\) k represents the process noise of the integrated navigation system.

[0028] By introducing a diagonal matrix \(\beta\) k \(= diag(\beta_1, \beta_2, \cdots, \beta_n)\) 1,k , \(\beta_2\) 2,k , \(\cdots\), \(\beta_n\) n,k ) to represent the first-order linear error in Equation (49), then Equation (49) can be rewritten as:

[0029]

[0030] Furthermore, the predicted covariance matrix of the state variable error can be expressed as:

[0031]

[0032] Taking the partial derivatives of Equation (51) and the predicted error covariance matrix of the measurement with respect to \(Q_{ij}\). When the filtering process within the estimation window reaches a steady state, il the first-order term of the result can be ignored, and then the following can be obtained:

[0033]

[0034] where \(H_{ij}\) k represents the observation matrix.

[0035] Substituting Equation (52) into Equation (48) gives:

[0036] ​

[0037] where \(l, i = 1, 2, \ldots, n\).

[0038] According to the filtering gain formula Equation (53) can be further rewritten as:

[0039]

[0040] where \(l, i = 1, 2, \ldots, n\).

[0041] Assume that the filtering process within the estimation window is in a stable state, such that For all \(j (j = k - N + 1, k - N + 2, \ldots, k)\) can be approximated as a constant. Therefore, Equation (54) can be rewritten as:

[0042]

[0043] where \(l, i = 1, 2, \ldots, n\), and when Equation (56) is satisfied, Equation (55) always holds.

[0044]

[0045] According to the Kalman filtering process, there exists:

[0046]

[0047]

[0048]

[0049] Substituting Equations (57), (58), and (59) into Equation (56), the maximum likelihood estimate \(\hat{Q}\) of the process noise covariance matrix \(Q\) can be obtained ML .

[0050]

[0051] Preferably, the specific operation method of step (2) is as follows:

[0052] Define the system prior cubature point error matrix the posterior cubature point error matrix and the corresponding weight matrix \(W\) as:

[0053]

[0054]

[0055]

[0056] where and respectively represent the predicted value and the estimated value of the state quantity at time k, X i,kk-1 = f(ξ i,k-1 ), f(·) represents the integrated navigation system function, ξ i,k-1 represents the cubature point, ω i represents the cubature point weight, i = 1, 2, …, 2n 2 +1, diag(·) represents the diagonal operation of the matrix, and n represents the dimension of the state quantity.

[0057] Assume can be represented by , then the following constraint equations hold.

[0058]

[0059]

[0060]

[0061] Among them, represents the predicted error covariance matrix of the state quantity at time k, represents the covariance matrix of the estimated error of the state quantity at time k, ΔR k represents the uncertainty matrix caused by noise, Λ k represents the scale matrix, K k represents the filtering gain, and R represents the measurement noise covariance matrix.

[0062] Under the constraints of equations (65) and (66), we can obtain:

[0063]

[0064] Among them, chol(·) represents the Cholesky decomposition.

[0065] Finally, according to equations (62) and (67), the newly generated cubature points can be represented as:

[0066]

[0067] Preferably, the specific operation method of step (3) is as follows:

[0068] 1) Initialize the cubature points

[0069] Initialize the process noise covariance matrix Q and the measurement noise covariance matrix R, and let Q ML = Q, then calculate the cubature points and the corresponding weights according to equations (69) and (70).

[0070]

[0071]

[0072] Among them, 0 n represents a zero column vector of size n dimensions, I n represents the identity matrix, e k and e l respectively represent the k-th column and the l-th column of the identity matrix I n , represents a matrix of row 1 column.

[0073] 2) Time update

[0074] Calculate the predicted value of the combined navigation system state quantity and the corresponding error covariance matrix

[0075]

[0076]

[0077] Then, calculate the prior cubature point error matrix

[0078] X i,kk-1 = f(ξ i,k-1 ), i = 1, 2,..., 2n 2 + 1 (73)

[0079]

[0080] Update according to Equation (75).

[0081]

[0082] Generate the propagated cubature points for approximating the likelihood function according to Equation (76).

[0083]

[0084] Among them, represents the i-th column element of

[0085] 3) Measurement update

[0086] Calculate the predicted value of the measurement quantity The measurement prediction error covariance matrix and the cross-covariance matrix between the predicted state quantity and the predicted measurement quantity

[0087]

[0088]

[0089]

[0090] wherein, H k represents the observation matrix,

[0091] Then, the estimated value of the state quantity and the corresponding covariance matrix are updated.

[0092]

[0093]

[0094]

[0095] 4) Cubature point update

[0096] Calculate the posterior cubature point error matrix according to Equation (83)

[0097]

[0098] Then, generate the cubature points for the (k + 1)-th filtering according to Equation (84).

[0099]

[0100] wherein, represents the i-th column element of

[0101] Finally, let k = k + 1, and return to step 2) to execute the next moment, that is, the next filtering period.

[0102] Beneficial effects: Compared with the prior art, the significant advantages of the present invention are: 1. The present invention discloses a combined navigation data fusion method based on a robust cubature point update framework, which solves the problem of the decline in the navigation accuracy of the combined navigation system in the presence of process uncertainty and measurement information loss in the combined navigation system; 2. Without changing the hardware structure of the combined navigation system, the stability and navigation accuracy of the combined navigation system are improved through software means. BRIEF DESCRIPTION OF THE DRAWINGS

[0103] Figure 1 is a working schematic diagram of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0104] Next, in combination with the accompanying drawings in the specific embodiments of the present invention, the technical solutions in the specific embodiments of the present invention will be clearly and completely described. Obviously, the described specific embodiments are only one specific embodiment of the present invention, rather than all specific embodiments. Based on the specific embodiments of the present invention, all other specific embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0105] As Figure 1 shown, the present invention discloses a multi-frequency INS / CNS integrated navigation method based on artificial intelligence, including the following steps:

[0106] (1) Use the statistical information of the innovation vector to construct the conditional probability density of the measurement, and then introduce an estimation window with a fixed-length memory into the maximum likelihood criterion to estimate the covariance matrix of the process noise. First, express the conditional probability density function of the measurement z k as:

[0107]

[0108] where k represents the time, Q represents the covariance matrix of the process noise, z represents the measurement, represents the predicted value of the measurement at time k, represents the innovation vector at time k, represents the covariance matrix of the measurement prediction error, and m represents the dimension of the measurement.

[0109] According to algebraic operations, calculate the likelihood function of the measurement vector (z k-N+1 , z k-N+2 , …, z k ) through Equation (86).

[0110]

[0111] where N represents the size of the fixed-length memory estimation window.

[0112] Finally, according to the maximum likelihood criterion, the problem of solving the covariance matrix Q of the process noise can be transformed into the solution process of the following optimization problem,

[0113]

[0114] Take the logarithmic operation on both sides of Equation (86) and ignore the constant term, and Equation (87) can be further rewritten as:

[0115]

[0116] To facilitate the solution of the optimization problem shown in Equation (88), the following cost function is defined.

[0117]

[0118] Taking the partial derivative of the above cost function with respect to each element of Q and setting it equal to 0, the following maximum likelihood equation can be obtained:

[0119]

[0120] where \(i, l = 1, 2, \ldots, n\), \(n\) represents the dimension of the state variables, \(tr(\cdot)\) represents the trace operation of the matrix, and \(Q_{ij}\) il represents the element in the \(i\)-th row and \(j\)-th column of Q.

[0121] By performing a Taylor expansion on the estimated value of the integrated navigation system state variables at time \(k - 1\), the prediction error of the state variables at time \(k\) can be expressed as:

[0122]

[0123] where represents the second-order and higher-order terms in the Taylor expansion coefficients, represents the estimation error of the integrated navigation system state variables at time \(k - 1\), and \(w_k\) k represents the process noise of the integrated navigation system.

[0124] By introducing a diagonal matrix \(\beta\) k \(= diag(\beta_1, \beta_2, \ldots, \beta_n)\) 1,k , \(\beta_2\) 2,k , \(\ldots\), \(\beta_n\) n,k ) to represent the first-order linear error in Equation (91), then Equation (91) can be rewritten as:

[0125]

[0126] Furthermore, the predicted covariance matrix of the state variable error can be expressed as:

[0127]

[0128] Taking the partial derivative of Equation (93) and the predicted covariance matrix of the measurement error with respect to \(Q_{ij}\). When the filtering process within the estimation window reaches a steady state, il the first-order term of the result can be ignored, and then the following can be obtained:

[0129]

[0130] where \(H_{ij}\) k k represents the observation matrix.

[0131] Substituting Equation (94) into Equation (90) gives:

[0132]

[0133] where \(l, i = 1, 2, \ldots, n\).

[0134] According to the filtering gain formula Equation (95) can be further rewritten as:

[0135]

[0136] where \(l, i = 1, 2, \ldots, n\).

[0137] Assume that the filtering process within the estimation window is in a steady state, such that is approximately a constant for all \(j (j = k - N + 1, k - N + 2, \ldots, k)\). Therefore, Equation (96) can be rewritten as:

[0138]

[0139] where \(l, i = 1, 2, \ldots, n\), and Equation (97) holds identically when Equation (98) is satisfied.

[0140]

[0141] According to the Kalman filtering process, there exists:

[0142]

[0143]

[0144]

[0145] Substituting Equations (99), (100), and (101) into Equation (98), the maximum likelihood estimate \(\hat{Q}\) of the process noise covariance matrix \(Q\) can be obtained ML ,

[0146]

[0147] (2) Using the predicted cubature point error matrix of the likelihood function and the posterior cubature point error matrix obtained through the linear transformation of the model prediction residuals to approximate the likelihood function and directly used to update the posterior cubature points, a new cubature point update framework is constructed.

[0148] Define the system prior cubature point error matrix the posterior cubature point error matrix and the corresponding weight matrix \(W\) as:

[0149]

[0150]

[0151]

[0152] Among them, represents the predicted value of the state quantity at time k, X i,kk-1 = f(ξ i,k-1 ), f(·) represents the integrated navigation system function, and ξ i,k-1 represents the cubature point, ω i represents the cubature point weight, i = 1, 2, …, 2n 2 +1, n represents the dimension of the state quantity, and diag(·) represents the diagonal operation of the matrix.

[0153] Assume that can be represented by , then the following constraint equations hold.

[0154]

[0155]

[0156]

[0157] Among them, represents the covariance matrix of the state quantity prediction error at time k, represents the covariance matrix of the state quantity estimation error at time k, ΔR k represents the uncertainty matrix caused by noise, Λ k represents the scale matrix, and R represents the measurement noise covariance matrix.

[0158] Under the constraints of equations (107) and (108), we can obtain:

[0159]

[0160] Among them, chol(·) represents the Cholesky decomposition.

[0161] Finally, according to equations (104) and (109), the newly generated cubature points can be represented as:

[0162]

[0163] (3) Incorporate the covariance matrix of the process noise estimated in step (1) and the cubature point update strategy described in step (2) into the high-order cubature Kalman filter framework, and a combined navigation data fusion method with high robustness and accuracy can be obtained.

[0164] 1) Initialize the cubature points

[0165] Initialize the process noise covariance matrix Q and the measurement noise covariance matrix R, and let Q ML = Q, and then calculate the cubature points and the corresponding weights of the cubature points according to equations (111) and (112).

[0166]

[0167]

[0168] where, 0 n represents a zero column vector of size n, I n represents the identity matrix, e k and e l represent the k-th column and the l-th column of the identity matrix I n respectively, represents a matrix of 1 row and 1 column.

[0169] 2) Time update

[0170] Calculate the predicted value of the combined navigation system state quantity and the corresponding error covariance matrix

[0171]

[0172]

[0173] Then, calculate the prior cubature point error matrix

[0174] X i,kk-1 = f(ξ i,k-1 ), i = 1, 2,..., 2n 2 + 1 (115)

[0175]

[0176] According to equation (117), update it.

[0177]

[0178] Generate the propagated cubature points for approximating the likelihood function according to Equation (118).

[0179]

[0180] where denotes the i-th column element of

[0181] 3) Measurement update

[0182] Calculate the predicted value of the measurement the predicted error covariance matrix of the measurement and the cross-covariance matrix between the predicted value of the state quantity and the predicted value of the measurement

[0183]

[0184]

[0185]

[0186] H k denotes the observation matrix

[0187] Then, update the estimated value of the state quantity and the corresponding covariance matrix

[0188]

[0189]

[0190]

[0191] where K k denotes the filtering gain, and z k denotes the measurement value

[0192] 4) Cubature point update

[0193] Calculate the posterior cubature point error matrix according to Equation (125)

[0194]

[0195] Then, generate the cubature points for the (k + 1)-th filtering according to Equation (126).

[0196]

[0197] where denotes the i-th column element of ​

[0198] Finally, let k = k + 1, and return to step 2) to execute the next filtering period.

[0199] The specific embodiments described above further elaborate on the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only the specific embodiments of the present invention and is not used to limit the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. A combined navigation data fusion method based on a robust cubature point update framework, characterized in that Including the following steps: (1) Construct the conditional probability density of the measurement using the statistical information of the innovation vector, and then introduce an estimation window with a fixed-length memory into the maximum likelihood criterion to estimate the covariance matrix of the process noise; (2) Use the predicted cubature point error matrix of the likelihood function and the posterior cubature point error matrix obtained through the linear transformation of the model prediction residual to approximate the likelihood function, and directly use it to update the posterior cubature points, thereby constructing a new type of cubature point update framework; (3) Incorporate the covariance matrix of the process noise estimated in step (1) and the cubature point update strategy described in step (2) into the high-order cubature Kalman filter framework to obtain an updated integrated navigation data fusion method; The step (1) includes the following steps: First, express the conditional probability density function of the measurement as follows: (1) Among them, represents the time, represents the process noise covariance matrix, represents the measurement, represents the predicted value of the measurement at time represents the innovation vector at time represents the measurement prediction error covariance matrix, represents the dimension of the measurement, According to algebraic operations, the likelihood function of the measurement vector is calculated by formula (2). ​ (2) Among them, represents the size of the fixed-length memory estimation window, Finally, according to the maximum likelihood criterion, the covariance matrix of the process noise is obtained by formula (3). (3)； The step (2) includes the following steps: Define the prior volume point error matrix of the system , the posterior volume point error matrix and the corresponding weight matrix as follows: (4) (5) (6) Among them, and respectively represent the predicted value and the estimated value of the state quantity at a moment, , represents the integrated navigation system function, represents the cubature point, represents the cubature point weight, , represents the dimension of the state quantity, represents the diagonal operation of the matrix; By representation, the following constraint equations hold: (7) (8) (9) Among them, denotes the covariance matrix of the prediction error of the state quantity at the moment, denotes the covariance matrix of the estimation error of the state quantity at the moment, , denotes the uncertainty matrix caused by noise, , denotes the scale matrix, denotes the measurement noise covariance matrix, Under the constraints of equations (8) and (9), we get: (10) Among them, , , represents Cholesky decomposition, Finally, according to equations (5) and (10), the generated new cubature points are expressed as: (11)。 2. The combined navigation data fusion method based on the robust cubature point update framework according to claim 1, characterized in that, The step (3) includes the following steps: 1) Initialize the cubature points Initialize the process noise covariance matrix and the measurement noise covariance matrix , and let , then calculate the cubature points and the corresponding weights according to equations (12) and (13). (12) (13) Among them, represents a zero column vector of size dimension, represents the identity matrix, , , and respectively represent the th column and the th column of the identity matrix represents a matrix with 1 row and 1 column; 2) Time update Calculate the predicted value of the combined navigation system state variables and the corresponding error covariance matrix , (14) (15) Then, calculate the prior volume point error matrix , (16) (17) Update according to Equation (18) for and (18) Generate the propagated cubature points used to approximate the likelihood function according to equation (19), (19) Among them, represents the column elements.

3. The combined navigation data fusion method based on the robust cubature point update framework according to claim 2, wherein, After initializing the cubature points and performing the time update, measurement update and cubature point update are required: 3) Measurement update Predicted value of computational load measurement , covariance matrix of measurement prediction error and cross-covariance matrix between predicted value of state quantity and predicted value of measurement , (20) (21) (22) Among them, represents the observation matrix, Then, the estimated value of the state quantity and the corresponding covariance matrix are updated. (23) (24) (25) Among them, represents the filtering gain, represents the measured value, 4) Cubature point update Calculate the posterior volume point error matrix according to Equation (26). , (26) Then, the volume points for secondary filtering are generated according to Equation (27). (27) Among them, represents the column elements of Finally, let , and return to step 2) to perform the next filtering cycle.

4. The combined navigation data fusion method based on the robust cubature point update framework according to claim 1, wherein, When solving equation (3): Take the logarithm operation on both sides of equation (2) and ignore the constant term, then equation (3) is rewritten as: (28) To facilitate the solution of the optimization problem shown in equation (28), the following cost function is defined: (29) Take the partial derivatives of the above cost function with respect to each element and set them equal to 0 to obtain the following maximum likelihood equations: (30) Among them, , represents the trace operation of a matrix, represents the th row and th column element of By performing a Taylor expansion on the estimated value of the state variables of the integrated navigation system at a certain moment the prediction error of the state variables at a certain moment can be expressed as: ​ (31) Among them, , represents the second-order and higher-order terms in the Taylor expansion coefficients, represents the state quantity estimation error of the integrated navigation system at a certain moment, represents the process noise of the integrated navigation system, By introducing a diagonal matrix to represent the first-order linear error in Equation (31), Equation (31) is rewritten as: (32) The predicted covariance matrix of the state quantity error is expressed as: (33) For Equation (33) and the measurement prediction error covariance matrix Solve for the partial derivative of. When the filtering process within the estimation window reaches a steady state, the first-order term of the result is ignored, and then the following can be obtained: (34) Among them, represents the observation matrix, Substitute equation (34) into equation (30) to get: (35) Among them, , According to the filtering gain formula , Equation (35) is rewritten as: (36) Among them, , The filtering processes within the estimation window are all in a stable state, such that For all is approximated as a constant, and Equation (36) is rewritten as: (37) wherein, and when the formula (38) is satisfied, the equation (37) always holds. (38) According to the Kalman filter process, there is: (39) (40) (41) Substituting Equations (39), (40), and (41) into Equation (38), the maximum likelihood estimate of the process noise covariance matrix can be obtained. , (42)。

Images