Multi-sensor height fusion estimation method and system

Abstract

The invention discloses a multi-sensor height fusion estimation method and system, and relates to the technical field of unmanned aerial vehicle navigation and positioning. The method comprises the following steps: preprocessing original data of an IMU, a GNSS and a barometer; constructing a velocity reconstruction value by using the height derivative, and constructing a height reconstruction value by using the velocity integral; constructing short-time high-confidence reference data as a judgment basis by using IMU acceleration integration and barometer height difference, carrying out comparison sequence fault diagnosis on GNSS data, executing a hierarchical repair strategy, and outputting GNSS effective height; the GNSS effective height is used as a reference, the weight of a state estimation filter is adjusted according to the confidence level, and deviation estimation and correction are carried out on the barometer; and performing navigation calculation by combining IMU data and second-order kinematics extrapolation compensation. According to the invention, the problems of low height estimation precision, data discontinuity and lagging under the condition of low-cost hardware are effectively solved.

Description

Technical Field

[0001] This invention belongs to the field of UAV navigation and positioning technology, specifically relating to a multi-sensor height fusion estimation method and system. Background Technology

[0002] The navigation system of an unmanned aerial vehicle (UAV) provides the necessary status data for flight. Compared with other moving vehicles, UAVs have extremely high requirements for navigation and positioning in the altitude direction (Z-axis). Currently, the sensors involved in altitude holding in navigation systems mainly include inertial measurement units (IMU), global navigation satellite systems (GNSS), and barometers.

[0003] In existing technologies, each sensor has its limitations: although GNSS can provide absolute position, its vertical accuracy is poor due to the influence of satellite geometry, and it is easily affected by obstruction and multipath effects, leading to jumps or unreliable measurement values; IMU is based on Newton's second law for integration, which has high short-term accuracy and is not affected by external factors, but it has divergence errors that accumulate over time; barometers can provide barometric altitude, but they are easily affected by temperature and external airflow, and they have zero-bias drift that varies over time.

[0004] Traditional altitude estimation strategies typically employ a simple GNSS / IMU / barometer combination. To improve accuracy, some solutions utilize deep multi-sensor fusion, allowing GNSS and barometer errors to compensate for each other. However, this approach struggles to handle drastic sensor anomalies and is limited by inherent biases in the observation data. Another approach involves adding additional sensors such as lidar, millimeter-wave radar, or binocular vision. While this improves accuracy, it significantly increases the drone's payload, power consumption, and manufacturing cost, making it unsuitable for cost- and weight-sensitive micro-drones.

[0005] Furthermore, existing technologies typically employ simple threshold-based judgment and outlier removal methods when handling GNSS faults. This approach leads to a loss of absolute observation updates during data removal, relying solely on IMU integration, which can easily result in state divergence. Therefore, how to achieve high-precision, high-reliability, and highly integrated estimation by mining the potential correlations between various sensor data using algorithms under low-cost hardware configurations is a pressing challenge that needs to be addressed. Summary of the Invention

[0006] Purpose of the invention: The purpose of this invention is to address the shortcomings of existing technologies by providing a multi-sensor high-fusion estimation method and system. Through multi-observation source reconstruction, repair and correction mechanisms, it overcomes the problems of low accuracy, high hardware cost and insufficient reliability of existing high-fusion systems.

[0007] Technical solution: The multi-sensor high fusion estimation method of the present invention includes the following steps:

[0008] The raw observation data from IMU, GNSS, and barometer are acquired and preprocessed to obtain preprocessed IMU acceleration data, GNSS altitude data, GNSS velocity data, and barometer altitude data.

[0009] The preprocessed GNSS altitude data and GNSS velocity data are reconstructed from each other, including: constructing velocity reconstruction values ​​using the derivative of GNSS altitude data, and constructing altitude reconstruction values ​​using the integral of GNSS velocity data;

[0010] Using the preprocessed IMU acceleration and barometer altitude data, short-term high-confidence reference data for velocity and altitude are constructed as a decision basis to perform sequential fault diagnosis and repair on GNSS data, including: when the original observation value is diagnosed as abnormal based on the comparison results of the decision basis, the original observation value is reconstructed and repaired using the corresponding reconstructed value, and the effective GNSS altitude is output through adaptive switching according to the GNSS positioning mode;

