An Adaptive Integrated Navigation Method Applicable to Multiple Scenarios

Abstract

The present invention provides an adaptive integrated navigation method applicable to multiple scenarios, including state estimation, receiving satellite data and inertial navigation data at time k, and performing state estimation based on the satellite data and inertial navigation data; measurement update, constructing an adjustment factor model according to the state estimation result, judging whether the Kalman filter diverges based on the adjustment factor model, if so, updating the measurement covariance matrix through adaptive filtering, and completing the measurement update based on the updated measurement covariance matrix; if not, updating the measurement covariance matrix through a multi-window robust strategy, and completing the measurement update based on the updated measurement covariance matrix; positioning update, judging whether to continue filtering, if not, ending the filtering, if so, k = k + 1, and executing the state estimation step. The present invention can automatically update the measurement noise covariance matrix according to the motion state of the carrier, and improve the adaptive ability of integrated navigation.

Description

Technical Field

[0001] The present invention relates to the technical field of integrated navigation, and particularly relates to an adaptive integrated navigation method applicable to multiple scenarios. Background Art

[0002] Kalman filtering is widely used in the field of GNSS / INS (Global Navigation Satellite System / Inertial Navigation System) integrated navigation. To achieve high-precision Kalman filtering solution, the following conditions need to be met: reliable observation information, accurate estimation method, and accurate dynamic model.

[0003] However, in practical applications, the motion state of an object is variable, including ultra-high-speed motion, variable acceleration motion, turning, etc. Different motion states correspond to different dynamic models. It is extremely complex to construct a high-precision dynamic model based on the object's motion state in actual motion. Therefore, there is a certain deviation between the dynamic model and the actual motion state of the carrier in complex motion scenarios, which affects the accuracy of Kalman filtering.

[0004] On the other hand, GNSS signals are extremely vulnerable to interference in complex environments, resulting in large GNSS positioning deviations, which in turn cause Kalman filtering to diverge. To further weaken the influence of abnormal observation information and dynamic models, adaptive filtering, robust filtering, and Sage-Husa adaptive filtering have been developed successively. Among them, Sage-Husa adaptive filtering is the most widely used. It can effectively solve the problem of filtering instability caused by considering measurement noise and system noise as white noise in traditional Kalman filtering. However, this filtering method still has the following problems:

[0005] The value of the forgetting factor will affect the accuracy of measurement noise estimation. In Sage-Husa adaptive filtering, the forgetting factor is preset according to empirical values and is difficult to adapt to complex and changeable external environments.

[0006] At the same time, online updating of the measurement noise covariance matrix and the system noise covariance matrix is likely to cause filtering divergence, and compared with traditional Kalman filtering, the calculation of the measurement noise covariance matrix and the system noise covariance matrix is more complex, requiring higher requirements for the navigation computer.

[0007] The positive semi-definiteness of the system noise covariance matrix and the positive definiteness of the measurement noise covariance matrix cannot be guaranteed, easily leading to matrix operation singularities. Summary of the Invention

[0008] In view of this, the problem to be solved by the present invention is to provide an adaptive integrated navigation method applicable to multiple scenarios, which can automatically update the measurement noise covariance matrix according to the motion state and improve the adaptive ability of integrated navigation.

[0009] To solve the above technical problems, the technical solution adopted by the present invention is:

[0010] An adaptive integrated navigation method applicable to multiple scenarios includes state estimation, receiving satellite data and inertial navigation data at time k, and performing state estimation based on the satellite data and inertial navigation data;

[0011] Measurement update, constructing an adjustment factor model according to the state estimation result, judging whether the Kalman filter diverges based on the adjustment factor model. If so, updating the measurement covariance matrix through adaptive filtering , and completing the measurement update based on the updated measurement covariance matrix ; if not, updating the measurement covariance matrix through a multi-window robust strategy , and completing the measurement update based on the updated measurement covariance matrix ;

[0012] Positioning update, judging whether to continue filtering. If not, ending the filtering; if so, k = k + 1, and executing the state estimation step.

[0013] Further, the formula of the adjustment factor model is:

[0014] ,

[0015] where is the innovation sequence at time k; is the measurement coefficient matrix; is the mean square error matrix of the one-step prediction of the state between time k - 1 and time k; is the measurement covariance matrix at time k; is the reserve coefficient, and ; tr represents the trace of the matrix;

