A multi-model adaptive robust combination navigation method

Abstract

This invention discloses a multi-model adaptive robust integrated navigation method. First, it overcomes the problem of low adjustment accuracy in adaptive and robust Kalman filters based on single adjustment factors by proposing an adaptive robust Kalman filter based on multiple adjustment factors. This filter retains the beneficial effects of beneficial channels while weakening the effects of harmful channels, effectively improving filtering speed and accuracy. Conventional adaptive robust unscented Kalman filters based on fading and enhancement factors sometimes cause the covariance matrix of state estimation to lose its positive definiteness, thus interrupting filtering calculations. This method effectively solves this problem by introducing a quadratic adjustment factor. Since the effects of fading adaptive and enhancement robust methods on the filter are contradictory, a multi-model information fusion strategy is proposed. This strategy automatically selects either an adaptive sub-filter or a robust sub-filter based on the error type, simultaneously compensating for motion model errors and measurement singularities.

Description

Technical Field

[0001] This invention belongs to the field of inertial navigation, specifically relating to a multi-model adaptive robust integrated navigation method. Background Technology

[0002] Inertial Navigation Systems (INS) are autonomous navigation and positioning systems that can operate independently of external information. They have high output frequencies, but their use is limited by the accumulation of constant drift in inertial devices over time. Global Positioning Systems (GNSS) can provide accurate velocity and position information, but their data refresh rate is low and their performance is susceptible to environmental interference. INS / GNSS-based integrated navigation systems combine the advantages of both independent systems to achieve a reliable solution for high-precision navigation and positioning under complex conditions. High-precision GNSS sensors can be acquired at low cost, but high-precision INS devices are expensive. MEMS-based INS are widely used in aerospace, marine, and underwater vehicles due to their low cost, small size, low power consumption, and high reliability. However, MEMS-INS suffers from large constant drift in inertial devices, which limits their application in high-precision navigation and positioning. Single-axis rotation modulation technology is a low-cost, system-level error compensation method that can effectively compensate for the constant drift of horizontal inertial devices. Therefore, it can effectively improve the performance of MEMS-INS and thus realize high-precision integrated navigation and positioning of MEMS-INS / GNSS.

[0003] In practical engineering applications, system models are often nonlinear. Unscented Kalman filters (UKFs) are widely used in nonlinear systems due to their low computational cost and high estimation accuracy. However, the optimal performance of UKFs depends on the accurate acquisition of the system's mathematical model. Motion model errors and measurement singularities are two common model errors that severely affect the filtering performance of UKFs. Adaptive filtering strategies based on fading factors are commonly used to compensate for motion model errors. Their basic idea is to correct the mean square error matrix of the predicted state variables one step using a fading factor, thereby enhancing the effect of fresh measurement information on the filter and weakening the effect of predicted information. Robust filtering strategies based on enhancement factors are commonly used to compensate for measurement singularities. Their basic idea is to correct the measurement noise covariance matrix using an enhancement factor, thereby enhancing the effect of predicted information on the filter and weakening the effect of fresh measurement information. Currently, both fading factors and enhancement factors are single variables and cannot effectively filter out harmful channel information while retaining the influence of beneficial channel information on the filter. Furthermore, the fading factor and the enhancement factor are two diametrically opposed adjustment strategies. The former amplifies the impact of fresh measurements on the filter, while the latter weakens it. Therefore, to simultaneously compensate for both motion model errors and measurement singularity errors, it is necessary to study adaptive robust filters based on multiple adjustment factors, including fading and enhancement.

[0004] Improving the real-time estimation accuracy of integrated navigation can also be achieved through hardware improvements, such as using high-performance measurement devices and computers. While this approach can improve the estimation accuracy of the algorithm to some extent, it increases the cost and complexity of the hardware, making it less suitable for widespread application in practical engineering. Summary of the Invention