[0011] A state estimation filter is constructed, and the effective GNSS altitude is used as the reference observation source. The barometer altitude data is then subjected to deviation estimation and correction to obtain the corrected barometer altitude.

[0012] The effective GNSS altitude or the corrected barometer altitude is adaptively selected as the fused observation value, and combined with the preprocessed IMU acceleration data, the navigation fusion algorithm is input for calculation, and the final fused altitude is output.

[0013] The present invention also provides a multi-sensor height fusion estimation system, including a data acquisition and preprocessing module, a GNSS data processing module, a barometer data correction module, and a height fusion estimation module, for performing the above-described method.

[0014] Beneficial effects: Compared with the prior art, the advantages of the present invention are as follows:

[0015] This invention utilizes the independent measurement principles of GNSS position and velocity, performing "mutual reconstruction" of data through calculus, and introducing an IMU and barometer as third-party references for "three-way comparison" judgment. This "logical AND" judgment condition (i.e., a fault is determined only when the original value deviates significantly from the reference and the reconstructed value deviates less from the reference) avoids the risk of misjudgment from a single threshold judgment, significantly improving the accuracy of fault detection.

[0016] Compared to the existing approach of simply removing outlier data, this invention employs a tiered repair strategy of "resetting and deducting net error" or "complementary filtering" when a fault is detected. This ensures that even when severe GNSS jumps occur, the system can still output smooth, continuous, and step-free observation data, avoiding filter oscillations or divergence.

[0017] This invention uses repaired GNSS data as a reference and dynamically adjusts the filter weights according to the confidence level (Case), enabling accurate estimation and elimination of barometer zero-bias drift. This allows the system to maintain high-precision altitude hold even when GNSS signal is poor, relying on the corrected barometer, without the need for expensive additional sensors.

[0018] This invention utilizes the least squares method to estimate observation delay in real time and combines it with IMU acceleration for second-order kinematic extrapolation compensation. Compared to linear extrapolation, this method can more accurately compensate for the position lag of the UAV during acceleration and deceleration, thus improving the response performance of dynamic flight control. Attached Figure Description

[0019] Figure 1 This is an overall flowchart of the multi-sensor high fusion estimation method in this invention;

[0020] Figure 2 This is a block diagram of the overall structure of the multi-sensor highly fused estimation system in this invention;

[0021] Figure 3 This is a detailed logical diagram illustrating the GNSS data reconstruction and repair and barometer correction in this invention;

[0022] Figure 4 This is a schematic diagram of the navigation data estimation and dynamic delay compensation process in this invention. Detailed Implementation

[0023] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings, but the scope of protection of the present invention is not limited to the embodiments described.

[0024] Example 1: As Figure 1 The present invention provides a multi-sensor high fusion estimation method, which includes the following steps.

[0025] Step 1: Data Acquisition and Preprocessing

[0026] Acquire observation data such as acceleration measured by IMU, Doppler velocity and observation altitude measured by GNSS, and barometer altitude measured by barometer, and perform preprocessing and downsampling. The specific process is as follows: (1) Perform zero bias error compensation and scale factor error compensation on IMU data based on preset parameters; (2) Perform real-time temperature compensation on IMU and barometer according to sensor temperature characteristic curve; (3) Align the sensor coordinate system with the flight control coordinate system; (4) Perform downsampling using the equal interval averaging method to obtain the processing results; (5) Output the processed acceleration, GNSS altitude, GNSS velocity and barometer altitude.

[0027] Step Two: GNSS Data Reconstruction, Diagnosis and Repair

[0028] The GNSS data processing module is one of the core components of this invention, and its purpose is to obtain a continuous and reliable "GNSS effective altitude". Specifically, GNSS data reconstruction refers to obtaining velocity from GNSS altitude observations through derivatives, and obtaining altitude from GNSS velocity observations through integration. This process of differentiation and integration is called reconstruction. Repair is equivalent to resetting. Taking altitude as an example, using GNSS altitude observations as the main line, if it is determined that the observed altitude is abnormal, the GNSS altitude is reset to the GNSS velocity integral (reconstructed observation value), and the net difference is recorded. Subsequent altitude observations are used as altitude observation values ​​minus the net difference. This process is called repair.

