Improved robust Kalman filtering integrated navigation method and system based on chi-square detection

Abstract

The invention provides an improved robust Kalman filtering integrated navigation method and system based on chi-square detection, and belongs to the technical field of navigation, and the method comprises the following steps: constructing a GNSS / INS loose integrated navigation Kalman filtering basic framework; before combined navigation Kalman filtering measurement updating, calculating an innovation chi-square test amount, recording the number of available satellites of each epoch, and calculating a satellite observation quality index according to the number of the available satellites; sage-Husa robust Kalman filtering based on IAE windowing is improved, a weighting factor is determined through an innovation chi-square test amount and is used for adjusting the weight of current epoch innovation in a window, and the window length is dynamically adjusted according to the change of a satellite observation quality index; a robust factor obtained through calculation of improved Sage-Husa robust Kalman filtering based on IAE windowing acts on an observation noise covariance matrix, so that the observation noise covariance matrix is expanded, and the weight of observation information with gross error in data fusion is reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of navigation technology, and in particular to an improved robust Kalman filtering integrated navigation method and system based on chi-square detection. BACKGROUND

[0002] The rapid expansion of high-precision positioning demand scenarios such as intelligent transportation and autonomous driving puts higher requirements on the stability and reliability of navigation systems in complex environments. The GNSS / INS integrated navigation system, which is composed of a Global Navigation Satellite System (GNSS) and an Inertial Navigation System (INS) and has the characteristics of high-precision continuous navigation, is widely used in urban transportation, unmanned aerial vehicles, autonomous driving and other fields.

[0003] In a complex urban environment, when a vehicle using a GNSS / INS integrated navigation system navigates through a tree-lined avenue, a tunnel, a city canyon or other environments with poor GNSS observation quality, frequent GNSS blockage or interruption can cause the integrated navigation accuracy to decrease, which often causes the observation accuracy of the GNSS and its corresponding covariance matrix to be mismatched, reducing the filtering effect and affecting the accuracy and stability of the integrated navigation system. These disturbances often cannot be perceived and resisted by ordinary extended Kalman filtering. Therefore, correcting the covariance matrix to adapt to the corresponding observation accuracy is an important way to maintain high-precision positioning of the vehicle integrated navigation system in complex environments.

[0004] In order to improve the navigation accuracy and stability of the integrated navigation system, robust Kalman filtering is generally used to suppress observation gross errors. Sage-Husa robust Kalman filtering based on Innovation Adaptive Estimation (IAE) windowing is a traditional robust Kalman filtering. However, in Sage-Husa robust Kalman filtering based on IAE windowing, the traditional approach is to equally average the innovations within the window, and the current epoch innovation will be smoothed by other innovations within the window, making it difficult to accurately reflect the statistical characteristics of real-time errors in dynamic navigation, thereby failing to timely process gross errors to achieve the desired robustness effect. Moreover, when facing frequent changes in innovation distribution, the window contains innovations of multiple different distributions, making it difficult to correctly estimate the covariance matrix. SUMMARY

[0005] The present application provides an improved robust Kalman filtering integrated navigation method and system based on chi-square detection to solve the defects in the prior art and effectively suppress the navigation positioning gross errors of the vehicle GNSS / INS integrated navigation system when encountering poor GNSS observation quality.

[0006] In a first aspect, the present invention provides an improved robust Kalman filter integrated navigation method based on chi-square detection, comprising: Constructing a basic framework for Kalman filtering in GNSS / INS loosely integrated navigation; Before updating the integrated navigation Kalman filter measurements, calculate the chi-square test of the new information, record the number of available satellites for each epoch, and calculate the satellite observation quality index based on the number of available satellites. An improvement is made to the Sage-Husa robust Kalman filter based on IAE windowing. The weighting factor is determined by the chi-square test of the new information and used to adjust the weight of the current epoch information within the window. The window length is dynamically adjusted according to the changes in satellite observation quality indicators. The robustness factor calculated by the improved Sage-Husa robust Kalman filter based on IAE windowing is applied to the observation noise covariance matrix, causing the observation noise covariance matrix to expand and reducing the weight of observation information with gross errors in data fusion, thereby suppressing observation gross errors.