[0016] The calculation formula of

[0017] ,

[0018] where is the measurement vector at time k; is the one-step prediction of the state between time k - 1 and time k.

[0019] Further, the calculation formula of the reserve coefficient is:

[0020] ,

[0021] Among them, exp represents the exponential function with the natural constant e as the base, is the velocity in the horizontal direction of the carrier, and is the auxiliary adjustment factor.

[0022] Furthermore, the satellite data is collected by a receiver, and the calculation formula for the receiver velocity is:

[0023] ,

[0024] where b represents the forgetting factor, is the velocity vector of satellite j, and the superscript represents the satellite, represents the receiver velocity, represents the clock drift of the receiver, is the change rate of the pseudorange observation noise of satellite j, and the superscript represents the change rate of the corresponding variable, represents the wavelength, is the Doppler observation value;

[0025] Based on the receiver velocity determine the velocity in the horizontal direction of the carrier.

[0026] Furthermore, the update formula for the forgetting factor is:

[0027] ,

[0028] where, is the nth-order state one-step transition matrix between time k-1 and time k; is the system noise covariance matrix at time k-1, represents the measurement noise covariance matrix at time k, is the one-step prediction mean square error matrix of the state at time k-1.

[0029] Furthermore, the multi-window robust strategy includes constructing estimates of multiple innovation covariance matrices, and each estimate of the innovation covariance matrix is calculated separately with the theoretical value of the innovation covariance matrix at the current moment to obtain an estimate of the observation vector covariance matrix. The estimate of the observation vector covariance matrix and the prior observation covariance matrix construct a robust factor , and by amplify the measurement noise covariance matrix .

[0030] ​Further, the estimated value of the innovation covariance matrix includes the estimated value of the innovation covariance matrix of the fixed solution, the estimated value of the innovation covariance matrix of the floating solution, and the estimated value of the innovation covariance matrix of the single-point solution.

[0031] Further, the robust factor The formula is:

[0032] ,

[0033] ,

[0034] where represents the prior observation covariance matrix, is the innovation vector between the one-step prediction value and the observation value at time k - i, and N is the number of epochs.

[0035] Further, it includes estimating the measurement noise covariance matrix based on the Allan variance , and the specific formula is:

[0036] ,

[0037] After simplifying the recurrence, we can get:

[0038] ,

[0039] In the above formula, If the fading memory algorithm is used for estimation, then there is:

[0040] ,

[0041] ,

[0042] where and represent the upper and lower limit conditions of the measurement noise covariance matrix , k and k - 1 represent the current time and the previous time respectively, , and represents the forgetting factor at time k, and b represents the initial forgetting factor set artificially in advance.

[0043] The advantages and positive effects of the present invention are:

[0044] (1) By setting the adjustment factor model, it is judged whether the filtering diverges according to the horizontal speed of the carrier, the scene is divided based on the moving speed of the carrier, the measurement value is updated by adaptive filtering when it diverges, and the measurement value is updated by the multi-window robust strategy when it does not diverge, which can improve the adaptability to various complex environments.

[0045] (2) By calculating the robust factors of the fixed solution window, floating solution window, and single-point solution window respectively to obtain the optimal robust factor for dynamic robust processing. Based on the robust factor for scenario division, the adaptability to various complex environments is improved.

[0046] (3) During the process of updating the measurement noise covariance matrix limit its upper and lower bounds to avoid the amplified from losing positive definiteness, and: avoid the denominator of the calculated robust factor being zero, thereby making the matrix operation singular and the filter diverge, and improving the long-term reliability of the filter of the multi-window robust strategy. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation to the present invention. In the drawings:

[0048] Figure 1 is the overall structure diagram of an adaptive integrated navigation method applicable to multiple scenarios of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0049] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0050] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those skilled in the technical field to which the present invention belongs. The terms used in the specification of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention. The term "and / or" used herein includes any and all combinations of one or more of the related listed items.

[0051] The present invention provides an adaptive integrated navigation method applicable to multiple scenarios, as Figure 1 shown, including state estimation, receiving satellite data and inertial navigation data at the current moment, and performing state estimation based on the satellite data and inertial navigation data.