[0005] To address the aforementioned issues, this invention discloses a multi-model adaptive robust integrated navigation method, which is validated on a single-axis rotating MEMS-INS / GNSS system with defined parameters. This method can simultaneously compensate for motion model errors and measurement singularities, achieving results that meet the requirements for high-precision navigation and positioning.

[0006] To achieve the above objectives, the technical solution of the present invention is as follows:

[0007] A multi-model adaptive robust integrated navigation method includes the following steps:

[0008] Step 1), for a single-axis rotation-modulated MEMS-INS, the Z-axis (upward axis) is the rotation axis, then the rotation modulation matrix is ​​as follows:

[0009]

[0010] Where u represents the MEMS-INS coordinate system, b represents the carrier coordinate system, θ represents the modulation angular velocity, and t represents time. Then, the constant drift vectors of the gyroscope and accelerometer in the b-frame can be expressed as follows:

[0011]

[0012]

[0013] Where, ε u and These represent the constant drift vectors of the gyroscope and accelerometer in the u-frame, respectively.

[0014] Step 2), the discrete filter model can be expressed as:

[0015]

[0016] in, This represents the filter state vector. n represents the navigation coordinate system, T represents the transpose operation, and k-1 and k represent time k-1 and time k, respectively. δv n δp and a represent the estimated velocity error, position error, and attitude error vectors, respectively. k H represents the measurement vector. k =[I 6*6 0 6*9 ] T Denotes the measurement transition matrix, where I 6*6 Represents a 6x6 identity matrix, 0 6*9 This represents a 6x9 zero matrix. k and v k Let Q represent the process noise and measurement noise vectors, respectively. Both are zero-mean Gaussian white noise, and their corresponding covariance matrices are Q and Q, respectively. k and R k f(·) represents the nonlinear state equation of the system.

[0017] Step 3), the one-step state estimation vector and covariance matrix of UKF are as follows:

[0018]

[0019]

[0020] in, x represents k The dimension of λ. i and This represents the correction coefficient. The intermediate variable g i，k / k-1 It can be calculated as:

[0021]

[0022]

[0023] in, Let P represent the state estimate vector at time k-1, and its covariance matrix be P. k-1 δ i and They represent The i-th column and Column vector, This represents the Cholesky decomposition operation. These are constant coefficients. The gain matrix can be represented as:

[0024]

[0025] Step 4), the calculation steps for the Multiple Elimination Factor Adaptive UKF (MAUKF) are as follows:

[0026] Substitute into the fading factor matrix F k In equation (6):

[0027]

[0028] Among them, F k For f k,11 f k,22 , ..., Form a diagonal matrix with diagonal elements. Then equation (9) can be updated to:

[0029]

[0030] Step 5), the calculation method for the fading factor is as follows:

[0031] Step 5.1): When the filter performance is optimal, the following chi-square test holds:

[0032]

[0033] Where m represents H k Rank. ξ represents the chi-square test value, where α represents the confidence level. k (i) represents ξ k The i-th element, where, a k，ii and b k，ii They represent and The i-th diagonal element. The fading factor can be calculated as follows:

[0034]

[0035] Among them, tk,ii and h k,ii T represents respectively k and H k The i-th diagonal element.

[0036] Step 5.2), to ensure P k-1 The positive definiteness is corrected by the decay factor as follows:

[0037] f k,ii =κ k (f k,ii -1)+1,0≤κ k ≤1, i=1,2,...,m (14)

[0038] Among them, κ k The second-order correction factor for the fading factor is calculated as follows:

[0039] Step 5.2.1), set κ k The maximum and minimum values ​​are respectively and k k , and k k The initial values ​​are 1 and 0 respectively, and the counter is set. The initial value and the maximum value are 0 and 0 respectively.

[0040] Step 5.2.2), let Substitution In equation (14), update the filter and P. k-1 .

[0041] Step 5.2.3), if P k-1 If it is positive definite, then If P k-1 If it is not positive definite, then