[0007] According to the present invention, an improved robust Kalman filter integrated navigation method based on chi-square detection is provided, which constructs a basic framework for GNSS / INS loosely integrated navigation Kalman filter, including: Constructing state vectors ,in For three-dimensional position error, For three-dimensional velocity error, For three-dimensional attitude error, For three-dimensional gyroscopes with zero bias, Zero bias for three-dimensional accelerometers; From the state vector Constructing state equations ,in Let be the state vector for the next state. Here is the state transition matrix. The noise figure matrix is... This is system noise; Construct observation vectors ,in The GNSS antenna phase center position vector predicted by INS mechanical orchestration. The GNSS antenna phase center position vector is obtained directly from GNSS RTK calculation; From the observation vector Constructing the observation matrix ,in For the observation matrix, To observe noise; Construct the Kalman filter prediction update formula:

[0008]

[0009] in, Let be the prior state vector of the k-th epoch. Let be the state vector of the (k-1)th epoch. This is the state transition matrix from the (k-1)th epoch to the kth epoch. Let be the prior state covariance matrix of the k-th epoch. Let be the state covariance matrix of the (k-1)th epoch. Let be the noise coefficient matrix of the (k-1)th epoch. Let be the system noise covariance matrix at epoch k-1, with superscript . Indicates matrix transpose; Construct the Kalman filter measurement update formula:

[0010]

[0011]

[0012] in, Let Kalman filter gain be the value at epoch k. Let be the observation matrix of the k-th epoch. Let be the observation noise covariance matrix of the k-th epoch. Let be the posterior state vector of the k-th epoch. Let be the observation vector at the k-th epoch. Let be the posterior state covariance matrix of the k-th epoch. It is an identity matrix.

[0013] According to the present invention, an improved robust Kalman filter integrated navigation method based on chi-square test is provided. Before updating the integrated navigation Kalman filter measurement, the novel chi-square test value is calculated, the number of available satellites at each epoch is recorded, and the satellite observation quality index is calculated based on the number of available satellites, including: New Chi-square test quantity The calculation formula is:

[0014] in, The information vector for the k-th epoch is calculated using the following formula. :

[0015] in, Let be the prior state vector of the k-th epoch, and... same, for The inverse of the covariance matrix is ​​calculated using the following formula. : .

[0016] This invention provides an improved robust Kalman filter integrated navigation method based on chi-square test, which improves the Sage-Husa robust Kalman filter based on IAE windowing. The method determines the weighting factor through the chi-square test of the new information, which is used to adjust the weight of the new information at the current epoch within the window. The window length is dynamically adjusted according to changes in satellite observation quality indicators. The method includes: Determine the weighting factors The calculation formula is as follows:

[0017] in, The chi-square detection threshold for new information; In Sage-Husa robust Kalman filtering, the formula for IAE windowing is:

[0018] in, Calculated using the current information The estimated value, For window length, Indicates the first The new information vector of an epoch, Indicates the first The transpose of the epoch's information vector; An improvement to the IAE-windowed Sage-Husa robust Kalman filter is made by weighting the innovation within the window: .

[0019] According to the present invention, an improved robust Kalman filter integrated navigation method based on chi-square detection is provided, with satellite observation quality indicators... The calculation formula is:

[0020] in, Indicates the current number k The quality indicators of satellite observations over an epoch. This indicates the maximum number of available satellites. Indicates the current number k Number of satellites available in an epoch. This represents the standard deviation of the number of available satellites near the current epoch. The threshold for observation quality indicators is The observation quality level is then divided into:

[0021] When the observation quality level changes, the window length is reset to the minimum. If the observation quality level remains unchanged after several observation epochs, the window length is gradually increased; otherwise, the minimum window length is maintained. The adjusted window length is determined to be Maximum window length is Minimum window length is The observation quality level was maintained for a period of time. The formula for increasing the window length is: .