[0052] The integrated navigation is GNSS / INS integrated navigation. When the GNSS / INS integrated navigation runs, it initializes data, receives satellite data and inertial navigation data, and estimates the position and attitude of the carrier based on the satellite data and inertial navigation data (i.e., state estimation). The GNSS / INS integrated navigation continuously receives satellite data and inertial navigation data to continuously estimate the position and attitude of the carrier for long-term positioning.

[0053] To be able to estimate the position and attitude of the carrier accurately and for a long time, the measured values at each sampling moment are continuously updated through the Kalman filter principle, and long-term and accurate positioning can be achieved.

[0054] Measurement update, construct an adjustment factor model, and based on the adjustment factor model, judge whether the Kalman filter diverges. If so, update the measurement covariance matrix through adaptive filtering based on the updated measurement covariance matrix to complete the measurement update; if not, update the measurement covariance matrix through a multi-window robust strategy based on the updated measurement covariance matrix to complete the measurement update.

[0055] The formula of the adjustment factor model is:

[0056] ,

[0057] where is the innovation sequence at time k; is the measurement coefficient matrix; is the one-step prediction mean square error matrix of the state between time k - 1 and time k; is the measurement covariance matrix at time k; is the reserve coefficient, and ; tr represents the trace of the matrix.

[0058] The formula for calculating the innovation sequence at time k is:

[0059] ,

[0060] where is the measurement vector at time k; is the one-step prediction of the state between time k - 1 and time k.

[0061] When the formula of the adjustment factor model holds, it means that the Kalman filter at time k is in a divergent state. At this time, the measurement noise covariance matrix is updated using adaptive filtering based on the updated measurement noise covariance matrix Update the measurement value at time k; when the adjustment factor model formula does not hold, it indicates that the Kalman filter at time k does not diverge, and the multi-window robust strategy is used to update the measurement noise covariance matrix , based on the updated measurement noise covariance matrix Update the measurement value at time k.

[0062] The reserve coefficient in the adjustment factor model The calculation formula is:

[0063] ,

[0064] where exp represents the exponential function with the natural constant e as the base, is the velocity in the horizontal direction of the carrier, and are auxiliary adjustment factors, and their values can be set according to the actual strictness of filtering anomaly discrimination, and: The closer it is to 1, the stricter the filtering divergence judgment. When the filtering divergence judgment is the strictest.

[0065] Velocity information can intuitively reflect the motion state of the carrier. During the motion of the carrier, the greater the velocity, the stronger the time-variability of the parameters, and it is more susceptible to the influence of the external environment and motion mode, which may lead to filtering divergence. The reserve coefficient is determined based on the velocity in the horizontal direction of the carrier, and then the reserve coefficient is substituted into the adjustment factor model to determine whether the Kalman filter diverges, which can improve the adaptive ability of the filtering. The Kalman filter can effectively distinguish the scenarios of the carrier at different velocities according to the velocity information (the velocity in the horizontal direction of the carrier), making the integrated navigation more adaptable to various complex environments.

[0066] To avoid the influence of filtering divergence on the velocity anomaly and further affect the calculation of the reserve coefficient , the horizontal velocity can be calculated by GNSS Doppler velocity measurement.

[0067] The GNSS Doppler velocity measurement calculation formula is:

[0068] ,

[0069] where b represents the forgetting factor, is the velocity vector of satellite j, and the superscript represents the satellite, represents the receiver velocity, represents the receiver clock drift, is the pseudo-range observation noise change rate of satellite j, and the superscript represents the change rate of the corresponding variable, represents the wavelength, Are Doppler observations.

[0070] The update formula for the forgetting factor b is:

[0071] ,

[0072] Where, Is the n - order state one - step transition matrix between time k - 1 and time k; Is the system noise covariance matrix at time k - 1, Represents the measurement noise covariance matrix at time k - 1, Is the one - step prediction mean square error matrix of the state at time k - 1. After the integrated navigation receives satellite data and inertial navigation data at time k, the forgetting factor at time k can be calculated. By dynamically updating the value of the forgetting factor, the accuracy of subsequent measurement noise estimation is improved, enabling the integrated system to adapt to complex and changing external environments.

[0073] An embodiment of this application is: calculating the carrier velocity (receiver velocity) according to the GNSS Doppler velocity measurement calculation formula, and then determining the velocity of the carrier in the horizontal direction Based on the carrier velocity , in order to calculate the reserve coefficient . The specific process of GNSS Doppler velocity measurement calculation is: when the receiver observes more than four satellites, the current receiver velocity Can be solved, and the solution equation is expressed as:

[0074]

[0075] Denote

[0076]

[0077] Using the least squares method, we can get:

[0078] .

[0079] The multi - window robust strategy includes constructing multiple innovation covariance windows and calculating the robust factor For each window respectively. The formula for the robust factor Is:

[0080] ,

[0081] Where, Represents the prior observation covariance matrix, tr() represents the trace of the matrix (the sum of the diagonal elements of the matrix), N is the number of epochs, Is the measurement coefficient matrix at time k, is the one-step prediction mean square error matrix of the state between time k-1 and time k. is the innovation vector (innovation sequence) between the one-step predicted value and the observed value at time k-i.

[0082] The innovation covariance window includes a fixed solution window, a floating solution window, and a single-point solution window. Each window records the estimated value of the innovation covariance matrix of the fixed solution, the estimated value of the innovation covariance matrix of the floating solution, and the estimated value of the innovation covariance matrix of the single-point solution, respectively. The estimated values of the innovation covariance matrices of the fixed solution, floating solution, and single-point solution (here, pseudo-range difference is equated with the single-point solution) for N epochs are saved in the three windows respectively. The estimated value of the observation vector covariance matrix is calculated based on the estimated value of the innovation covariance matrix in the window and the theoretical value of the innovation covariance matrix at the current time k, and then through the estimated value of the observation vector covariance matrix and the prior observation covariance matrix Construct the robust factor .

[0083] Calculate the robust factors of the three windows respectively , and determine the optimal robust factor through the formula . Based on the optimal robust factor amplify the measurement noise covariance matrix , and the amplification formula is: (subsequently continue to use to represent the amplified measurement noise covariance matrix), which can reduce the influence brought by the innovations with different distributions in a single window and improve the accuracy of subsequent measurement updates.

[0084] The multi-window robust strategy includes: calculating the innovation vector Vk between the predicted value and the observed value based on the measurement noise covariance matrix , updating the measurement noise covariance matrix based on the innovation vector Vk , and jointly updating the measurement value based on the updated measurement noise covariance matrix and the measurement covariance matrix .

[0085] During the process of calculating the robust factor , may be less than zero, resulting in the subsequent calculated measurement noise covariance matrix losing positive definiteness, making the inner denominator of the formula zero when calculating the subsequent robust factor (filter divergence), which affects the update result of the measurement value. To avoid the above problems, estimate the measurement noise covariance matrix based on Allan variance , and the specific formula is:

[0086] ,

[0087] Simplified recurrence gives:

[0088] ,

[0089] In the above formula, If the fading memory algorithm is used for estimation, then there is:

[0090] ,

[0091] ,

[0092] where, , k and k - 1 represent the current time and the previous time respectively, represents the forgetting factor at time k, and b represents the initially set forgetting factor manually.

[0093] By restricting the upper and lower limit conditions of the measurement noise covariance matrix and ( , where is the initial value of the measurement noise covariance matrix), the non - negativity of is ensured, while reducing the matrix operation amount and greatly improving the reliability of the filtering.

[0094] Position update, determine whether to continue filtering. No, end the filtering. Yes, update the current time and execute the state estimation step.

[0095] After updating the measurement value, determine whether to continue filtering according to whether the navigation ends. If the navigation ends, end the filtering. If the navigation continues, then k = k + 1, execute the state estimation step, and continue to receive the satellite data and inertial navigation data at time k.

[0096] The working principle and process of the present invention are as follows:

[0097] At time k, receive satellite data or inertial navigation data, and estimate the position data and attitude data of the carrier according to the measurement matrix (the position data and attitude data together constitute the pose data of the carrier);

[0098] Substitute the position data and attitude data at time k into the Kalman filter. There is an adjustment factor model in the Kalman filter to judge whether the filtering diverges. There is a reserve coefficient in the adjustment factor model, which is dynamically adjusted according to the carrier speed, so that the value of is not necessarily the same at different times. Therefore, the judgment strictness of whether the Kalman filtering diverges is directly related to the speed, realizing that the Kalman filtering can automatically adapt to the carrier speed, enabling the integrated navigation system to navigate stably under dynamic speeds and improving the adaptive ability of the integrated navigation. ​