[0042] Step 5.2.4),

[0043] Step 5.2.5), if If the condition is met, return to step 5.2.2; otherwise, proceed to step 5.2.6.

[0044] Step 5.2.6), calculate based on P k-1 Value, denoted as P k-1 Calculation based on k k P k-1 Value, denoted as P k-1 ( kk If P k-1 If it is positive definite, then If P k-1 It is non-positive definite and P k-1 ( k k If κ is positive definite, then k = k k If P k-1 P k-1 ( k k If all κ are non-positive definite, then κ k =0.

[0045] Step 5.2.7), output κ k .

[0046] Step 6), the calculation steps for the Multi-Enhancement Factor Robust UKF (MRUKF) are as follows:

[0047] Substitute into the enhancement factor matrix E k In equation (9):

[0048]

[0049] Among them, E k For e k,11 e k,22 , ..., e k,mm A diagonal matrix consisting of diagonal elements.

[0050] Step 7), the enhancement factor is calculated as follows:

[0051] Step 7.1): When the filter performance is optimal, the following chi-square test holds:

[0052]

[0053] Among them, c k，ii and d k，ii They represent and The i-th diagonal element. The enhancement factor can be calculated as follows:

[0054]

[0055] Where, r k,ii For R k The i-th diagonal element.

[0056] Step 7.2), to ensure P k-1 The positive definiteness is corrected by the enhancement factor as follows:

[0057]

[0058] in, The calculation steps for the second-order correction factor of the enhancement factor are as follows:

[0059] Step 7.2.1), set The maximum and minimum values ​​are respectively and k k , and k k The initial values ​​are 1 and 0 respectively, and the counter is set. The initial value and the maximum value are 0 and 0 respectively.

[0060] Step 7.2.2), let Substitution In equation (18), update the filter and P. k-1 .

[0061] Step 7.2.3), if P k-1 If it is positive definite, then If P k-1 If it is not positive definite, then

[0062] Step 7.2.4),

[0063] Step 7.2.5), if If the condition is met, return to step 7.2.2; otherwise, proceed to step 7.2.6.

[0064] Step 7.2.6), calculate based on P k-1 Value, denoted as P k-1 Calculation based on k k P k-1 Value, denoted as P k-1 ( k k If P k-1 If it is positive definite, then If P k-1 It is non-positive definite and P k-1 ( k k If ) is positive definite, then If P k-1 It is non-positive definite and P k-1 ( k k If ) is not positive definite, then

[0065] Step 7.2.7), output

[0066] Step 8), the calculation steps for the multi-model-based adaptive robust Kalman filter model are as follows:

[0067] Step 8.1), Hybrid Initialization. Calculate the input state vector and covariance matrix of the sub-filters:

[0068]

[0069]

[0070] Where i and j (i, j = 1, 2) represent the i-th and j-th sub-filters respectively, and 1 and 2 represent the sub-filters MAUKF and MRUKF respectively. The mixture probability can be calculated as follows:

[0071]

[0072] Where, π ij This represents the transition probability from sub-filter i to sub-filter j. This represents the model probability.

[0073] Step 8.2), Sub-filter update. Update sub-filters MAUKF and MRUKF to obtain the state estimation vector and covariance matrix of the sub-filters, respectively. and

[0074] Step 8.3), Model Probabilities Update. for:

[0075]

[0076] in, It can be calculated as:

[0077]

[0078] in, and Let represent the innovation vector and covariance matrix of the i-th sub-filter, respectively.

[0079] Step 8.4), fusion output. The final output state estimation vector and its covariance matrix of the filter are as follows:

[0080]

[0081]

[0082] The beneficial effects of this invention are:

[0083] 1. This method overcomes the problem of low adjustment accuracy of adaptive and robust Kalman filters with single adjustment factors. The adaptive robust Kalman filter based on multiple adjustment factors described in this method can retain the effect of beneficial channels on the filter while weakening the effect of harmful channels on the filter, thus effectively improving the filtering speed and accuracy.

