An unmanned aerial vehicle inertial measurement unit switching method based on improved Sage-Husa filtering
By improving the Sage-Husa filtering algorithm, the navigation instability problem when switching from a high-precision inertial navigation system to a low-precision MEMS inertial navigation system was solved, realizing a smooth transition and safe flight of the UAV navigation system.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- QINGDAO YILAN AVIATION CO LTD
- Filing Date
- 2026-03-02
- Publication Date
- 2026-05-29
Smart Images
Figure CN122108101A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of UAV navigation, specifically relating to a technology for achieving fast filter convergence based on an improved Sage-Husa filtering algorithm during the switching process between a high-precision inertial measurement unit and a low-precision MEMS inertial measurement unit in a UAV integrated navigation system. Background Technology
[0002] In UAV navigation systems, the continuity and reliability of navigation information are directly related to flight safety. Therefore, navigation systems commonly employ multiple inertial navigation systems (INS) to create a redundant framework. Typically, a high-precision fiber optic or laser INS serves as the primary system, while one or more low-precision MEMS INS serve as backups. This redundancy design allows for rapid switching to the backup INS in the event of a sudden failure of the primary system. It also avoids the risk of UAV loss of control due to attitude information interruption when both the primary INS and the satellite navigation system fail simultaneously, thus ensuring safe flight for the UAV.
[0003] High-precision inertial navigation systems (INS), as core components providing continuous navigation information, have a low probability of failure, but can still malfunction due to sudden hardware failures, extreme environmental interference, or other factors. Once a INS fails, it directly leads to navigation information interruption, causing serious consequences. Therefore, equipping a backup redundant INS is a necessary design to ensure the safe flight of UAVs, as it can intervene and maintain navigation functionality when the primary INS fails. However, the noise characteristics of the two differ significantly. The zero-bias instability and random walk coefficient of MEMS INS are much higher than those of high-precision INS, causing drastic changes in the statistical characteristics of process noise and measurement noise. Furthermore, state estimation during switching is prone to jumps. Traditional filtering algorithms, relying on prior noise information, struggle to adapt quickly to changes, resulting in slow convergence or even divergence, and can lead to abrupt changes in navigation results, seriously affecting the flight safety of UAVs. Summary of the Invention
[0004] This invention addresses the issues of slow filter convergence, poor navigation stability, and abrupt changes in results caused by changes in noise characteristics and discontinuities in state estimation when switching from a high-precision inertial group configured for safety redundancy to a low-precision MEMS inertial group in UAV navigation systems. It provides a fast convergence method based on an improved Sage-Husa filter to ensure a smooth switching process.
[0005] This invention discloses a UAV inertial navigation system (INS) switching method based on an improved Sage-Husa filter, comprising: Step 1: Real-time acquisition of acceleration and angular velocity output data from a high-precision INS; when a fault is detected, switching detection is performed, and INS switching is completed; Step 2: Initial parameter adjustment; the uncertainty of the initial value at the moment of switching is adapted by amplifying the state estimation covariance matrix P; Step 3: Attenuation memory Sage-Husa filtering is performed to complete innovation calculation, noise estimation update, and Kalman filtering processing, achieving accurate estimation of the system state; Step 4: The calculated innovation is compared with a preset threshold; when the innovation exceeds the threshold, strong tracking filtering is performed; Step 5: The adjusted process noise variance matrix Q, measurement noise variance matrix R, and state estimation covariance matrix P are updated, and data from satellite navigation and MEMS INS are fused to achieve navigation solution based on Kalman filtering, and the navigation results are output in real time.
[0006] In step one, failure detection is achieved using acceleration and angular velocity data output by the high-precision inertial navigation system itself. The process is as follows: Parameter settings: Set the maximum range of data acquired by the high-precision inertial group. When the value of the data acquired by the sensor exceeds the range, the high-precision inertial group is judged to be faulty and inertial group switching is performed. Data acquisition and processing: If the sensor data does not exceed the range, the raw triaxial angular velocity data is filtered in real time through a Butterworth low-pass filter to remove high-frequency interference caused by the UAV’s own vibration, and the smoothed triaxial angular velocity is obtained. The change in angular acceleration is calculated to determine whether inertial navigation system switching is required.
[0007] In step two, upon receiving the switching signal, the system adjusts the state estimation covariance matrix P based on the MEMS inertial group's own performance indicators, actively setting it to 5-10 times the value used by the high-precision inertial group during normal operation. Increasing the value of the state estimation covariance matrix proactively adapts to the high initial uncertainty of the MEMS inertial group during switching, avoiding jumps in state estimation due to significant differences in the error characteristics of the two inertial groups, and improving the balance during the switching process. This method of expanding the state estimation covariance reduces instability factors caused by initial state deviations.
[0008] In step three, the decaying memory Sage-Husa filter is activated. The innovation, as a key indicator reflecting the difference between the actual measured and predicted values of the system, provides crucial data support for subsequent noise estimation and state updates, ensuring data consistency and validity. Its calculation method is as follows:
[0009] In the formula, For the new interest, For observation purposes, For the observation matrix, This is the predicted value of the state.
[0010] When switching to a MEMS inertial navigation system, the new information is calculated based on the MEMS inertial navigation system output data. The process noise covariance matrix Q and measurement noise covariance matrix R of the attenuated memory Sage-Husa filter are updated iteratively, with the update formula as follows:
[0011]
[0012]
[0013]
[0014] In the formula, This is the forgetting factor, typically ranging from 0.95 to 0.99. The current sampling count is set to 1 after switching. Because the above calculation process requires continuously storing intermediate variables during operation, it places high demands on the hardware storage performance and computational efficiency of the UAV. To simplify the process and improve computational efficiency, this invention uses the state variables from the previous time step to complete the iteration. The calculation formula is as follows:
[0015]
[0016] In step four, the new information needs to be processed. The new information is compared with a preset threshold, and a strong tracking factor is calculated when the threshold is exceeded. :
[0017] pass The prediction covariance matrix is modified, and the Kalman gain is increased to suppress state bias.
[0018] In step five, the navigation solution is based on the adjusted P, Q, R matrix to update the state, and integrates MEMS inertial navigation and satellite navigation data to output navigation parameters such as position, velocity, and attitude.
[0019] The state equation of the system is:
[0020] in, Here is the state transition matrix. This is the noise gain matrix. This is process noise.
[0021] The observation equation is as follows:
[0022] in, To observe noise.
[0023] The Kalman filter iterates according to the formula, and the prediction steps are as follows:
[0024]
[0025] The update steps are as follows:
[0026]
[0027]
[0028] The above processing steps can ensure that the navigation results during the UAV inertial navigation system switching process are continuous and stable without obvious abrupt changes, providing reliable navigation information for the safe flight of the UAV.
[0029] Compared with the prior art, the advantages of the present invention are: 1. The invention combines recursively updated Sage-Husa filtering with a forgetting factor to achieve a smooth transition of noise characteristics from high-precision inertial navigation systems to MEMS inertial navigation systems, avoiding sudden changes in navigation parameters.
[0030] 2. This invention uses a recursive update formula instead of cumulative summation, which optimizes computation and storage, and the algorithm can adapt to the limited hardware resources of UAVs.
[0031] 3. This invention achieves continuity assurance of state estimation. By actively adjusting the initial value of P at the moment of switching, and in conjunction with a strong tracking mechanism, the convergence speed of state estimation is improved. The strong tracking factor suppresses sudden errors in real time, ensuring the flight safety of the UAV. Attached Figure Description
[0032] Appendix Figure 1 This is a flowchart of the overall solution process of the system of this invention. (Attached) Figure 2 This is a flowchart of the improved Sage-Husa filtering process in this invention. Detailed Implementation
[0033] To make the technical problems, technical solutions, and beneficial effects of this invention clearer, the invention will be described in detail below with reference to the accompanying drawings and embodiments. It should be noted that the specific embodiments described herein are merely illustrative and not intended to limit the invention.
[0034] Figure 1 This is a flowchart of the overall solution process of the present invention. In this invention, the coordinate system is first defined as follows: the UAV carrier coordinate system is "front right lower", and the navigation coordinate system is "northeast ground". The solution method includes: Step 1: Real-time acquisition of high-precision inertial navigation system (INS), MEMS INS, and satellite navigation data carried on the UAV. During operation, the UAV can switch INS by injecting errors, such as adding over-measurement errors to the accelerometer or gyroscope outputs, or by causing a large sudden change in angular velocity. At this time, the satellite navigation information is valid, and the UAV navigation mode needs to be switched from the high-precision INS plus satellite combined navigation to the MEMS INS plus satellite combined navigation.
[0035] The second step involves the system adjusting the state estimation covariance matrix based on the MEMS inertial group's performance indicators upon receiving the switching signal. This covariance matrix is proactively set to 10 times the value used by the high-precision inertial group during normal operation. Increasing the value of the state estimation covariance matrix proactively adapts to the higher initial uncertainty of the MEMS inertial group during the switching process, preventing jumps in state estimation due to significant differences in the error characteristics of the two inertial groups and improving the balance during the switching process. This method of expanding the state estimation covariance reduces instability factors caused by initial state deviations. The third step is to activate the decaying memory Sage-Husa filter. The innovation, as a key indicator reflecting the difference between the actual measured values and the predicted values of the system, provides important data support for subsequent noise estimation and state updates, ensuring the consistency and validity of the data. Its calculation method is as follows:
[0037] In the formula, For the new interest, For observation purposes, For the observation matrix, This is the predicted value of the state.
[0038] When switching to a MEMS inertial navigation system, the innovation is calculated based on the MEMS inertial navigation system output data. The process noise covariance matrix Q and the measurement noise covariance matrix R are updated using the following iterative method, with the update formula as follows:
[0039]
[0040]
[0041]
[0042] In the formula, This is the forgetting factor, typically ranging from 0.95 to 0.99. The current sampling count is set to 1 after switching. To improve computational efficiency and optimize storage space, the process noise covariance matrix Q and the measurement noise covariance matrix R are updated using the following iterative method:
[0043]
[0044] The fourth step is to transfer the new information. The new information is compared with a preset threshold, and a strong tracking factor is calculated when the threshold is exceeded. :
[0045] pass The prediction covariance matrix is modified, and the Kalman gain is increased to suppress state bias until the innovation returns to normal.
[0046] The fifth step involves updating the state of P, Q, and R based on the attenuated memory Sage-Husa filter, fusing MEMS inertial navigation and satellite navigation data, and outputting navigation parameters such as position, velocity, and attitude.
[0047] The state equation of the system is:
[0048] in, Here is the state transition matrix. This is the noise gain matrix. This is process noise.
[0049] The observation equation is as follows:
[0050] in, For the velocity observation vector to satisfy the zero-velocity correction condition, For the observation matrix, To observe noise.
[0051] The Kalman filter iterates according to the formula, and the prediction steps are as follows:
[0052]
[0053] The update steps are as follows:
[0054]
[0055]
[0056] The above processing steps can ensure that the navigation results during the UAV inertial navigation system switching process are continuous and stable without obvious abrupt changes, providing reliable navigation information for the safe flight of the UAV.
[0057] The above processing method ensures continuous and stable navigation results during UAV inertial navigation system switching, without significant abrupt changes, providing reliable navigation information for the safe flight of the UAV. The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Any modifications, equivalent substitutions, and improvements made within the principles and spirit of the present invention are included within the scope of protection of the present invention.
Claims
1. A UAV inertial navigation system (INS) switching method based on improved Sage-Husa filtering, characterized in that, include: Step 101: Collect real-time acceleration and angular velocity data from the high-precision inertial group, perform filtering processing, compare with a preset threshold, and determine whether to switch to the MEMS inertial group. Step 102: After the switch, based on the error characteristics of the MEMS inertial group, set the state estimation covariance matrix to 5-10 times that of the high-precision inertial group when it is working normally. Step 103: Start the decay memory Sage-Husa filter, update the process noise variance matrix Q and the measurement noise variance matrix R in real time through recursive formulas, and realize state estimation by combining the Kalman filter process. Step 104: The innovation monitoring module monitors the innovation in real time, and introduces a strong tracking factor to correct the prediction covariance matrix when the innovation exceeds the threshold. Step 105: The navigation solution module completes the combined navigation solution based on the adjusted parameters and outputs a stable navigation result.
2. The UAV inertial navigation system switching method based on improved Sage-Husa filtering according to claim 1, characterized in that, The first step is to set the maximum range of data collected by the high-precision inertial group. If the output exceeds the maximum range, the system will switch directly to the MEMS inertial group. If the range is not exceeded, the raw three-axis angular velocity data will be filtered in real time through a Butterworth low-pass filter to remove high-frequency interference caused by the UAV’s own vibration and obtain smoothed three-axis angular velocity. The change in angular acceleration will be calculated to determine whether the inertial group needs to be switched.
3. The UAV inertial navigation system switching method based on improved Sage-Husa filtering according to claim 1, characterized in that, The second step involves the system receiving a switching signal and adjusting the state estimation covariance matrix P based on the MEMS inertial group's own performance indicators, actively setting it to 5-10 times the value required for normal high-precision inertial group operation. By increasing the value of the state estimation covariance matrix, the system can proactively adapt to the higher initial uncertainty of the MEMS inertial group at the moment of switching, thereby improving the balance during the switching process.
4. The UAV inertial navigation system switching method based on improved Sage-Husa filtering according to claim 1, characterized in that, The third step is to activate the decaying memory Sage-Husa filter. The innovation, as a key indicator reflecting the difference between the actual measured values and the predicted values of the system, provides important data support for subsequent noise estimation and state updates, ensuring the consistency and validity of the data. Its calculation method is as follows: ； In the formula, For the new interest, For observation purposes, For the observation matrix, For the predicted state, When switching to a MEMS inertial navigation system, the new information is calculated based on the MEMS inertial navigation system output data. The process noise covariance matrix Q and measurement noise covariance matrix R of the attenuated memory Sage-Husa filter are updated iteratively, with the update formula as follows: ； ； ； ； In the formula, This is the forgetting factor, typically ranging from 0.95 to 0.
99. The current sampling count is set to 1 after switching. To simplify the above process and improve computational efficiency, this invention uses the state variable from the previous time step to complete the iteration. The calculation formula is as follows: ； 。 5. The UAV inertial navigation system switching method based on improved Sage-Husa filtering according to claim 1, characterized in that, The fourth step requires the new information. The new information is compared with a preset threshold, and a strong tracking factor is calculated when the threshold is exceeded. : ； pass The prediction covariance matrix is modified, and the Kalman gain is increased to suppress state bias.
6. The UAV inertial navigation system switching method based on improved Sage-Husa filtering according to claim 1, characterized in that, In the fifth step, the navigation solution is updated based on the adjusted P, Q, R matrices. It then fuses MEMS inertial navigation and satellite navigation data to output navigation parameters such as position, velocity, and attitude. The state equation of the system is: ； in, Here is the state transition matrix. This is the noise gain matrix. For process noise, The observation equation is as follows: ； in, To observe the noise, The Kalman filter iterates according to the formula, and the prediction steps are as follows: ； ； The update steps are as follows: ； ； ； The above processing steps can ensure that the navigation results during the UAV inertial navigation system switching process are continuous and stable without obvious abrupt changes, providing reliable navigation information for the safe flight of the UAV.