[0029] It should be noted that the Global Navigation Satellite System (GNSS) described in this invention includes, but is not limited to, the Global Positioning System (GPS), the BeiDou Navigation Satellite System (BDS), the GLONASS system, or the Galileo system. As a preferred embodiment, this system may use GPS as a specific GNSS observation source. Figure 4 As shown, when GPS is used as the specific application scenario, the GNSS speed repair and GNSS altitude repair are specifically manifested as GPS speed repair and GPS altitude repair as shown in the figure, and the specific process is as follows:

[0030] (1) Constructing short-term high-confidence judgment criteria:

[0031] Reference velocity construction: Time integration is performed using the pre-processed IMU acceleration, that is, the sum of the acceleration integral at the previous time step and the filtered acceleration multiplied by the interval time (acceleration integral at the previous time step + filtered acceleration * interval time). The IMU has high short-time accuracy, thus constructing a short-time high-confidence "reference velocity".

[0032] Construction of reference altitude change: The reference altitude change is constructed using the difference value (i.e., altitude increment) of barometer altitude data to obtain a short-term high-confidence "reference altitude change" as a reference for judging the trend of altitude change.

[0033] (2) Reconstruction and Diagnosis of the Speed ​​Dimension:

[0034] Velocity reconstruction: The time derivative of the original GNSS altitude observation is obtained (e.g., using the central difference method: altitude increment / time increment), and the "velocity reconstruction value" is calculated.

[0035] Fault Diagnosis: Based on the raw GNSS observation velocity, the "GNSS average velocity" is obtained. The GNSS average velocity and the reconstructed velocity value are compared with the reference velocity. The judgment logic is as follows: when the difference between the GNSS average velocity and the reference velocity exceeds a first preset threshold (indicating that the raw value is questionable), and the difference between the reconstructed velocity value and the reference velocity is less than a second preset threshold (indicating that the reconstructed value is reliable), the raw GNSS velocity is determined to be faulty. The data characteristics of the GNSS velocity are used as an auxiliary judgment to determine the degree to which it deviates from the velocity source with the highest confidence level.

[0036] Data Repair: If a speed fault is determined, dynamically reset the original GNSS speed using the "speed reconstruction value" and output the "repaired GNSS speed" (in...). Figure 4 In the preferred embodiment, the corresponding output is "GPS speed repair"; otherwise, the original speed value remains unchanged.

[0037] (3) High-dimensional reconstruction and diagnostic repair:

[0038] Height Reconstruction: Based on the "restored GNSS velocity" output in step two, perform an integral operation (velocity integral at the previous time step + (velocity * interval time)) to obtain the velocity integral increment (i.e., the height reconstruction value increment). Calculate the difference between the original GNSS height observations to obtain the "height increment".

[0039] Fault Diagnosis: The focus is on analyzing whether the altitude increment of the observed values ​​is reasonable. The original GNSS altitude increment and the altitude reconstruction increment are compared with the reference altitude change (barometer trend), including:

[0040] Judgment condition: If the deviation between the "altitude increment" and the "reference altitude change" is less than the third preset threshold, it means that the trend of the original GNSS observation altitude change is reasonable and there is no jump. Action: Determine that the GNSS observation altitude is fault-free and directly use the original GNSS observation altitude as the effective altitude at the current moment.

[0041] Judgment criteria: When the altitude increment deviation is greater than the third preset threshold, and the deviation between the altitude integral increment and the reference altitude change is less than the fourth preset threshold, a serious fault in the original GNSS altitude is determined, indicating that a serious jump has occurred in the GNSS altitude, while the velocity integral path is correct. Actions performed (reset and net difference recording): The system's current GNSS altitude state is reset to the GNSS velocity integral value; the difference between the original altitude and the reset value at the current moment is calculated and recorded as the "net difference"; in subsequent moments, this is output, thus maintaining the continuity of the data stream while eliminating jumps.