[0022] According to the present invention, an improved robust Kalman filter integrated navigation method based on chi-square detection is provided. The robustness factor calculated by the improved Sage-Husa robust Kalman filter based on IAE windowing is applied to the observation noise covariance matrix, causing the observation noise covariance matrix to expand and thus obtaining an estimated observation noise covariance matrix. To reduce the weight of observations with gross errors in data fusion, in order to suppress observational gross errors, the following measures are taken:

[0023] in, For dynamic window length, As a weighting factor; The robustness factor is:

[0024] in, This indicates taking the trace of the matrix. It is a constant; The formula acting on the original observation noise covariance matrix is:

[0025] in, This is the robust observation noise covariance matrix.

[0026] Secondly, the present invention also provides an improved robust Kalman filter integrated navigation system based on chi-square detection, comprising: The building block is used to construct the basic framework for Kalman filtering in GNSS / INS loosely integrated navigation; The calculation module is used to calculate the chi-square test of the new information before updating the Kalman filter measurement of the integrated navigation, record the number of available satellites in each epoch, and calculate the satellite observation quality index based on the number of available satellites. The improved module is used to improve the Sage-Husa robust Kalman filter based on IAE windowing. The weighting factor is determined by the chi-square test of the new information and used to adjust the weight of the new information of the current epoch within the window. The window length is dynamically adjusted according to the changes in satellite observation quality indicators. The suppression module is used to apply the robust factor calculated by the improved IAE-window-based Sage-Husa robust Kalman filter to the observation noise covariance matrix, thereby expanding the observation noise covariance matrix and reducing the weight of observation information with gross errors in data fusion, so as to suppress observation gross errors.

[0027] Thirdly, the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the improved robust Kalman filter combined navigation method based on chi-square detection as described above.

[0028] Fourthly, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the improved robust Kalman filter combined navigation method based on chi-square detection as described above.

[0029] This invention provides an improved robust Kalman filter integrated navigation method and system based on chi-square detection. By incorporating innovation weighting and dynamic window length adjustment into the traditional IAE-windowed Sage-Husa robust Kalman filter, this method improves the algorithm's sensitivity in handling gross errors and enhances the accuracy of window estimation of the observation noise covariance matrix in complex environments. This timely and sufficient handling of observation gross errors improves the robustness and stability of the navigation system in complex environments. Compared to the classic extended Kalman filter, the root mean square (RMSE) of position errors in the east, north, and celestial directions is reduced by 29.32%, 36.46%, and 48.65%, respectively. Compared to the traditional IAE-windowed Sage-Husa robust Kalman filter, the RMSE of position errors in the east, north, and celestial directions is reduced by 19.10%, 18.19%, and 6.45%, respectively. Attached Figure Description

