High-real-time GNSS and IMU integrated navigation method

Abstract

The invention discloses a high-real-time GNSS (Global Navigation Satellite System) and IMU (Inertial Measurement Unit) integrated navigation method, which comprises the following steps of: acquiring an initial state vector, and setting an initial covariance matrix; predicting the state by using the state transition matrix to obtain priori estimation of the state at the next moment; calculating a prior covariance matrix by using the state transition matrix and diagonal elements of the covariance matrix in the previous step; calculating a Kalman gain by using the covariance matrix; the prior state estimation is updated by using Kalman gain, and posterior state estimation is obtained by combining the observation value; updating the covariance matrix to obtain a posterior covariance matrix; calculating posterior state estimation at the moment k; the problems that in GNSS and IMU data fusion, along with the increase of state vector dimensions and the improvement of sensor data updating frequency, the requirements of a traditional Kalman filter in the aspects of calculation and storage are exponentially increased, application and popularization in high-real-time application are seriously restricted, and the real-time performance is poor are solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation and positioning, and in particular relates to a high real-time GNSS and IMU combined navigation method. Background Art

[0002] In modern navigation and positioning systems, the application of high-precision positioning algorithms is becoming more and more extensive, covering many key areas such as autonomous driving, drone navigation, and line sway monitoring. These applications place extremely high demands on the accuracy and real-time performance of positioning. However, high-precision positioning algorithms are usually accompanied by complex calculation processes and high consumption of computing resources, resulting in severe challenges in the system's real-time processing capabilities. Specifically, high-precision positioning algorithms need to process a large amount of sensor data, such as global navigation satellite systems and inertial measurement units. The fusion of multi-source data and complex filtering calculations significantly increase the computational complexity of the algorithm. This high complexity not only increases the hardware cost and energy consumption of the system, but may also cause processing delays, which in turn affects the real-time response capability of the navigation system and the timeliness of monitoring.

[0003] Currently, Kalman filters and their extended forms (such as extended Kalman filters) have been widely used in GNSS and IMU data fusion. This type of filter achieves high-precision estimation of state variables such as position, velocity, and attitude angle by iteratively updating the system state vector and covariance matrix. However, with the increase in the dimension of the state vector and the increase in the frequency of sensor data updates, the demand for computing and storage of traditional Kalman filters has increased exponentially, which has seriously restricted its promotion and application in high-real-time applications. In addition, the update process of the filter's covariance matrix requires a large number of matrix operations, which is particularly prominent in resource-constrained embedded systems, further exacerbating the difficulty of real-time processing. Summary of the invention

[0004] The technical problem to be solved by the present invention is to provide a high real-time GNSS and IMU combined navigation method to solve the problems existing in the prior art of GNSS and IMU data fusion, such as the exponential growth of the computing and storage requirements of the traditional Kalman filter with the increase of the state vector dimension and the increase of the sensor data update frequency, which seriously restricts its promotion and application in high real-time applications, and the poor real-time performance caused by the need to perform a large number of matrix operations in the covariance matrix update process of the filter.

[0005] Technical solution of the present invention:

[0006] A high real-time GNSS and IMU combined navigation method, the method comprising:

[0007] Step 1, obtain the initial state vector x0 and set the initial covariance matrix P0;

[0008] Step 2: Use the state transfer matrix F k Predict the state and get a priori estimate of the state at the next moment

[0009] Step 3: Use the state transfer matrix F k And the covariance matrix of the previous step The diagonal elements of the prior covariance matrix P are calculated k|k-1 ;

[0010] Step 4: Use the covariance matrix Calculate the Kalman gain K k ;

[0011] Step 5: Use Kalman gain K k Update the prior state estimate and combine it with the observed value z k , and obtain the posterior state estimate

[0012] Step 6: Update the covariance matrix to obtain the posterior covariance matrix P k|k ;

[0013] Step 7: Calculate the posterior state estimate at time k

[0014] The method further comprises:

[0015] Step 8: Calculate the residual vector x k represents the observation vector at time k;

[0016] Step 9: Calculate the residual covariance matrix

[0017] Step 10: Calculate the chi-square statistic

[0018] Step 11: If J k >γ, then adjust R k and Q k , adjust R k and Q k Then jump to step 2, otherwise jump to step 12, R k and Q k The adjustment formula is:

[0019] [R k ] ij =β*[R k ] ij

[0020] [Q k ] ij =δ*[Qk ] ij

[0021] γ is the set threshold, [R k ] ij Represents the observation noise covariance matrix R k The i-th row and j-th column element, [Q k ] ij Denotes the process noise covariance matrix Q k The i-th row and j-th column element of , β represents the observation noise adjustment coefficient, δ represents the process noise adjustment coefficient, and both β and δ are numbers greater than 1;

[0022] Step 12: Output the posterior state estimate at time k Increase the time k by 1 and repeat steps 2 to 11.

[0023] The initial state vector x0 in step 1 includes position p0, velocity v0 and attitude angle θ0, which are set according to the initial position data of GNSS and the initial velocity and attitude data of IMU. The setting method of the initial covariance matrix P0 is:

[0024]

[0025] Where: Represents the uncertainty variance of the initial position, which is set according to the initial positioning accuracy of GNSS; Represents the uncertainty variance of the initial velocity, which is set according to the initial velocity estimation accuracy of the IMU; σ 2 θ0 It represents the uncertainty variance of the initial attitude angle, which is set according to the attitude estimation accuracy of the IMU; I3 represents the 3×3 unit matrix.

[0026] A priori estimate of the state at the next moment The calculation formula is:

[0027]

[0028] Among them, B k is the control input matrix; u k is the control input vector measured by the IMU, including the acceleration a k and angular velocity ω k , x k-1 Represents the state vector at time k-1.

[0029] Prior covariance matrix P k|k-1 The calculation formula is:

[0030]

[0031] Among them, [F k ]ij Denotes the state transfer matrix F k The i-th row and j-th column element of Represents the covariance matrix of the previous step The j-th diagonal element of k ] ii is the process noise covariance matrix Q k The i-th diagonal element of k|k-1 ] ii Denotes the prior covariance matrix P k|k-1 The i-th diagonal element of ; represents the simplified prior covariance matrix, containing only the diagonal elements.

[0032] Calculate the Kalman gain K k The calculation formula is:

[0033]

[0034] Among them, [H k ] i Denotes the observation matrix H k The i-th row of represents the relationship between the i-th observation and the state vector; [R k ] ii Represents the observation noise covariance matrix R k The i-th diagonal element of k ] i represents the Kalman gain K k The ith element of .

[0035] The posterior state estimate x^ k|k The calculation formula is:

[0036]

[0037] Among them, [z k ] i represents the i-th element of the observation vector; [H k ] i represents the i-th row of the observation matrix; represents the posterior state estimate The ith element of .

[0038] The posterior covariance matrix P k|k The calculation method is:

[0039]

[0040] Among them, [P k|k ] ii represents the i-th diagonal element of the posterior covariance matrix.

[0041] If if J k >γ, adjust R k and Q k The methods include:

[0042] Step 11.1: Adjust the observation noise covariance matrix R k : By increasing R k The corresponding elements of reflect the increase in uncertainty of observation data, thereby reducing the reliance on current observation data;

[0043] Step 11.2: Adjust the process noise covariance matrix Q k : By increasing Q k The corresponding elements reflect the uncertainty of the system dynamics and allow greater flexibility in state prediction;

[0044] Step 11.3. Switch the covariance matrix simplification method: Switch from the diagonal approximation to the sparsification method to retain more important covariance elements and improve the estimation accuracy.

[0045] Beneficial effects of the present invention:

[0046] The present invention reduces the number and scale of matrix operations by performing diagonal approximation on the covariance matrix and retaining only the diagonal elements. This optimization enables the algorithm to maintain efficient real-time processing capabilities under high-frequency data updates, meet the strict real-time requirements of applications such as wire status monitoring, and ensure that the system can quickly respond to changes in dynamic environments.

[0047] The present invention introduces an error evaluation and adaptive adjustment mechanism to ensure that the observation noise and process noise covariance matrix can be dynamically adjusted when necessary. This mechanism effectively balances computational efficiency and estimation accuracy, ensuring that the system can still maintain high-precision positioning performance in complex and dynamic environments, meeting the needs of line sway monitoring for high-precision positioning.