[0042] Judgment condition: When neither of the above two conditions is met or the deviation is within the intermediate range. Action to perform (smooth update): Use a complementary filtering algorithm to fuse the reconstructed height value with the original GNSS height, generating a smoothed height as the "repaired GNSS height" (in...). Figure 4 In the preferred embodiment, the corresponding output is "GPS altitude repair".

[0043] (4) Adaptive output: The system determines the current GNSS positioning mode and the above diagnostic results in real time.

[0044] The system determines the current GNSS positioning mode in real time. If it is in RTK mode, the system prioritizes outputting the original GNSS altitude as the "effective GNSS altitude". If it is in non-RTK mode or a fault is detected, the system selects the "repaired GNSS altitude" obtained through the above repair process as the "effective GNSS altitude" for use by the subsequent barometer correction and navigation fusion modules.

[0045] Step 3: Barometer Deviation Estimation and Correction

[0046] The barometer data correction module uses the effective GNSS altitude after the "cleaning" in step two as a reference, and estimates the barometer's bias in real time using a Kalman filter algorithm, thereby eliminating its drift error. The specific process is as follows:

[0047] (1) Filter construction: Establish a state estimation filter (e.g., Kalman filter) and use the barometer deviation (Bias) as the system state variable to be estimated.

[0048] (2) Dynamic confidence assessment: Based on the current GNSS data positioning mode, observation noise covariance, and fault diagnosis results in step two, the current calibration scenario is divided into different confidence levels. For example:

[0049] Example strategy: If the GNSS is in RTK mode and the data consistency test passes, the confidence level is determined to be the highest, and a small value is assigned to the R matrix (strong observation update), the filter converges quickly, and the barometer bias closely follows the GNSS reference for calibration; if the GNSS is in non-RTK mode and the data exceeds the limit, the confidence level is determined to be the lowest, and a maximum value is assigned to the R matrix (weak observation update or no update).

[0050] (3) Deviation update: Adjust the filter using the set R matrix parameters, and update the estimated result of the barometer deviation value using the “GNSS effective height” output in step two as the observation.

[0051] (4) Correction output: Subtract the Bias value from the original height of the barometer after preprocessing, and output "corrected barometer height".

[0052] Step 4: Navigation Fusion and Delay Compensation

[0053] The altitude fusion estimation module is responsible for fusing IMU observations with the selected altitude sources, compensating for system latency, and generating the final navigation result. The specific process is as follows:

[0054] (1) Adaptive optimization of observation source: The system dynamically selects the observation input of the fusion unit according to the GNSS positioning mode.

[0055] If the GNSS is in high-precision mode (such as RTK Fixed), the "GNSS effective altitude" is selected as the observation value. If the GNSS is in normal mode (non-RTK), the "corrected barometer altitude" is selected as the observation value to take advantage of its smooth and drift-free characteristics (corrected in step three).

[0056] (2) Error State Kalman Filter (ESKF) Solution: The ESKF algorithm is used to generate the integrated navigation results.

[0057] Prediction Step: State recursion (prediction) is performed using high-frequency acceleration data from the IMU to establish a priori estimate of the system. Update Step: A mathematical model is established between the error state (including altitude error, IMU zero bias, etc.) and the observed values. Using the observed values ​​selected in Step 1, the posterior estimate of the error state is calculated in conjunction with the Kalman gain. Correction Step: The posterior estimate of the error state is fed back to the nominal state to complete the correction of the system state and generate the integrated navigation solution.

[0058] (3) Dynamic time delay estimation: Considering the time lag introduced by sensor data transmission and algorithm processing, the module performs online delay estimation. Specifically, based on the time matching residual between the fused observations and the system's predicted state, the least squares method is used to construct an optimization objective function (minimizing the sum of squared residuals) to calculate the system's time delay error in real time.

[0059] (4) Real-time kinematic extrapolation compensation: Based on the estimated delay error, the high-frequency measurement data (acceleration) of the IMU is used to perform second-order kinematic extrapolation on the lagging navigation solution. This step compensates for the position lag caused by the time difference, and finally generates the fused altitude at the current moment (real-time), which is then published to the flight control system for closed-loop control.

[0060] Example 2: As Figure 2 , Figure 3 As shown, this embodiment provides a multi-sensor highly fusion estimation system. Logically, the system is divided into four collaborative subsystems, each implementing data acquisition, cleaning, reconstruction, correction, and fusion through modular design.