[0030] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0031] Figure 1This is one of the flowcharts of the improved robust Kalman filter integrated navigation method based on chi-square detection provided by the present invention; Figure 2 This is the second flowchart of the improved robust Kalman filter integrated navigation method based on chi-square detection provided by the present invention; Figure 3 This is a comparison of the eastward positioning error curves of the improved robust Kalman filter (DWLW RKF), the traditional Sage-Husa robust Kalman filter (RKF) based on IAE windowing, and the classic extended Kalman filter (EKF) provided in this invention in complex urban environments (tree-lined roads, urban canyons). Figure 4 This is a comparison of the northbound positioning error curves of the improved robust Kalman filter (DWLW RKF), the traditional Sage-Husa robust Kalman filter (RKF) based on IAE windowing, and the classic extended Kalman filter (EKF) provided in this invention in complex urban environments (tree-lined roads, urban canyons). Figure 5 This is a comparison of the overhead positioning error curves of the improved robust Kalman filter (DWLW RKF), the traditional Sage-Husa robust Kalman filter (RKF) based on IAE windowing, and the classic extended Kalman filter (EKF) provided in this invention in complex urban environments (tree-lined roads, urban canyons). Figure 6 This is a comparison of the cumulative distribution curves of the three-dimensional absolute values ​​of error of the improved robust Kalman filter (DWLW RKF), the traditional Sage-Husa robust Kalman filter (RKF) based on IAE windowing, and the classic extended Kalman filter (EKF) provided in this invention in complex urban environments (tree-lined roads, urban canyons). Figure 7 This is a schematic diagram of the structure of the improved robust Kalman filter integrated navigation system based on chi-square detection provided by the present invention; Figure 8 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0032] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0033] Figure 1This is one of the flowcharts illustrating the improved robust Kalman filter integrated navigation method based on chi-square detection provided in this embodiment of the invention, such as... Figure 1 As shown, it includes: Step 100: Construct the basic framework of Kalman filtering for GNSS / INS loosely integrated navigation; Step 200: Before updating the integrated navigation Kalman filter measurement, calculate the chi-square test value of the new information, record the number of available satellites for each epoch, and calculate the satellite observation quality index based on the number of available satellites; Step 300: Improve the Sage-Husa robust Kalman filter based on IAE windowing, determine the weighting factor through the chi-square test of the new information, and use it to adjust the weight of the current epoch information within the window. Dynamically adjust the window length according to the changes in satellite observation quality indicators. Step 400: The robust factor calculated by the improved Sage-Husa robust Kalman filter based on IAE windowing is applied to the observation noise covariance matrix, causing the observation noise covariance matrix to expand and reducing the weight of observation information with gross errors in data fusion, so as to suppress observation gross errors.

[0034] Specifically, in this embodiment of the invention, the prior state vector and prior state covariance matrix are first obtained through Kalman filtering prediction update. Then, the innovation chi-square fault check quantity is calculated, and a weighting factor is calculated based on this. Then, the satellite observation quality index is calculated based on the number of available satellites, and the dynamic window length is calculated accordingly. Then, the estimated observation noise covariance matrix is ​​calculated based on the calculated dynamic window length and weighting factor, thereby calculating the robust factor. Finally, it is substituted into the measurement update of Kalman filtering to complete one integrated navigation robust filtering.

[0035] like Figure 2 As shown, the specific implementation steps include the following: First, we construct the basic framework for Kalman filtering in GNSS / INS loosely integrated navigation: Its state vector is:

[0036] In the formula, For three-dimensional position error, For three-dimensional velocity error, For three-dimensional attitude error, For three-dimensional gyroscopes with zero bias, It is a three-dimensional accelerometer with zero bias.

[0037] Its equation of state is:

[0038] In the formula, Let be the state vector for the next state. Here is the state transition matrix. The noise figure matrix is... This represents system noise.

[0039] Its observation vector is:

[0040] In the formula, The GNSS antenna phase center position vector predicted by INS mechanical orchestration. This is the GNSS antenna phase center position vector obtained directly from GNSS RTK.

[0041] Its observation matrix is:

[0042] In the formula, For the observation matrix, To observe noise.

[0043] The Kalman filter prediction update formula is:

[0044]

[0045] In the formula, Let be the prior state vector of the k-th epoch. Let be the state vector of the (k-1)th epoch. This is the state transition matrix from the (k-1)th epoch to the kth epoch. Let be the prior state covariance matrix of the k-th epoch. Let be the state covariance matrix of the (k-1)th epoch. Let be the noise coefficient matrix of the (k-1)th epoch. Let be the system noise covariance matrix at epoch k-1, with superscript . This indicates the matrix transpose.

[0046] The Kalman filter measurement update formula is:

[0047]

[0048]

[0049] In the formula, Let Kalman filter gain be the value at epoch k. Let be the observation matrix of the k-th epoch. Let be the observation noise covariance matrix of the k-th epoch. Let be the posterior state vector of the k-th epoch. Let be the observation vector at the k-th epoch. Let be the posterior state covariance matrix of the k-th epoch. It is an identity matrix.