[0048] Since the present invention optimizes the calculation process of the Kalman filter, it reduces the dependence on high-performance computing resources, thereby reducing the system's demand for hardware resources. This can not only reduce the overall hardware cost of the system, but also reduce energy consumption and extend the working time of the device, and is particularly suitable for resource-constrained embedded systems and portable devices.

[0049] The error evaluation and adaptive adjustment mechanism of the present invention enables the navigation system to dynamically adjust the filter parameters according to real-time environmental changes. When the estimated error is detected to exceed the preset threshold, the system can automatically adjust the observation noise and process noise. This adaptive capability significantly improves the robustness of the system in complex and changing environments, ensuring stable operation under various conditions.

[0050] The problem existing in the existing technology of GNSS and IMU data fusion is solved. With the increase of state vector dimension and the increase of sensor data update frequency, the demand for computing and storage of traditional Kalman filter grows exponentially, which seriously restricts its promotion and application in high real-time applications, and the covariance matrix update process of the filter requires a large number of matrix operations, resulting in poor real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION

[0052] Implementation Example 1: Reference Figure 1 , a high real-time GNSS and IMU combined navigation method, the method comprising:

[0053] Step 1: Get the initial state vector x0 and set the initial covariance matrix P0. The initial state vector x0 includes position p0, velocity v0 and attitude angle θ0. It is set according to the initial position data of GNSS and the initial velocity and attitude data of IMU. The setting method of the initial covariance matrix P0 is as follows:

[0054]

[0055] in, Represents the uncertainty variance of the initial position, which is set according to the initial positioning accuracy of GNSS; Represents the uncertainty variance of the initial velocity, which is set according to the initial velocity estimation accuracy of the IMU; represents the uncertainty variance of the initial attitude angle, which is set according to the attitude estimation accuracy of the IMU; I3 represents the 3×3 unit matrix;

[0056] Step 2: Use the state transfer matrix F k Predict the state and get a priori estimate of the state at the next moment

[0057]

[0058] Among them, B k is the control input matrix; u k is the control input vector measured by the IMU, including the acceleration a k and angular velocity ω k , x k-1 represents the state vector at time k-1;

[0059] Step 3: Use the state transfer matrix F k And the covariance matrix of the previous step The diagonal elements of the prior covariance matrix P are calculated k|k-1,

[0060]

[0061] Among them, [F k ] ij Denotes the state transfer matrix F k The i-th row and j-th column element of Represents the covariance matrix of the previous step The j-th diagonal element of k ] ii is the process noise covariance matrix Q k The i-th diagonal element of k|k-1 ] ii Denotes the prior covariance matrix P k|k-1 The i-th diagonal element of ; represents the simplified prior covariance matrix, containing only the diagonal elements.

[0062] Step 4: Use the covariance matrix Calculate the Kalman gain K k ,

[0063]

[0064] Among them, [H k ] i Denotes the observation matrix H k The i-th row of represents the relationship between the i-th observation and the state vector; [R k ] ii Represents the observation noise covariance matrix R k The i-th diagonal element of k ] i represents the Kalman gain K k The ith element of .

[0065] Step 5: Use Kalman gain K k Update the prior state estimate and combine it with the observed value z k , and obtain the posterior state estimate

[0066]

[0067] Among them, [z k ] i represents the i-th element of the observation vector; [H k ] i represents the i-th row of the observation matrix; represents the posterior state estimate The ith element of .

[0068] Step 6: Update the covariance matrix to obtain the posterior covariance matrix P k|k ,

[0069]

[0070] Among them, [P k|k ] ii represents the i-th diagonal element of the posterior covariance matrix;

[0071] Step 7: Calculate the posterior state estimate at time k

[0072] Step 8: Calculate the residual vector x k represents the observation vector at time k;

[0073] Step 9: Calculate the residual covariance matrix

[0074] Step 10: Calculate the chi-square statistic

[0075] Step 11: If J k >γ, then adjust R k and Q k , adjust R k and Q k Then jump to step 2, otherwise jump to step 12, R k and Q k The adjustment method is as follows:

[0076] [R k ] ij =β*[R k ] ij

[0077] [Q k ] ij =δ*[Q k ] ij