[0061] 1. Data Acquisition and Preprocessing Subsystem: This subsystem is the input front end of the entire system and is mainly responsible for acquiring raw physical quantities and performing standardization processing.

[0062] Sensor module: includes IMU (provides acceleration and angular velocity), GNSS receiver (provides observation altitude and Doppler velocity) and barometer (provides barometric altitude).

[0063] The preprocessing module performs the following processing: Temperature compensation: receiving raw data from the IMU and barometer, and compensating for temperature drift by combining it with pre-stored temperature characteristic curves; Coordinate alignment: converting the GNSS azimuth velocity / altitude to the NED coordinate system, and converting the IMU body axis acceleration to the navigation coordinate system; Downsampling: averaging the high-frequency IMU data at equal intervals to align its frequency with the GNSS / barometer data (e.g., reducing it from 1kHz to 100Hz).

[0064] Observation data output module: uniformly outputs the preprocessed "GNSS altitude and velocity", "barometer altitude" and "acceleration" to subsequent subsystems.

[0065] 2. GNSS Data Processing Subsystem (Core Processing Unit): This subsystem is responsible for performing mutual reconstruction and cascading repair of data, and outputting a single, reliable "GNSS effective altitude".

[0066] The data reconstruction module includes: velocity observation reconstruction: receiving the original GNSS altitude and generating a "velocity reconstruction value" through central difference differentiation; altitude observation reconstruction: receiving the "restored GNSS velocity" and generating a "altitude reconstruction value" through numerical integration.

[0067] The auxiliary judgment module (judgment data generation) includes: reference velocity construction: integrating the IMU acceleration to generate a short-term high-confidence reference velocity; reference altitude construction: differentiating the barometer altitude to generate a short-term high-confidence reference altitude change.

[0068] The fault diagnosis and repair module includes: multi-source fault diagnosis (three-way comparison), where the velocity layer compares the "original GNSS velocity," "reconstructed velocity value," and "reference velocity." If the original velocity deviates significantly from the reference velocity, while the reconstructed velocity deviates less, a velocity fault is determined. The altitude layer compares the "original GNSS altitude increment," "reconstructed altitude increment," and "reference altitude change." If the original increment deviates significantly, while the reconstructed increment deviates less, an altitude fault is determined. Data repair includes: velocity repair uses the "reconstructed velocity value" to reset the abnormal original GNSS velocity; altitude repair (net difference preservation) uses the "reconstructed altitude value" to reset the abnormal GNSS altitude state and maintains a "net difference" variable to ensure continuous output.

[0069] Adaptive switching module: Receives GNSS positioning status flags (such as RTK Fix) as well as GNSS observation altitude and GNSS repair altitude; Data switching logic: If it is in RTK mode and there is no fault, select "GNSS observation altitude"; otherwise, select "GNSS repair altitude" processed by the repair module, and finally output a single "GNSS effective altitude" signal.

[0070] 3. Barometer data correction subsystem: This subsystem uses reliable GNSS data to dynamically calibrate the barometer.

[0071] Repair the data source module: Receive "barometer observations" and "GNSS effective altitude" from the GNSS data processing subsystem.

[0072] The data evaluation module includes: data source quality inspection, used to analyze the noise level (such as variance) of GNSS signals; and dynamic confidence analysis, which calculates the current observation weight (confidence) based on the empirical reliability of the barometer.

[0073] The data correction module includes: a Kalman filter, which uses barometer bias as the state variable. It updates the bias estimate using the calculated observation weights and the "GNSS effective altitude" as the true value; and barometer correction, which performs subtraction operations and outputs the "corrected barometer data".

[0074] 4. Highly fused estimation subsystem, which is responsible for generating the final navigation state.

[0075] Adaptive switching judgment: Check the GNSS status again. If the accuracy meets the requirements (high accuracy mode), the fused observation value will be pointed to "GNSS effective altitude"; otherwise, it will be pointed to "corrected barometer data".

[0076] Fusion module: Runs navigation fusion algorithms (such as ESKF) for: state updates, using IMU data for state recursion (prediction); observation updates, using selected fused observations to correct the state.