[0050] Further calculations are performed on the innovation weighting factor and dynamic window length. Then, the estimated observation noise covariance matrix is ​​obtained, and the robustness factor is calculated. The robustness factor is used to dilate the original observation noise covariance matrix to complete the robustness processing. Finally, Kalman filtering is performed to update the measurements, completing one robust filtering operation. On the one hand, the amount of new information card verification The calculation formula is

[0051] In the formula, The information vector for the k-th epoch can be calculated using the following formula:

[0052] In the formula, Let be the prior state vector of the k-th epoch, and... same, for The inverse of the covariance matrix, The following formula can be used to calculate:

[0053] Weighting factor The calculation formula is:

[0054] In the formula, This is the chi-square detection threshold, which is usually set to 15.

[0055] In traditional Sage-Husa robust Kalman filtering, the formula for IAE windowing is:

[0056] in, Calculated using the current information The estimated value, For window length, Indicates the first The new information vector of an epoch, Indicates the first The transpose of the epoch's information vector. Weighting of the information within the window:

[0057] On the other hand, satellite quality observation indicators The calculation formula is:

[0058] In the formula, Indicates the current number k Satellite observation quality indicators for each epoch; Indicates the maximum number of available satellites; Indicates the current number k Number of satellites available at each epoch; This represents the standard deviation of the number of available satellites near the current epoch. Typically, the standard deviation is calculated by taking the number of available satellites at the current epoch and the previous four epochs. Let the observation quality index threshold be... Here, we typically take 10, and the observation quality level is divided into:

[0059] When the observation quality level changes, the window length is reset to the minimum. Usually, if the observation quality level remains unchanged after calculating 3 observation epochs, the window length is gradually increased; otherwise, the minimum window length is maintained.

[0060] Let the adjusted window length be... The maximum window length is Typically, 30 is chosen, and the minimum window length is... Typically, the value is 1, and the observation quality level is maintained for a period of time. The formula for increasing the window length is:

[0061] Estimated observation noise covariance matrix The calculation formula is:

[0062] In the formula, For dynamic window length, This is the weighting factor.

[0063] Therefore, the robustness factor is:

[0064] In the formula, This indicates taking the trace of the matrix. It is a constant, and is usually taken as 1 here.

[0065] The formula acting on the original observation noise covariance matrix is:

[0066] In the formula, The robust observation noise covariance matrix is ​​then substituted into the measurement update process of the Kalman filter to complete a robust filtering step.

[0067] In practical applications, such as Figure 3, Figure 4 and Figure 5 The figures show a comparison of the positioning error curves for the improved robust Kalman filter (DWLWRKF), the traditional IAE-window-based Sage-Husa robust Kalman filter (RKF), and the classic extended Kalman filter (EKF) in complex urban environments (tree-lined roads, urban canyons) for eastward, northward, and celestial directions. It can be seen that the EKF, due to the lack of any measures to suppress observational gross errors, has a larger error and its error curve fluctuates significantly. The RKF suppresses observational gross errors, but its adaptability is poor in areas where faults occur and change frequently; its fault handling is lagging, its filtering is unstable, and its errors accumulate significantly. DWLW RKF distinguishes between well-observed and poorly-observed areas by using changes in the number of observable satellites in these road sections, and adjusts the length of the information window in real time to adapt to changes in information distribution. In addition, DWLW RKF weights and amplifies the current information, and has high processing efficiency for sudden faults. Therefore, its overall position error is relatively small. According to calculations, compared with the classic extended Kalman filter, its root mean square (RMSE) position errors in the east, north, and sky directions are reduced by 29.32%, 36.46%, and 48.65%, respectively. Compared with the traditional Sage-Husa robust Kalman filter based on IAE windowing, its RMSE position errors in the east, north, and sky directions are reduced by 19.10%, 18.19%, and 6.45%, respectively, significantly reducing the positioning error of the integrated navigation system in complex urban environments.