[0078] Among them, γ is the set threshold, [R k ] ij Represents the observation noise covariance matrix R k The i-th row and j-th column element, [Q k ] ij Denotes the process noise covariance matrix Q k The i-th row and j-th column element of , β represents the observation noise adjustment coefficient, δ represents the process noise adjustment coefficient, and both β and δ are numbers greater than 1;

[0079] Step 12: Output the posterior state estimate at time k Increase the time k by 1 and repeat steps 2 to 11.

[0080] The residual vector is the difference between the actual observation and the predicted observation. Specifically, the system compares the actual position observation obtained by GNSS with the position information predicted by IMU to obtain the residual vector. This vector reflects the inconsistency between the current observation data and the system prediction, and is a key indicator for evaluating the performance of the filter. The residual covariance matrix measures the uncertainty of the residual vector, including the combined effects of observation noise and prediction uncertainty. By calculating the residual covariance matrix, the reliability of the current observation data and the accuracy of the prediction can be quantified, providing a basis for the subsequent calculation of the chi-square statistic. The chi-square statistic is used to evaluate the significance of the residual vector. This statistic reflects the degree of consistency between the observed data and the predicted data. If the chi-square statistic value is large, it means that there is a significant difference between the observed data and the predicted data, which may be caused by sensor anomalies, environmental changes, or system dynamic changes.

[0081] Compare the calculated chi-square statistic Jk with the preset threshold γ:

[0082] If J k >γ, it is considered that the current observed data is abnormal or the system dynamics have changed, and the filter parameters need to be adjusted. The specific adjustment methods include:

[0083] o Adjust the observation noise covariance matrix (R k ): By increasing R k The corresponding elements of reflect the increase in uncertainty of the observation data, thereby reducing the reliance on the current observation data.

[0084] o Adjust the process noise covariance matrix (Q k ): By increasing Q k The corresponding elements reflect the uncertainty of the system's dynamic changes and allow greater flexibility in state prediction.

[0085] o Switch the covariance matrix simplification method: for example, switch from diagonal approximation to sparsification method to retain more important covariance elements and improve estimation accuracy.

[0086] These adjustments enhance the system’s adaptability to dynamic changes by relaxing the filter’s reliance on current observations, ensuring that the filter remains stable and accurate in a changing environment.

[0087] Otherwise, J k ≤γ, the current observed data is considered to be consistent with the predicted data, and there is no need to adjust the filter parameters. Jump directly to step 12 to continue the normal filtering process.

[0088] The high real-time GNSS and IMU combined navigation method proposed in the present invention realizes real-time monitoring and high-precision positioning of line dancing by integrating the precise position information provided by GNSS and the high-frequency dynamic data of IMU. In practical applications, the initial position, speed and attitude angle of the line are first obtained by GNSS and IMU, and the corresponding uncertainty covariance matrix is ​​set. Subsequently, the state transfer matrix is ​​used to predict the state at the next moment, and the covariance matrix is ​​simplified by the diagonal approximation method to reduce the computational complexity. When it is detected that the system estimation error exceeds the preset threshold, the error evaluation and adaptive adjustment mechanism are introduced to ensure that the observation noise and process noise covariance matrix can be dynamically adjusted when necessary. This mechanism effectively balances the computational efficiency and estimation accuracy, ensuring that the system can still maintain high-precision positioning performance in complex and dynamic environments, and meet the needs of line dancing monitoring for high-precision positioning. Through the calculation and state update of the Kalman gain, the state estimation is continuously optimized in combination with the observation data, and finally the efficient and accurate monitoring of line dancing is realized. This method effectively solves the contradiction between the complexity and real-time processing capability of the high-precision positioning algorithm, ensuring that the system can still provide reliable navigation and monitoring results in a dynamic environment, and is suitable for the monitoring of power transmission lines.

Claims