[0077] The time-domain delay compensation module includes: delay compensation, which caches historical data, fits the observation residual sequence using the least squares method, and calculates the time delay; IMU state extrapolation, which uses the current IMU acceleration and the estimated time delay error to perform second-order kinematic extrapolation on the lagging fusion result; and altitude data publishing, which sends the final "fused altitude" to the flight control system for altitude hold flight. This embodiment has a clear system structure, with decoupled and progressively layered modules, effectively supporting the system architecture limitations in the claims and ensuring the feasibility of the technical solution.

[0078] As described above, although the invention has been shown and described with reference to specific preferred embodiments, it should not be construed as limiting the invention itself. Various changes in form and detail may be made without departing from the spirit and scope of the invention as defined in the appended claims.

Claims

1. A multi-sensor high-fusion estimation method, characterized in that, Includes the following steps: The raw observation data from IMU, GNSS, and barometer are acquired and preprocessed to obtain preprocessed IMU acceleration data, GNSS altitude data, GNSS velocity data, and barometer altitude data. The preprocessed GNSS altitude data and GNSS velocity data are reconstructed from each other, including: constructing velocity reconstruction values ​​using the derivative of GNSS altitude data, and constructing altitude reconstruction values ​​using the integral of GNSS velocity data; Using the preprocessed IMU acceleration and barometer altitude data, short-term high-confidence reference data for velocity and altitude are constructed as a decision basis to perform sequential fault diagnosis and repair on GNSS data, including: when the original observation value is diagnosed as abnormal based on the comparison results of the decision basis, the original observation value is reconstructed and repaired using the corresponding reconstructed value, and the effective GNSS altitude is output through adaptive switching according to the GNSS positioning mode; A state estimation filter is constructed, and the effective GNSS altitude is used as the reference observation source. The barometer altitude data is then subjected to deviation estimation and correction to obtain the corrected barometer altitude. The effective GNSS altitude or the corrected barometer altitude is adaptively selected as the fused observation value, and combined with the preprocessed IMU acceleration data, the navigation fusion algorithm is input for calculation, and the final fused altitude is output.

2. The multi-sensor high fusion estimation method according to claim 1, characterized in that, Preprocessing of raw observation data includes: Based on preset parameters, zero bias error compensation and scale factor error compensation are performed on the raw IMU observation data; Real-time temperature compensation is performed on the IMU and barometer based on the sensor temperature characteristic curves. All sensor coordinate systems are aligned with the flight control coordinate system, and downsampling is performed using an equal-interval averaging method. The processed acceleration data, GNSS altitude data, GNSS velocity data, and barometer altitude data are then output.

3. The multi-sensor high fusion estimation method according to claim 2, characterized in that, The process of constructing short-time high-confidence reference data for velocity and altitude using the preprocessed IMU acceleration and barometer altitude data includes: A short-time reference velocity is constructed by integrating the preprocessed IMU acceleration over time. Using the difference values ​​of the barometer altitude data, a short-term reference altitude change is constructed.

4. The multi-sensor high fusion estimation method according to claim 3, characterized in that, The sequential fault diagnosis and repair of GNSS data is performed according to the following logic: Velocity reconstruction and restoration: constructing velocity reconstruction values ​​using the time derivative of GNSS altitude data; obtaining the average velocity using the raw GNSS velocity from the original GNSS observation data; Using the short-time reference velocity as a judgment benchmark, the average velocity and the velocity reconstruction value are compared with the short-time reference velocity. When the deviation between the average velocity and the reference velocity is greater than a first preset threshold and the deviation between the velocity reconstruction value and the short-time reference velocity is less than a second preset threshold, it is determined that there is a fault in the original GNSS velocity. The original GNSS velocity is then reset using the velocity reconstruction value to obtain the repaired GNSS velocity. Altitude reconstruction and repair: The altitude reconstruction value increment is constructed by time integration using the repaired GNSS velocity; Calculate the altitude increment using the raw GNSS altitude from the raw GNSS observation data; Using the short-term reference altitude change as a decision criterion, the altitude increment and the altitude integral increment are compared with the short-term reference altitude change; based on the comparison results, a graded repair strategy is executed to obtain the repaired GNSS altitude.