[0068] exist Figure 6 A comparison of the cumulative distribution curves of the absolute values ​​of three-dimensional errors of the improved robust Kalman filter (DWLW RKF), the traditional IAE-window-based Sage-Husa robust Kalman filter (RKF), and the classic extended Kalman filter (EKF) in complex urban environments (tree-lined roads, urban canyons) shows that in most of the middle region, the curve of DWLW RKF is closer to the upper left corner than that of RKF and EKF, and the error values ​​are more concentrated in the small range. The proportion of errors less than 2m is about 80% for DWLW RKF, while the proportions of errors less than 2m for RKF and EKF are 70.2% and 63.8%, respectively. This indicates that DWLW RKF has better and more stable anti-interference performance.

[0069] The improved robust Kalman filter integrated navigation system based on chi-square detection provided by the present invention will be described below. The improved robust Kalman filter integrated navigation system based on chi-square detection described below can be referred to in correspondence with the improved robust Kalman filter integrated navigation method based on chi-square detection described above.

[0070] Figure 7 This is a schematic diagram of the structure of the improved robust Kalman filter integrated navigation system based on chi-square detection provided in an embodiment of the present invention, as shown below.Figure 7 As shown, it includes: a construction module 71, a calculation module 72, an improvement module 73, and a suppression module 74, wherein: The construction module 71 is used to construct the basic framework of GNSS / INS loosely integrated navigation Kalman filtering; the calculation module 72 is used to calculate the chi-square test of the innovation before updating the integrated navigation Kalman filter measurement, record the number of available satellites at each epoch, and calculate the satellite observation quality index based on the number of available satellites; the improvement module 73 is used to improve the Sage-Husa robust Kalman filter based on IAE windowing, determine the weighting factor through the chi-square test of the innovation, and adjust the weight of the innovation at the current epoch within the window, and dynamically adjust the window length according to the changes in the satellite observation quality index; the suppression module 74 is used to apply the robust factor calculated by the improved Sage-Husa robust Kalman filter based on IAE windowing to the observation noise covariance matrix, causing the observation noise covariance matrix to expand, reducing the weight of observation information with gross errors in data fusion, so as to suppress observation gross errors.

[0071] Figure 8 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 8 As shown, the electronic device may include: a processor 810, a communication interface 820, a memory 830, and a communication bus 840, wherein the processor 810, the communication interface 820, and the memory 830 communicate with each other through the communication bus 840. The processor 810 can call logic instructions in the memory 830 to execute an improved robust Kalman filter integrated navigation method based on chi-square detection. This method includes: constructing a basic framework for GNSS / INS loosely coupled navigation Kalman filtering; calculating the chi-square test value for the new information before updating the Kalman filter measurements for the integrated navigation, recording the number of available satellites at each epoch, and calculating the satellite observation quality index based on the number of available satellites; improving the Sage-Husa robust Kalman filter based on IAE windowing, determining a weighting factor through the chi-square test value for the new information, and dynamically adjusting the window length according to changes in the satellite observation quality index; and applying the robust factor calculated using the improved Sage-Husa robust Kalman filter based on IAE windowing to the observation noise covariance matrix, causing the observation noise covariance matrix to expand and reducing the weight of observation information with gross errors in data fusion, thereby suppressing observation gross errors.

[0072] Furthermore, the logical instructions in the aforementioned memory 830 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0073] On the other hand, the present invention also provides a computer program product, which includes a computer program that can be stored on a non-transitory computer-readable storage medium. When the computer program is executed by a processor, the computer can execute the improved robust Kalman filter integrated navigation method based on chi-square detection provided by the above methods. The method includes: constructing a basic framework for GNSS / INS loose integrated navigation Kalman filtering; calculating the chi-square test value of the new information before updating the integrated navigation Kalman filter measurement, recording the number of available satellites at each epoch, and calculating the satellite observation quality index based on the number of available satellites; improving the Sage-Husa robust Kalman filter based on IAE windowing, determining the weighting factor through the chi-square test value of the new information, which is used to adjust the weight of the new information at the current epoch within the window, and dynamically adjusting the window length according to the change of the satellite observation quality index; using the robust factor calculated by the improved Sage-Husa robust Kalman filter based on IAE windowing to act on the observation noise covariance matrix, causing the observation noise covariance matrix to expand, reducing the weight of observation information with gross errors in data fusion, so as to suppress observation gross errors.