[0084] 2. Conventional adaptive robust unscented Kalman filters based on fading and enhancement factors sometimes cause the covariance matrix of the state estimation to lose its positive definiteness, thus interrupting the filtering calculation. This method effectively solves this problem by introducing a quadratic adjustment factor.

[0085] 3. Conventional fading adaptive and enhanced robust methods have contradictory effects on filters. This method uses a multi-model information fusion strategy to automatically select adaptive or robust sub-filters based on the error type, which can simultaneously compensate for motion model errors and measurement singularities. Attached Figure Description

[0086] Figure 1 This is a flowchart of the algorithm design for the method of this invention.

[0087] Figure 2 This is the trajectory curve used for simulation by the method of the present invention.

[0088] Figure 3 This is the curve showing the eastward velocity estimation error value of the method of this invention.

[0089] Figure 4 This is the curve showing the northbound velocity estimation error value of the method of this invention.

[0090] Figure 5 This is the curve showing the eastward position estimation error value of the method of this invention.

[0091] Figure 6 This is the curve showing the northward position estimation error value of the method of this invention. Detailed Implementation

[0092] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the present invention.

[0093] As shown in the figure, the multi-model adaptive robust integrated navigation method of the present invention includes the following steps:

[0094] Step 1), for a single-axis rotation-modulated MEMS-INS, the Z-axis (upward axis) is the rotation axis, then the rotation modulation matrix is ​​as follows:

[0095]

[0096] Where u represents the MEMS-INS coordinate system, b represents the carrier coordinate system, θ represents the modulation angular velocity, and t represents time. θ = 20° / s. Then, the constant drift vectors of the gyroscope and accelerometer in the b-frame can be expressed as:

[0097]

[0098]

[0099] Where, ε u and These represent the constant drift vectors of the gyroscope and accelerometer in the u-frame, respectively.

[0100] Step 2), establish the discrete filter model:

[0101]

[0102] in, This represents the filter state vector. n represents the navigation coordinate system, T represents the transpose operation, and k-1 and k represent time k-1 and time k, respectively. δv n δp and a represent the estimated velocity error, position error, and attitude error vectors, respectively. k H represents the measurement vector. k =[I 6*6 0 6*9 ] T Denotes the measurement transition matrix, where I 6*6 Represents a 6x6 identity matrix, 0 6*9 This represents a 6x9 zero matrix. k and v k Let Q represent the process noise and measurement noise vectors, respectively. Both are zero-mean Gaussian white noise, and their corresponding covariance matrices are Q and Q, respectively. k and R k f(·) represents the nonlinear state equation of the system.

[0103] Step 3), the one-step state estimation vector and covariance matrix of UKF are as follows:

[0104]

[0105]

[0106] in, x represents kThe dimension of λ. i and This represents the correction coefficient. The intermediate variable g i，k / k-1 It can be calculated as:

[0107]

[0108]

[0109] in, Let P represent the state estimate vector at time k-1, and its covariance matrix be P. k-1 δ i and They represent The i-th column and Column vector, This represents the Cholesky decomposition operation. λ is a constant coefficient. i , and The following can be calculated:

[0110]

[0111] in, a, The values ​​of β are 15, 0.02, 0, and 2, respectively. The gain matrix can be calculated as:

[0112]

[0113] The state estimation vector can be calculated as follows:

[0114]

[0115] The state estimation covariance matrix can be calculated as follows:

[0116] P k =(IK k H k )P k / k-1 (12)

[0117] Where I represents the identity matrix.

[0118] Step 4), the calculation steps for the Multiple Elimination Factor Adaptive UKF (MAUKF) are as follows:

[0119] Substitute into the fading factor matrix F k In equation (6):

[0120]

[0121] Among them, F k For f k,11 fk,22 , ..., Form a diagonal matrix with diagonal elements. Then equation (10) can be updated to:

[0122]

[0123] Step 5), the calculation method for the fading factor is as follows:

[0124] Step 5.1): When the filter performance is optimal, the following chi-square test holds:

[0125]

[0126] Where m represents H k Rank. ξ represents the chi-square test value, where α represents the confidence level. k (i) represents ξ k The i-th element, where, a k，ii and b k，ii They represent and The i-th diagonal element. The fading factor can be calculated as follows:

[0127]

[0128] Among them, t k,ii and h k,ii T represents respectively k and H k The i-th diagonal element, m, has a value of 6.

[0129] Step 5.2), to ensure P k-1 The positive definiteness is corrected by the decay factor as follows:

[0130] f k,ii =κ k (f k,ii -1)+1,0≤κ k ≤1, i=1,2,...,m (17)

[0131] Among them, κ k The calculation steps are as follows:

[0132] Step 5.2.1), set κ k The maximum and minimum values ​​are respectively and k k , and k k The initial values ​​are 1 and 0 respectively, and the counter is set. The initial value and the maximum value are 0 and 0 respectively.

[0133] Step 5.2.2), let Substitution In equation (17), the filter is updated based on equations (5), (13), (14) and (12).

[0134] Step 5.2.3), if P k-1 If it is positive definite, then If P k-1 If it is not positive definite, then

[0135] Step 5.2.4),

[0136] Step 5.2.5), if If the condition is met, return to step 5.2.2; otherwise, proceed to step 5.2.6.

[0137] Step 5.2.6), calculate based on P k-1 Value, denoted as P k-1 Calculation based on k k P k-1 Value, denoted as P k-1 ( k k If P k-1 If it is positive definite, then If P k-1 It is non-positive definite and P k-1 ( k k If κ is positive definite, then k = k k If P k-1 and P k-1 ( k k If all κ are non-positive definite, then κ k =0.

[0138] Step 5.2.7), output κ k The fading factor is corrected using equation (17), and the filter is updated.

[0139] Step 6), the calculation steps for the Multi-Enhancement Factor Robust UKF (MRUKF) are as follows:

[0140] Substitute into the enhancement factor matrix E k In equation (10):

[0141]

[0142] Among them, E k For e k,11 e k,22 , ..., e k,mm A diagonal matrix consisting of diagonal elements.

[0143] Step 7), the enhancement factor is calculated as follows:

[0144] Step 7.1): When the filter performance is optimal, the following chi-square test holds:

[0145]

[0146] Among them, c k，ii and d k，ii They represent and The i-th diagonal element. The enhancement factor can be calculated as follows:

[0147]

[0148] Where, r k,ii For R k The i-th diagonal element.

[0149] Step 7.2), to ensure P k-1 The positive definiteness is corrected by the enhancement factor as follows:

[0150]

[0151] in, The calculation steps for the second-order correction factor of the enhancement factor are as follows:

[0152] Step 7.2.1), set The maximum and minimum values ​​are respectively and k k , and k k The initial values ​​are 1 and 0 respectively, and the counter is set. The initial value and the maximum value are 0 and 0 respectively.

[0153] Step 7.2.2), let Substitution In equation (21), the filter is updated based on equations (5), (6), (18) and (12).

[0154] Step 7.2.3), if Pk-1 If it is positive definite, then If P k-1 If it is not positive definite, then

[0155] Step 7.2.4),

[0156] Step 7.2.5), if If the condition is met, return to step 7.2.2; otherwise, proceed to step 7.2.6.

[0157] Step 7.2.6), calculate based on P k-1 Value, denoted as P k-1 Calculation based on k k P k-1 Value, denoted as P k-1 ( k k If P k-1 If it is positive definite, then If P k-1 It is non-positive definite and P k-1 ( k k If ) is positive definite, then If P k-1 P k-1 ( k k If all are non-positive definite, then

[0158] Step 7.2.7), output Update the filter.

[0159] Step 8), the calculation steps for the multi-model-based adaptive robust Kalman filter model are as follows:

[0160] Step 8.1), calculate the input state vector and covariance matrix of the sub-filter:

[0161]

[0162]

[0163] Where i and j (i, j = 1, 2) represent the i-th and j-th sub-filters respectively, and 1 and 2 represent the sub-filters MAUKF and MRUKF respectively. The mixture probability can be calculated as follows:

[0164]

[0165] Where, π ij This represents the transition probability from sub-filter i to sub-filter j. This represents the model probability. M is the probability derived from π. ij The matrix formed, π ij Let b be the element in the i-th row and j-th column of M. k-1 For the reason The vector formed For b k-1 The i-th element. M and b k-1 The initial values ​​are defined as: M = [0.8, 0.2; 0.2; 0.8], b k-1 = [0.8, 0.2] T .

[0166] Step 8.2) Update the sub-filters MAUKF and MRUKF to obtain the state estimation vector and covariance matrix of the sub-filters, respectively. and

[0167] Step 8.3), Update for:

[0168]

[0169] in, It can be calculated as:

[0170]

[0171] in, and Let represent the innovation vector and covariance matrix of the i-th sub-filter, respectively.

[0172] Step 8.4), the final output state estimation vector and its covariance matrix of the filter are as follows:

[0173]

[0174]

[0175] The parameters used for simulation verification in this invention are as follows:

[0176] In this system, the gyroscope constant drift is 5° / h, and the random noise is... The accelerometer constant bias is 1 mg, and the random noise is... The initial attitude error is a = [-1°, 1°, 2°]. T The initial velocity error is δv n =[0,0,0] Tδp=[0,0,0] T . The satellite signal has a horizontal position error of 2m (root mean square error) and a velocity error of 0.2m / s (root mean square error). The inertial system sampling time is T = 5ms, the satellite sampling time is T = 1s, and the total simulation time is 600s. Simulation results show that the proposed method can meet high navigation and positioning requirements.