5. The multi-sensor high fusion estimation method according to claim 4, characterized in that, The step of implementing a graded repair strategy based on the comparison results includes: When the deviation between the altitude increment and the short-term reference altitude change is less than the third preset threshold, the original GNSS altitude is determined to be normal, and the original GNSS altitude is directly output as the repaired GNSS altitude. When the deviation between the altitude increment and the short-term reference altitude change is greater than a third preset threshold, and the deviation between the altitude integral increment and the short-term reference altitude change is less than a fourth preset threshold, a serious fault is determined in the original GNSS altitude. The current GNSS altitude status of the system is reset to the reconstructed value corresponding to the altitude integral increment, and the difference between the original altitude and the reconstructed value is recorded as the net difference. In subsequent observation cycles, the original GNSS altitude after subtracting the net difference is continuously output as the repaired GNSS altitude until the next fault reset or system reset. When neither of the above two conditions is met or the deviation is within the middle range, a complementary filtering algorithm is used to fuse the reconstructed value corresponding to the height integral increment with the original GNSS height to generate a smoothed height as the repaired GNSS height.

6. The multi-sensor high fusion estimation method according to claim 1, characterized in that, The adaptive switching of outputting the effective GNSS altitude includes: Real-time determination of the current GNSS positioning mode; If the GNSS is in RTK mode, then the original GNSS altitude is selected as the effective GNSS altitude. If the GNSS is in non-RTK mode, the corrected GNSS altitude obtained through the sequential fault diagnosis and repair is selected as the effective GNSS altitude.

7. The multi-sensor high fusion estimation method according to claim 1, characterized in that, The deviation estimation and correction of the barometer altitude data includes: In the state estimation filter, the barometer deviation value is set as the state variable to be estimated; Based on the current GNSS data positioning mode, observation noise covariance, and the results of the fault diagnosis, the current calibration scenario is divided into different confidence levels; For each confidence level, a corresponding observation noise covariance matrix parameter is set, and the input weight of the state estimation filter is adjusted using this parameter. The effective GNSS altitude is used as the estimation result of the observation update barometer deviation value. The corrected barometer height is obtained by subtracting the estimated barometer deviation value from the preprocessed barometer height data.

8. The multi-sensor high fusion estimation method according to claim 1, characterized in that, The navigation fusion algorithm further includes a dynamic delay compensation step, comprising: Collect the fused observation sequence and the system predicted state sequence from the past N frames; The residuals between the fused observation sequence and the system predicted state sequence are fitted using the least squares method, and the time delay error between the two is estimated in real time. Based on the time delay error, the kinematic extrapolation compensation is performed on the lag calculation results output by the navigation fusion algorithm using the preprocessed IMU acceleration to generate the fusion height at the current moment.

9. A multi-sensor high-fusion estimation system for implementing the method as described in any one of claims 1 to 8, characterized in that, The system includes: The data acquisition and preprocessing module is configured to acquire and preprocess raw observation data from IMU, GNSS, and barometer. The GNSS data processing module is configured to perform mutual reconstruction and repair of data, including: constructing velocity reconstruction values ​​using the derivative of GNSS altitude, and constructing altitude reconstruction values ​​using the integral of GNSS velocity; constructing short-time high-confidence reference data using preprocessed IMU acceleration and barometer altitude data, and using three-way comparison logic as the basis for judgment; when anomalies are diagnosed in the original observation values, resetting and repairing the original observation values ​​using the corresponding reconstruction values, and performing graded repair strategies including direct output, resetting and deducting net error or complementary filtering according to the degree of deviation, and adaptively switching the output of the effective GNSS altitude of a single signal stream according to the GNSS positioning mode; The barometer data correction module is configured to construct a state estimation filter, use the effective GNSS altitude as a reference observation source, perform deviation estimation and correction on the barometer altitude data, and obtain the corrected barometer altitude. The height fusion estimation module is configured to adaptively select the effective GNSS height or the corrected barometer height as the fusion observation value, combine it with the preprocessed IMU acceleration input navigation fusion algorithm for calculation, and output the final fusion height.

Images