[0074] In another aspect, the present invention also provides a non-transitory computer-readable storage medium storing a computer program thereon. When executed by a processor, the computer program performs the improved robust Kalman filter integrated navigation method based on chi-square detection provided by the methods described above. The method includes: constructing a basic framework for GNSS / INS loose integrated navigation Kalman filtering; calculating the chi-square test value of the new information before updating the integrated navigation Kalman filter measurement, recording the number of available satellites at each epoch, and calculating the satellite observation quality index based on the number of available satellites; improving the Sage-Husa robust Kalman filter based on IAE windowing, determining a weighting factor through the chi-square test value of the new information, which is used to adjust the weight of the new information at the current epoch within the window, and dynamically adjusting the window length according to the changes in the satellite observation quality index; and applying the robust factor calculated by the improved Sage-Husa robust Kalman filter based on IAE windowing to the observation noise covariance matrix, causing the observation noise covariance matrix to expand, reducing the weight of observation information with gross errors in data fusion, thereby suppressing observation gross errors.

[0075] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0076] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0077] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An improved robust Kalman filter integrated navigation method based on chi-square detection, characterized in that, include: Constructing a basic framework for Kalman filtering in GNSS / INS loosely integrated navigation; Before updating the integrated navigation Kalman filter measurements, calculate the chi-square test of the new information, record the number of available satellites for each epoch, and calculate the satellite observation quality index based on the number of available satellites. An improvement is made to the Sage-Husa robust Kalman filter based on IAE windowing. The weighting factor is determined by the chi-square test of the new information and used to adjust the weight of the current epoch information within the window. The window length is dynamically adjusted according to the changes in satellite observation quality indicators. The robustness factor calculated by the improved Sage-Husa robust Kalman filter based on IAE windowing is applied to the observation noise covariance matrix, causing the observation noise covariance matrix to expand and reducing the weight of observation information with gross errors in data fusion, thereby suppressing observation gross errors.

2. The improved robust Kalman filter integrated navigation method based on chi-square detection according to claim 1, characterized in that, Constructing the basic framework of Kalman filtering for GNSS / INS loosely integrated navigation, including: Constructing state vectors ,in For three-dimensional position error, For three-dimensional velocity error, For three-dimensional attitude error, For three-dimensional gyroscopes with zero bias, Zero bias for three-dimensional accelerometers; From the state vector Constructing state equations ,in Let be the state vector for the next state. Here is the state transition matrix. The noise figure matrix is... This is system noise; Construct observation vectors ,in The GNSS antenna phase center position vector predicted by INS mechanical orchestration. The GNSS antenna phase center position vector is obtained directly from GNSS RTK calculation; From the observation vector Constructing the observation matrix ,in For the observation matrix, To observe noise; Construct the Kalman filter prediction update formula: in, Let be the prior state vector of the k-th epoch. Let be the state vector of the (k-1)th epoch. This is the state transition matrix from the (k-1)th epoch to the kth epoch. Let be the prior state covariance matrix of the k-th epoch. Let be the state covariance matrix of the (k-1)th epoch. Let be the noise coefficient matrix of the (k-1)th epoch. Let be the system noise covariance matrix at epoch k-1, with superscript . Indicates matrix transpose; Construct the Kalman filter measurement update formula: in, Let Kalman filter gain be the value at epoch k. Let be the observation matrix of the k-th epoch. Let be the observation noise covariance matrix of the k-th epoch. Let be the posterior state vector of the k-th epoch. Let be the observation vector at the k-th epoch. Let be the posterior state covariance matrix of the k-th epoch. It is an identity matrix.