1. A high real-time GNSS and IMU combined navigation method, characterized by: The method comprises: Step 1, obtain the initial state vector x0 and set the initial covariance matrix P0; Step 2: Use the state transfer matrix F k Predict the state and get a priori estimate of the state at the next moment Step 3: Use the state transfer matrix F k And the covariance matrix of the previous step The diagonal elements of the prior covariance matrix P are calculated k|k-1 ; Step 4: Use the covariance matrix Calculate the Kalman gain K k ; Step 5: Use Kalman gain K k Update the prior state estimate and combine it with the observed value z k , and obtain the posterior state estimate Step 6: Update the covariance matrix to obtain the posterior covariance matrix P k|k ; Step 7: Calculate the posterior state estimate at time k 2. A high real-time GNSS and IMU integrated navigation method according to claim 1, characterized in that: The method further comprises: Step 8: Calculate the residual vector x k represents the observation vector at time k; Step 9: Calculate the residual covariance matrix Step 10: Calculate the chi-square statistic Step 11: If J k >γ, then adjust R k and Q k , adjust R k and Q k Then jump to step 2, otherwise jump to step 12, R k and Q k The adjustment formula is: [R k ] ij =β*[R k ] ij [Q k ] ij =δ*[Q k ] ij γ is the set threshold, [R k ] ij Represents the observation noise covariance matrix R k The i-th row and j-th column element, [Q k ] ij Denotes the process noise covariance matrix Q k The i-th row and j-th column element of , β represents the observation noise adjustment coefficient, δ represents the process noise adjustment coefficient, and both β and δ are numbers greater than 1; Step 12: Output the posterior state estimate at time k Increase the time k by 1 and repeat steps 2 to 11.

3. The high real-time GNSS and IMU integrated navigation method according to claim 1, characterized in that: The initial state vector x0 in step 1 includes position p0, velocity v0 and attitude angle θ0, which are set according to the initial position data of GNSS and the initial velocity and attitude data of IMU. The setting method of the initial covariance matrix P0 is: Where: Represents the uncertainty variance of the initial position, which is set according to the initial positioning accuracy of GNSS; Represents the uncertainty variance of the initial velocity, which is set according to the initial velocity estimation accuracy of the IMU; It represents the uncertainty variance of the initial attitude angle, which is set according to the attitude estimation accuracy of the IMU; I3 represents the 3×3 unit matrix.

4. The high real-time GNSS and IMU integrated navigation method according to claim 1, characterized in that: A priori estimate of the state at the next moment The calculation formula is: Among them, B k is the control input matrix; u k is the control input vector measured by the IMU, including the acceleration a k and angular velocity ω k , x k-1 Represents the state vector at time k-1.

5. The high real-time GNSS and IMU integrated navigation method according to claim 1, characterized in that: Prior covariance matrix P k|k-1 The calculation formula is: Among them, [F k ] ij Represents the state transfer matrix F k The i-th row and j-th column element of Represents the covariance matrix of the previous step The j-th diagonal element of k ] ii is the process noise covariance matrix Q k The i-th diagonal element of k|k-1 ] ii Denotes the prior covariance matrix P k|k-1 The i-th diagonal element of ; represents the simplified prior covariance matrix, containing only the diagonal elements.

6. A high real-time GNSS and IMU integrated navigation method according to claim 1, characterized in that: Calculate the Kalman gain K k The calculation formula is: Among them, [H k ] i Denotes the observation matrix H k The i-th row of represents the relationship between the i-th observation and the state vector; [R k ] ii Represents the observation noise covariance matrix R k The i-th diagonal element of k ] i represents the Kalman gain K k The ith element of .

7. The high real-time GNSS and IMU integrated navigation method according to claim 1, characterized in that: Posterior state estimation The calculation formula is: Among them, [z k ] i represents the i-th element of the observation vector; [H k ] i represents the i-th row of the observation matrix; represents the posterior state estimate The ith element of .

8. The high real-time GNSS and IMU integrated navigation method according to claim 1, characterized in that: The posterior covariance matrix P k|k The calculation method is: Among them, [P k|k ] ii represents the i-th diagonal element of the posterior covariance matrix.

9. The high real-time GNSS and IMU integrated navigation method according to claim 2, characterized in that: If if J k >γ, adjust R k and Q k The methods include: Step 11.1: Adjust the observation noise covariance matrix R k : By increasing R k The corresponding elements of reflect the increase in uncertainty of observation data, thereby reducing the reliance on current observation data; Step 11.2: Adjust the process noise covariance matrix Q k : By increasing Q k The corresponding elements reflect the uncertainty of the system dynamics and allow greater flexibility in state prediction; Step 11.

3. Switch the covariance matrix simplification method: Switch from the diagonal approximation to the sparsification method to retain more important covariance elements and improve the estimation accuracy.

Images