[0177] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A multi-model based adaptive robust integrated navigation method, characterized in that: Includes the following steps: Step 1), for a single-axis rotationally modulated MEMS-INS, with the axial direction as the rotation axis, the rotational modulation matrix is ​​as follows: （1）； in, Indicates the MEMS-INS coordinate system. Indicates the carrier coordinate system. Indicates the modulation angular velocity, Representing time, the constant drift vectors of the gyroscope and accelerometer are in... The systems are represented as follows: （2）； （3）； wherein and respectively represent are constant drift vectors of the gyroscopes and accelerometers in the system, Step 2), the discrete filter model is represented as: （4）； in, Represents the filter state vector. Indicates the navigation coordinate system. This indicates the transpose operation. and Representing time respectively and time , , ,and These represent the estimated velocity error, position error, and attitude error vectors, respectively. Represents the measurement vector; Denotes the measurement transition matrix, where Represents a 6x6 identity matrix. Represents a 6x9 matrix with zeros. and Let these represent the process noise and measurement noise vectors, respectively. Both are zero-mean Gaussian white noise, and their corresponding covariance matrices are respectively... and , The nonlinear state equations of the system are represented. Step 3), the one-step state estimation vector and covariance matrix of UKF are as follows: （5）； （6）； in, express dimensionality and Indicates the correction coefficient, an intermediate variable. The calculation is as follows: （7）； （8）； in, express The state estimation vector at time t is given by the covariance matrix of the vector. , and They represent The column sum Column vectors This represents the Cholesky decomposition operation. For constant coefficients, the gain matrix is ​​expressed as: （9）； Step 4), Multiple Elimination Factor Adaptive UKF, denoted as MAUKF, is calculated as follows: Substitute into the fading factor matrix In equation (6): （10）； in, For the reason , , ..., If we form a diagonal matrix with diagonal elements, then equation (9) is updated to: （11）； Step 5), the calculation method for the fading factor is as follows: Step 5.1): When the filter performance is optimal, the following chi-square test holds: （12）； in, express rank, Represents the chi-square test value, where Indicates the confidence level; express The There are elements, among which... , and They represent and The diagonal elements, The fading factor is calculated as follows: （13）； in, and They represent and The diagonal elements, Step 5.2), to ensure The positive definiteness is corrected by the decay factor as follows: （14）； wherein is a quadratic modification factor of the evanescent factor, Step 6), the Multi-Enhancement Factor Robust UKF, denoted as MRUKF, is calculated as follows: substituting the enhancement factor matrix into equation (9): （15）； in, For the reason , , ..., A diagonal matrix consisting of diagonal elements. Step 7), the calculation method for the enhancement factor is as follows: Step 7.1): When the filter performance is optimal, the following chi-square test holds: （16）； in, and They represent and The The enhancement factor for each diagonal element is calculated as follows: （17）； in, for The diagonal elements, Step 7.2), to ensure positive definiteness, the enhancement factor is modified as follows: （18）； in, As a secondary correction factor for the enhancement factor, Step 8), the calculation steps for the multi-model-based adaptive robust Kalman filter model are as follows: Step 8.1), Hybrid initialization, calculate the input state vector and covariance matrix of the sub-filters: （19）； （20）； in, , They represent the first The and the first Sub-filters, and Both are equal to 1 or 2. 1,2 denote the sub-filters MAUKF and MRUKF, respectively, denotes the mixing probability and is calculated as follows: （21）； in, Sub-filter To sub-filter The transition probability, Represents the model probability; Step 8.2), Sub-filter update: Update sub-filters MAUKF and MRUKF to obtain the state estimation vector and covariance matrix of the sub-filters, respectively. and , Step 8.3), model probability update, update is: （22）； in, , , The calculation is as follows: （23）； wherein, and respectively denote the innovation vector and its covariance matrix of the first sub-filter, respectively, Step 8.4), the fused output, the final output state estimation vector of the filter and its covariance matrix are as follows: （24）； （25）。 2. The multi-model adaptive robust combination navigation method according to claim 1, wherein, The detailed steps of step 5.2) are as follows: Step 5.2.1), settings The maximum and minimum values ​​are respectively and , and The initial values ​​are 1 and 0 respectively, and the counter is set. The initial value and the maximum value are 0 and 0 respectively. , Step 5.2.2), let Substitute In equation (11), update the filter and , Step 5.2.3), if If it is positive definite, then ;like If it is not positive definite, then , Step 5.2.4), , Step 5.2.5), if then go to Step 5.2.2), otherwise perform Step 5.2.6), Step 5.2.6), calculate based on of Value, represented as ( ), calculation based on of Value, represented as ( ),like ( If ) is positive definite, then ,like ( ) is non-positive definite and ( If ) is positive definite, then ,like ( ) is non-positive definite and ( If ) is not positive definite, then , Step 5.2.7), output .

3. The multi-model adaptive robust combination navigation method according to claim 1, wherein, The detailed steps of step 7.2) are as follows: Step 7.2.1), settings The maximum and minimum values ​​are respectively and , and The initial values ​​are 1 and 0 respectively, and the counter is set. The initial value and the maximum value are 0 and 0 respectively. , Step 7.2.2), let Substitute In equation (18), update the filter and , Step 7.2.3), if If it is positive definite, then ,like If it is not positive definite, then , Step 7.2.4), , Step 7.2.5), if then go to Step 7.2.2), otherwise perform Step 7.2.6), Step 7.2.6), calculate based on of Value, represented as ( ), calculation based on of Value, represented as ( ),like ( If ) is positive definite, then ,like ( ) is non-positive definite and ( If ) is positive definite, then ,like ( ) ( If all of them are non-positive definite, then , Step 7.2.7), output .

Images