[0099] When the Kalman filter does not diverge, the Sage-Husa adaptive filter is used to update the measurement matrix, and the pose data of the carrier at the (k + 1)-th moment is continuously estimated based on the updated measurement matrix. This solves the problem that the forgetting factor in the Sage-Husa adaptive filter is preset according to empirical values and is difficult to adapt to complex and changeable external environments.

[0100] When the Kalman filter diverges, the multi-window robust strategy is used to update the measurement matrix, and the pose data of the carrier at the (k + 1)-th moment is continuously estimated based on the updated measurement matrix. Without complex calculations, the measurement noise covariance matrix can be updated, saving computational effort.

[0101] The above has described the embodiments of the present invention in detail, but the content described is only the preferred embodiments of the present invention and cannot be considered as limiting the scope of implementation of the present invention. All equivalent changes and improvements made according to the scope of the present invention should still fall within the scope covered by this patent.

Claims

1. An adaptive integrated navigation method suitable for multiple scenarios, characterized in that: include, State estimation: receiving satellite data and inertial navigation data at time k, and performing state estimation based on the satellite data and inertial navigation data; Measurement update, build a regulation factor model based on the state estimation results, and determine whether the Kalman filter diverges based on the regulation factor model. If yes, update the measurement covariance matrix through adaptive filtering , based on the updated measurement covariance matrix Complete measurement update; No, update the measurement covariance matrix through the multi-window anti-error strategy , based on the updated measurement covariance matrix Complete measurement update; Positioning update, determine whether to continue filtering, if not, end filtering, if yes, k=k+1, execute state estimation step; The formula of the adjustment factor model is: ， in, is the new information sequence at time k; is the measurement coefficient matrix; is the one-step prediction mean square error matrix of the state between time k-1 and time k; is the measurement covariance matrix at time k; is the reserve factor, and ; tr represents the trace of the matrix; The calculation formula is: ， in, is the measurement vector at time k; It is a one-step prediction of the state between time k-1 and time k; The reserve factor The calculation formula is: ， Among them, exp represents the exponential function with the natural constant e as the base, is the horizontal velocity of the carrier, and Co-regulatory factor The multi-window anti-error strategy includes constructing multiple estimates of the innovation covariance matrix, each of which is calculated with the theoretical value of the innovation covariance matrix at the current moment to obtain an estimate of the observation vector covariance matrix, and the estimate of the observation vector covariance matrix is ​​calculated with the prior observation covariance matrix. Structural robustness factor ,pass Amplify the measurement noise covariance matrix .

2. The adaptive integrated navigation method applicable to multiple scenarios according to claim 1, characterized in that: The satellite data is collected by a receiver, and the calculation formula of the receiver speed is: ， Where b represents the forgetting factor, is the velocity vector of satellite j, with a superscript Indicates satellite, Indicates the receiver speed, represents the receiver clock drift, is the pseudorange observation noise change rate of satellite j, with the superscript represents the rate of change of the corresponding variable, represents the wavelength, is the Doppler observation value; According to the receiver speed Determine the horizontal speed of the carrier .

3. The adaptive integrated navigation method applicable to multiple scenarios according to claim 2, characterized in that: The updating formula of the forgetting factor is: ， in, It is the n-order state one-step transfer matrix between time k-1 and time k; is the system noise covariance matrix at time k-1, represents the measurement noise covariance matrix at time k-1.

4. The adaptive integrated navigation method applicable to multiple scenarios according to claim 3, characterized in that: The estimated value of the innovation covariance matrix includes the estimated value of the innovation covariance matrix of the fixed solution, the estimated value of the innovation covariance matrix of the floating-point solution, and the estimated value of the innovation covariance matrix of the single-point solution.

5. The adaptive integrated navigation method applicable to multiple scenarios according to claim 3, characterized in that: The robustness factor The formula is: ， ， in, represents the prior observation covariance matrix, is the new information vector between the one-step predicted value and the observed value at time ki, and N is the number of epochs.

6. The adaptive integrated navigation method applicable to multiple scenarios according to claim 3, characterized in that: The multi-window robustness strategy includes estimating the measurement noise covariance matrix based on the Allan variance , the specific formula is: ， Simplifying the recursion, we can get: ， In the above formula, Using the gradually disappearing memory algorithm for estimation, we have: ， ， in, and represents the measurement noise covariance matrix The upper and lower limit conditions, , k and k-1 represent the current moment and the previous moment respectively, represents the forgetting factor at time k, and b is set in advance by humans.

Images