3. The improved robust Kalman filter integrated navigation method based on chi-square detection according to claim 2, characterized in that, Before updating the integrated navigation Kalman filter measurements, the chi-square test value for the new information is calculated, the number of available satellites at each epoch is recorded, and satellite observation quality indicators are calculated based on the number of available satellites, including: New Chi-square test quantity The calculation formula is: in, The information vector for the k-th epoch is calculated using the following formula. : in, Let be the prior state vector of the k-th epoch, and... same, for covariance matrix The inverse is calculated using the following formula. : 。 4. The improved robust Kalman filter integrated navigation method based on chi-square detection according to claim 3, characterized in that, An improvement to the IAE-based Sage-Husa robust Kalman filter is made by determining the weighting factor through the chi-square test of the innovation, which is used to adjust the weight of the current epoch innovation within the window. The window length is dynamically adjusted according to changes in satellite observation quality indicators, including: Determine the weighting factors The calculation formula is as follows: in, The chi-square detection threshold for new information; In Sage-Husa robust Kalman filtering, the formula for IAE windowing is: in, Calculated using the current information The estimated value, For window length, Indicates the first The new information vector of an epoch, Indicates the first The transpose of the epoch's information vector; An improvement to the IAE-windowed Sage-Husa robust Kalman filter is made by weighting the innovation within the window: 。 5. The improved robust Kalman filter integrated navigation method based on chi-square detection according to claim 4, characterized in that, Satellite observation quality indicators The calculation formula is: in, Indicates the current number k The quality indicators of satellite observations over an epoch. This indicates the maximum number of available satellites. Indicates the current number k Number of satellites available in an epoch. This represents the standard deviation of the number of available satellites near the current epoch. The threshold for observation quality indicators is The observation quality level is then divided into: When the observation quality level changes, the window length is reset to the minimum. If the observation quality level remains unchanged after several observation epochs, the window length is gradually increased; otherwise, the minimum window length is maintained. The adjusted window length is determined to be Maximum window length is Minimum window length is The observation quality level was maintained for a period of time. The formula for increasing the window length is: 。 6. The improved robust Kalman filter integrated navigation method based on chi-square detection according to claim 5, characterized in that, The robustness factor calculated using the improved IAE-windowed Sage-Husa robust Kalman filter is applied to the observation noise covariance matrix, causing it to expand and thus yielding the estimated observation noise covariance matrix. To reduce the weight of observations with gross errors in data fusion, in order to suppress observational gross errors, the following measures are taken: in, For dynamic window length, As a weighting factor; The robustness factor is: in, This indicates taking the trace of the matrix. It is a constant; The formula acting on the original observation noise covariance matrix is: in, This is the robust observation noise covariance matrix.

7. An improved robust Kalman filter integrated navigation system based on chi-square detection, characterized in that, include: The building block is used to construct the basic framework for Kalman filtering in GNSS / INS loosely integrated navigation; The calculation module is used to calculate the chi-square test of the new information before updating the Kalman filter measurement of the integrated navigation, record the number of available satellites in each epoch, and calculate the satellite observation quality index based on the number of available satellites. The improved module is used to improve the Sage-Husa robust Kalman filter based on IAE windowing. The weighting factor is determined by the chi-square test of the new information and used to adjust the weight of the new information of the current epoch within the window. The window length is dynamically adjusted according to the changes in satellite observation quality indicators. The suppression module is used to apply the robust factor calculated by the improved IAE-window-based Sage-Husa robust Kalman filter to the observation noise covariance matrix, thereby expanding the observation noise covariance matrix and reducing the weight of observation information with gross errors in data fusion, so as to suppress observation gross errors.

8. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the improved robust Kalman filter integrated navigation method based on chi-square detection as described in any one of claims 1 to 6.

9. A non-transitory computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the improved robust Kalman filter integrated navigation method based on chi-square detection as described in any one of claims 1 to 6.

10. A computer program product, comprising a computer program, characterized in that, When the computer program is executed by the processor, it implements the improved robust Kalman filter integrated navigation method based on chi-square detection as described in any one of claims 1 to 6.

Images