A high-precision positioning method based on multimodal fusion filtering

Abstract

A high-precision positioning method based on multimodal fusion filtering, relating to the field of intelligent positioning technology, includes the following steps: acquiring sensor data collected in real time by a GNSS submodule, an IMU submodule, and a barometer submodule, and performing collaborative preprocessing on the sensor data; performing state estimation through an extended Kalman filter (EKF) and an adaptive particle filter (APF) to obtain the corresponding EKF positioning output and APF positioning output; calculating a multi-dimensional positioning reliability reflecting the geometric accuracy of the GNSS signal, the stability of the IMU data, and the high consistency between the barometer and GNSS; dynamically allocating fusion weights for the EKF positioning output, APF positioning output, and historical positioning data; and performing weighted fusion of the EKF positioning output, APF positioning output, and historical positioning data to output the final positioning result. This invention, through a three-layer architecture of collaborative data preprocessing, intelligent filtering, and dynamic decision-making, systematically improves the accuracy, continuity, and overall robustness of positioning in complex environments.

Description

Technical Field

[0001] This invention relates to the field of intelligent positioning technology, and in particular to a high-precision positioning method based on multimodal fusion filtering. Background Technology

[0002] High-precision positioning technology is a core foundation for fields such as autonomous driving, drone navigation, and precision surveying. Currently, multi-sensor fusion schemes based on Global Navigation Satellite Systems (GNSS) and Inertial Measurement Units (IMUs) have become the mainstream technical approach for achieving continuous and reliable positioning. However, when facing complex dynamic scenarios such as urban canyons, underground tunnels, and high-speed maneuvers, existing technical solutions still have significant shortcomings at the core algorithm level, resulting in positioning accuracy, robustness, and environmental adaptability that are difficult to meet the requirements of high-end applications. These shortcomings are mainly reflected in the following three aspects: 1. State estimation algorithms suffer from poor adaptability, struggling to balance linear accuracy with nonlinear robustness. Existing methods largely rely on a single Extended Kalman Filter (EKF) or its fixed-parameter variants. While the EKF is efficient under favorable signal conditions and smooth motion, its inherent linearization approximation leads to model mismatch in highly nonlinear scenarios such as intermittent GNSS signals and severe vehicle maneuvers, easily causing estimation errors to expand or even filter divergence. Although some improvements attempt to introduce particle filtering (PF) or multi-model switching mechanisms, their key parameters (such as particle count and switching threshold) are usually statically preset, unable to adaptively adjust based on real-time satellite signal quality and vehicle motion state, resulting in insufficient generalization ability and stability of the algorithm in complex environments.

[0003] 2. The multi-source information fusion decision-making mechanism is rigid and lacks dynamic weight allocation capabilities. When fusing multi-source positioning information such as GNSS and IMU, most existing methods adopt fixed-weight fusion or a simple switching strategy based on the "presence" or "absence" of GNSS signals. These static strategies cannot quantitatively assess the instantaneous reliability of each information source in real time, such as the geometric distribution strength of GNSS signals, the zero-drift stability of IMU data, and the consistency of data among multiple sensors. Therefore, when the environment changes gradually or abruptly, the system cannot intelligently and smoothly adjust the fusion weights, resulting in jumps, lags, or continuous deviations in positioning output, seriously impairing the continuity and reliability of positioning.

[0004] 3. Insufficient collaboration and in-depth utilization of multi-sensor data, failing to fully realize the potential of data fusion. Current methods typically perform independent preprocessing and subsequent merging of data streams from sensors such as GNSS and IMU, lacking a pre-emptive, cross-sensor collaborative calibration and consistency verification mechanism. Particularly for sensors like barometers that provide independent altitude observations, their data value is not fully exploited, often resulting in a weakness in system vertical accuracy. This "data silo" approach means that the data input to the core algorithm may contain uncorrected system errors or inherent contradictions, fundamentally limiting the potential for improving overall accuracy.

[0005] In summary, existing high-precision positioning methods suffer from systemic deficiencies in three key areas: adaptive state estimation, intelligent fusion decision-making, and deep data collaboration. Therefore, there is an urgent need in this field for a novel integrated positioning method capable of online adaptive optimization of model parameters, dynamic intelligent adjustment of fusion weights, and deep fusion preprocessing of multiple sensors, in order to overcome existing performance bottlenecks. Summary of the Invention

[0006] This invention provides a high-precision positioning method based on multimodal fusion filtering, the main purpose of which is to solve the problems of poor algorithm adaptability, rigid fusion strategy and insufficient sensor coordination in existing positioning methods.

[0007] The present invention adopts the following technical solution: A high-precision positioning method based on multimodal fusion filtering includes the following steps: Step S1: Acquire sensor data collected in real time by the GNSS submodule, IMU submodule and barometer submodule, and perform collaborative preprocessing on the sensor data; Step S2: Based on the preprocessed sensor data, state estimation is performed using Extended Kalman Filter (EKF) and Adaptive Particle Filter (APF) respectively to obtain the corresponding EKF positioning output and APF positioning output; Step S3: Based on the preprocessed sensor data, calculate the multi-dimensional positioning reliability that reflects the geometric accuracy of the GNSS signal, the stability of the IMU data, and the high consistency between the barometer and the GNSS. Step S4: Based on the multi-dimensional positioning confidence, dynamically allocate the fusion weights of the EKF positioning output, the APF positioning output, and the historical positioning data; Step S5: Based on the fusion weight, perform weighted fusion on the EKF positioning output, the APF positioning output, and the historical positioning data, and output the final positioning result.

[0008] Furthermore, the collaborative preprocessing method in step S1 includes the following steps: Step S11: Perform outlier removal and sliding window smoothing on the GNSS data based on the RANSAC algorithm; Step S12: Perform dynamic zero-bias calibration based on static time period detection and installation error compensation based on fixed installation deviation angle on the IMU data; Step S13: Perform temperature compensation coefficient-based processing on the barometer data. Altitude compensation, and calculate the barometer height after compensation. With GNSS altitude The difference .

[0009] Furthermore, in step S2, the Extended Kalman Filter (EKF) is based on a 15-dimensional state vector. It is used for online estimation of position, velocity, attitude angle, and sensor zero bias, and is defined as follows: in: These represent the directions of east, north, and sky. These are the velocities in the corresponding directions; These are roll angle, pitch angle, and yaw angle, respectively. These are the accelerometer zero bias values; These are the zero bias of the gyroscope.

[0010] Furthermore, in step S2, the observation noise covariance of the extended Kalman filter (EKF) The adjustment formula is based on the real-time average signal-to-noise ratio of the GNSS signal and the dynamic adjustment of the satellite elevation angle: in: The baseline covariance; These are correction coefficients based on the mean signal-to-noise ratio. This is a correction factor based on the satellite elevation angle.

[0011] Furthermore, in step S2, the total number N of particles in the adaptive particle filter (APF) is dynamically determined based on the GNSS satellite visibility count (sat_count), according to the following rules: When sat_count ≥ 6, N = 50; When 4 ≤ sat_count < 6, N = 120; When sat_count < 4, N = 200.

[0012] Furthermore, step S3 includes the following sub-steps: Step S31: Calculate the GNSS geometric accuracy reliability based on the GNSS signal's position accuracy attenuation factor PDOP. ; Step S32: IMU-based gyroscope zero bias mean Calculate IMU stability reliability ; Step S33: Based on the altitude difference between the barometer and GNSS Calculate sensor consistency reliability ; Step S34: Assess GNSS geometric accuracy reliability IMU stability reliability Consistency reliability with sensors By performing a weighted summation, the multi-dimensional location confidence is obtained: in: The predetermined weighting coefficients, and satisfying .

[0013] Furthermore, in step S2, the extended Kalman filter (EKF) includes a divergence detection mechanism. When divergence is detected, the adaptive particle filter (APF) is triggered to perform strong auxiliary correction, and the APF is used to locate and correct or reset the state estimate of the EKF.

[0014] Furthermore, in step S4, the dynamic allocation of fusion weights takes the comprehensive positioning confidence and GNSS satellite visibility count (sat_count) as inputs, and outputs the fusion weights corresponding to the EKF positioning output, APF positioning output, and historical positioning data. The weighting rules are as follows: When sat_count ≥ 6 and Confidence ≥ 0.8, =0.8, =0.1, 0.1; When sat_count < 4 or Confidence < 0.3, =0.1, =0.6, 0.3.

[0015] Furthermore, in steps S4 and S5, the historical positioning data is generated by weighted smoothing of the final positioning results of the first two frames, and its calculation formula is as follows: in: Historical location data; and QUOTE and The final positioning result at any given moment.

[0016] Furthermore, in step S5, the final positioning result is output after standardized encoding and then output through the communication bus; the standardized encoding method is to amplify the floating-point position component by 10. 7 After being multiplied, the data is converted to int64_T type. The floating-point attitude angle component is amplified by 100 times and then converted to int16_T type. The communication bus includes a first CAN bus and a second CAN bus. The first CAN bus transmits the encoded position, velocity and attitude angle data at a frequency of 50Hz, and the second CAN bus transmits the multi-dimensional positioning confidence data at a frequency of 10Hz.

[0017] Compared with the prior art, the beneficial effects of the present invention are as follows: This invention systematically improves the accuracy, continuity, and overall robustness of positioning methods in complex environments through a three-layer architecture of collaborative preprocessing, intelligent filtering, and dynamic decision-making. First, by employing an improved parallel and collaborative architecture of extended Kalman filtering and adaptive particle filtering, it addresses the issues of single-filter models easily diverging in nonlinear scenarios and the weak generalization ability of fixed-parameter algorithms. Second, by introducing and calculating multi-dimensional positioning confidence and dynamically allocating fusion weights based on this, it solves the problems of discontinuous and unstable positioning output caused by fixed weights or simple switching strategies. Finally, by performing collaborative preprocessing and deep fusion of GNSS, IMU, and barometer data, it addresses the problems of independent processing of multi-sensor data, insufficient value mining, and especially poor vertical accuracy and data consistency, thereby improving positioning performance at the system level. Attached Figure Description

[0018] Figure 1 This is a hardware block diagram of the positioning terminal of the present invention.

[0019] Figure 2 This is a flowchart of the core algorithm of the present invention.

[0020] Figure 3 This is a schematic diagram of the algorithm flow of the present invention.

[0021] Figure 4 This is a flowchart of the Extended Kalman Filter (EKF) algorithm of the present invention.

[0022] Figure 5 This is a flowchart of the adaptive particle filter (APF) algorithm of the present invention. Detailed Implementation

[0023] Specific embodiments of the present invention will now be described with reference to the accompanying drawings. Many details are described below to provide a comprehensive understanding of the invention; however, those skilled in the art will be able to implement the invention without these details.

[0024] like Figure 1As shown, this embodiment provides a high-precision positioning terminal based on multimodal fusion filtering. From the perspective of system functional architecture, the terminal can be divided into a data input layer, a core control layer, a data storage layer, a data output layer, and a control layer. Correspondingly, from the perspective of hardware device composition, the terminal specifically includes a multi-sensor module, a core processing module, a storage module, a communication module, and a low-power control module. The structure and function of each module are described in detail below: 1. Multi-sensor module: corresponding to Figure 1 The data input layer, the system's data source, consists of three complementary sub-modules with different data characteristics, designed to provide redundant and multi-dimensional perception of the physical world. The three sub-modules include: (1) GNSS submodule: used to receive signals from the Global Navigation Satellite System and provide an absolute spatiotemporal reference. Preferably, the GNSS submodule can use the UBLOX ZED-F9P chip, which supports multiple systems such as GPS, BDS, and GLONASS. It is connected to the core processing module through a UART interface (such as a baud rate of 921600bps) and outputs NMEA format data at a frequency of 10Hz. The data includes at least: latitude and longitude coordinates, altitude, ground velocity vector, and key quality indicators such as the number of visible satellites (sat_count), signal-to-noise ratio (SNR), and position accuracy attenuation factor (PDOP).

[0025] (2) IMU Submodule: This is the inertial measurement unit used to sense the motion of the carrier itself. Preferably, a QMI8658 chip can be used, which is a 6-axis sensor (3-axis accelerometer ±16g + 3-axis gyroscope ±2000dps). It is connected to the core processing module via an SPI interface (e.g., 10MHz) to output raw specific force (acceleration) and angular velocity data in the carrier coordinate system at a high frequency of 100Hz. This module is the core for maintaining navigation continuity when GNSS signals fail.

[0026] (3) Barometer Submodule: Used to measure ambient atmospheric pressure, providing a GNSS-independent observation source in the vertical direction. Preferably, an MS5611 chip (resolution 0.012 mbar) can be used. It connects to the core processing module via an I2C interface (e.g., slave address 0x77) and outputs pressure and temperature data at a frequency of 50 Hz. Altitude information can be calculated using the pressure-altitude formula, mainly used to constrain and correct the vertical positioning error of the system.

[0027] 2. Core processing module: corresponding to Figure 1 The core control layer, which is the "brain" of the system, is responsible for running the core multimodal fusion filtering algorithm in this embodiment, and executing sensor data synchronization, preprocessing, filtering calculation, fusion decision and control logic.

[0028] like Figure 2 As shown, in terms of software architecture, the multimodal fusion filtering algorithm specifically includes the following four layers working in concert: (1) Preprocessing layer: used to clean and calibrate the raw sensor data, and output smoothed GNSS data, calibrated IMU data and compensated barometer altitude data.

[0029] (2) Basic filtering layer: The preprocessed data is processed based on the improved extended Kalman filter (EKF). The improved EKF includes a 15-dimensional state vector and dynamic observation covariance adjustment logic.

[0030] (3) Enhanced filtering layer: The output of the basic filtering layer is optimized based on the adaptive particle filter (APF). The adaptive APF includes dynamic adjustment of particle number and Gaussian mixture model (GMM) weight update logic.

[0031] (4) Fusion decision layer: Based on the multi-dimensional location reliability calculation results, dynamically allocate the fusion weights of EKF output, APF output and historical trajectory, and output the final location data.

[0032] In terms of hardware implementation, this module preferably uses a high-performance microcontroller based on the ARM Cortex-M7 core, such as the STM32H743VIT6 (480MHz clock speed, built-in FPU). It is responsible for connecting and driving various sensors, and the specific pin assignment can be configured as follows: UART3 connects to the GNSS submodule, SPI1 connects to the IMU submodule, and I2C2 connects to the barometer submodule.

[0033] 3. Storage module: corresponding to Figure 1 The data storage layer is used for hierarchical storage of data and programs required for system operation. Specifically, it includes volatile memory for real-time caching of intermediate computation data, and non-volatile memory for storing system firmware and algorithm parameters. In this embodiment, a built-in 1MB SRAM can be used as volatile memory to cache intermediate data such as EKF state vectors and APF particle sets in real time; and an external 16MB Nor Flash (e.g., a W25Q128JV chip, connected via an SPI2 interface) can be used as non-volatile memory to store algorithm firmware, Gaussian mixture model (GMM) parameters, scene threshold tables, etc.

[0034] 4. Communication module: corresponding to Figure 1 The data output layer is used to output the final positioning results to the host computer or other systems. A dual-channel CAN bus is preferably used as the main communication interface (e.g., CAN1 and CAN2, 500kbps), supplemented by an Ethernet interface for debugging.

[0035] (1) CAN bus: Responsible for outputting standardized coded positioning data in a highly reliable and low-latency manner. To achieve lossless and deterministic transmission of high-precision values ​​on the digital bus, the CAN bus in this embodiment adopts a dual-channel design (CAN1 and CAN2). Its data output format is carefully designed to convert high-precision floating-point values ​​into fixed-point integers for transmission, ensuring both accuracy and unambiguity in data parsing between different hardware platforms. The specific encoding rules and transmission arrangements are as follows: Data Items Data types accuracy physical units transmission frequency CAN bus number CAN ID Notes (Coding Basis) Eastward position (x) int64_T <![CDATA[10 −7 m]]> m 50Hz CAN1 0x220 <![CDATA[Double - type position value × 10 7 , e.g., x = 1234.5678 m → 12345678000 (int64_T)]]> Northward position (y) int64_T <![CDATA[10 −7 m]]> m 50Hz CAN1 0x221 Same Eastward Position Coding Rules Celestial position (z) int64_T <![CDATA[10 −7 m]]> m 50Hz CAN1 0x222 <![CDATA[Height data after barometer compensation, refer to h in step S13 comp > Eastward velocity (x˙) int32_T <![CDATA[10 −3 m / s]]> m / s 50Hz CAN1 0x223 <![CDATA[double type speed value × 10 3 , such as x˙ = 15.678 m / s → 15678 (int32_T)]]> Northbound speed (y˙) int32_T <![CDATA[10 −3 m / s]]> m / s 50Hz CAN1 0x224 Eastward speed coding rules Celestial velocity (z˙) int32_T <![CDATA[10 −3 m / s]]> m / s 50Hz CAN1 0x225 Used for drone vertical take-off and landing control Roll angle (ϕ) int16_T <![CDATA[10 −2 rad]]> rad 50Hz CAN1 0x226 <![CDATA[double - type angular value × 10 2 , range ±π → ±314 (int16_T)]]> Pitch angle (θ) int16_T <![CDATA[10 −2 rad]]> rad 50Hz CAN1 0x227 Same roll angle coding rules to avoid exceeding drone pitch limits. Heading angle (ψ) int16_T <![CDATA[10 −2 rad]]> rad 50Hz CAN1 0x228 The attitude angle ψ in step S21 is used for directional control. Locational confidence uint8_T 1 0~100 10Hz CAN2 0x229 Referring to the calculation results of step S34, 0 = invalid, 100 = highest accuracy. As shown in the table, the standardized coding rules in this embodiment are as follows: core positioning data (such as east, north, and celestial positions) are encoded by amplifying double-precision floating-point values ​​by 10. 7 After being multiplied, it is converted to int64_T type for transmission, thus retaining 10. -7 The theoretical accuracy is in meters (i.e., 0.1 micrometers); the attitude angle data is converted to int16_T type for transmission by magnification by 100 times, thus retaining an accuracy of 0.01 radians.

[0036] The data transmission plan in this embodiment is as follows: all data items are assigned a unique CAN ID, and specific transmission buses and frequencies are planned, forming an efficient and orderly data flow. Specifically: the first CAN bus (e.g., CAN1) transmits the encoded core positioning data, including position, velocity, and attitude angle, at a frequency of 50Hz. The second CAN bus (e.g., CAN2) transmits system status information, mainly including the aforementioned multi-dimensional positioning reliability data, at a frequency of 10Hz.

[0037] (2) Ethernet interface: The LAN8720 chip can be used, which works in RMII mode with a speed of 100Mbps, and is used to output raw sensor data or for system debugging.

[0038] 5. Low-power control module: corresponding to Figure 1 The control layer is a hardware control loop driven by software decisions, used to implement system-level dynamic power management. It typically consists of GPIO pins (e.g., PA9) of the core processing module and power management circuitry.

[0039] Control logic: When the core processing module determines that the positioning status is continuously stable according to the fusion decision algorithm (such as the positioning confidence is higher than 0.9 for several consecutive frames), it will issue a control command.

[0040] Actions: This module responds to commands by directly controlling the power switch of the GNSS submodule via GPIO (high level for power-on, low level for power-off), and can dynamically adjust the operating frequency of the core processing module itself (e.g., from 480MHz to 240MHz). During idle periods, it is woken up by a timer (e.g., TMR0) interrupt to perform minimal necessary sampling, thereby reducing the overall standby power consumption of the system to an extremely low level (e.g., 80μA) without sacrificing the recoverability of the positioning function.

[0041] Through the collaborative work of the above modules, the positioning terminal in this embodiment constructs a complete hardware closed loop from accurate perception and intelligent processing to efficient control, providing a solid physical foundation for achieving high-precision, high-continuity, and low-power positioning in complex environments.

[0042] like Figures 2 to 5 As shown, based on the aforementioned high-precision positioning terminal, a high-precision positioning method based on multi-modal fusion filtering is introduced below. The core of this positioning method lies in the intelligent scheduling and fusion of multi-source information through a dynamic fusion decision center, specifically including the following steps: Step S1: Acquire sensor data collected in real time by the GNSS submodule, IMU submodule and barometer submodule, and preprocess the sensor data.

[0043] The purpose of this step is to transform raw, noisy sensor data with varying coordinate systems into clean, aligned, and standardized data that can be directly used by subsequent algorithms. The innovation of this step lies in achieving collaborative preprocessing and cross-validation among GNSS, IMU, and barometer data. This step includes the following sub-steps: Step S11: Perform outlier removal and smoothing on the GNSS data.

[0044] The Random Sample Consensus (RANSAC) algorithm was employed, with a residual threshold of 0.5 meters, to identify and eliminate abnormal positioning jumps caused by multipath effects. Subsequently, a 5-frame sliding time window was used to smooth and filter the latitude and longitude sequences, resulting in a more stable position estimate. During this process, only satellite data with a signal-to-noise ratio (SNR) ≥ 35 dB was retained for calculation, and the GNSS signal quality was quantified into three levels based on the number of visible satellites (sat_count): "Excellent" when sat_count ≥ 6; "Good" when 4 ≤ sat_count < 6; and "Poor" when sat_count < 4. This quality level will be used for adaptive parameter adjustment in subsequent filtering algorithms.

[0045] Step S12: Perform dynamic zero-bias calibration and installation error compensation on the IMU data.

[0046] The system detects the stationary period of the carrier every 100 milliseconds during operation, with the criterion being that the variance of the accelerometer is less than 0.01g² (where g is the acceleration due to gravity). During the static period, the mean zero bias of the gyroscope is updated by calculating the average of the angular velocities of the most recent 100 sampling points. For example, for the x-axis: This zero-bias parameter will be stored in real time and used for compensation. Simultaneously, the fixed installation deviation angle (e.g., roll deviation angle) between the IMU and the carrier coordinate system, determined during factory calibration, will be used. =0.3°, pitch deviation angle =0.2°), the original measured values ​​are compensated using a corresponding rotation matrix to eliminate inter-axis coupling errors. The acceleration and angular velocity data obtained after compensation are denoted as . and .

[0047] Step S13: Perform temperature compensation and consistency verification on the barometer data.

[0048] Based on the readings of the built-in temperature sensor Using the preset temperature compensation coefficient =0.02 m / ℃ to the original air pressure value Corrections were made. The barometer height was adjusted after compensation. The calculation formula is: .

[0049] in: The original altitude is calculated from the air pressure value using the standard air pressure altitude formula.

[0050] In addition, this step also calculates the compensated barometer height. With GNSS altitude The difference The calculation formula is as follows: .

[0051] when A distance greater than 5 meters is considered an inconsistency in the height between sensors. This difference... The absolute value will be directly used as the subsequent calculation of sensor consistency reliability. Input parameters.

[0052] Through the collaborative preprocessing steps S11 to S13, the data from the three independent sensors are synchronized in time, purified in quality, and clearly unified in physical meaning. More importantly, this process not only optimizes the data from each sensor independently, but also calculates the height difference. Preliminary consistency verification across sensors was achieved, providing a direct and crucial quality assessment basis for the subsequent intelligent weight allocation in the fusion decision layer.

[0053] Step S2: Based on the preprocessed sensor data, state estimation is performed using Extended Kalman Filter (EKF) and Adaptive Particle Filter (APF) to obtain the corresponding EKF positioning output and APF positioning output.

[0054] This step employs a two-layer filtering architecture, with two paths operating in parallel and featuring a collaborative mechanism. The execution steps of each path are described in detail below.

[0055] (1) Extended Kalman Filter (EKF) Path: This embodiment makes several key improvements to the traditional EKF, and its execution includes the following sub-steps: Step S21: Define the state vector of the extended Kalman filter (EKF). The state vector includes position, velocity, attitude angle, and the zero-bias state of the accelerometer and gyroscope to achieve online estimation and compensation of the main errors of the sensors.

[0056] Specifically, this embodiment constructs a 15-dimensional state vector. Defined as: in: These represent the directions of east, north, and sky. These are the velocities in the corresponding directions; These are roll angle, pitch angle, and yaw angle, respectively. These are the accelerometer zero bias values; These are the zero bias of the gyroscope.

[0057] Step S22: Perform state prediction and covariance prediction. Based on the preprocessed IMU data and the state at the previous time step, recursively obtain the predicted state and predicted covariance at the current time step.

[0058] Based on preprocessed IMU data (accelerometer) With angular velocity ) and the state at the previous moment Covariance A prediction is performed every 10ms. The state prediction formula is: in: This is the state transition matrix; To control the input matrix; , which is the control input vector.

[0059] The formula for predicting covariance is: Among them, the process noise covariance matrix Use a preset diagonal matrix, for example: .

[0060] Step S23: Perform observation update. When the GNSS signal is valid, dynamically construct the observation vector based on the signal quality and adjust the observation noise covariance. Update the state and covariance using Kalman gain.

[0061] When the GNSS signal is valid, an observation update is performed. This embodiment makes a key improvement in this step: ① Dynamically construct observation vectors : Adaptively select observation values ​​based on the number of visible GNSS satellites, sat_count.

[0062] If sat_count ≥ 6, then the observation vector Includes smoothed GNSS position and barometer-compensated altitude. and GNSS velocity information (6-dimensional); If 4 ≤ sat_count < 6, then the observation vector It contains only GNSS velocity information (3D).

[0063] ② Dynamically calculate the observation noise covariance Observation noise covariance It is not fixed, but dynamically adjusted according to the GNSS signal quality. The adjustment formula is as follows: in: The baseline covariance; As a correction coefficient based on the mean signal-to-noise ratio, this embodiment sets it to be... ; This is a correction factor based on the satellite elevation angle, with values ​​selected according to the satellite elevation angle range, following these rules: when the satellite elevation angle is <15°, =1.5; when the satellite elevation angle is between 15° and 45°, =1.2, when the satellite elevation angle is greater than 45°. =1.0.

[0064] ③ Calculate and update the Kalman gain: Calculate the Kalman gain based on the basic formula of Kalman filtering. and update the state vector. With covariance The calculation formula is as follows: in For the observation matrix, It is an identity matrix.

[0065] Step S24: Perform divergence detection and trigger APF strong auxiliary correction when divergence is detected to achieve synergy of two-layer filtering.

[0066] To enhance robustness, this embodiment adds a divergence detection mechanism. First, the observation residual vector is calculated. : in: For observation vectors; The observation matrix; This is the predicted state value.

[0067] Then observe the Mahalanobis distance of the residuals. This is used to make divergent judgments. The Mahalanobis distance of the observed residuals is then used. The calculation formula is: in: For predicting the state covariance matrix; This is the predicted state covariance matrix.

[0068] This embodiment uses the following two conditions as divergence criteria; if either condition is met, the EKF filter is determined to be diverging: ① Mahalanobis distance of the observed residuals ;in, This represents the threshold for a chi-square distribution with degrees of freedom (dof) at a confidence level of 0.99. For example, when the observations are 6-dimensional (dof=6), this threshold is approximately 16.81.

[0069] ②Predict the trace of the covariance matrix >1000.

[0070] Under normal circumstances, the Extended Kalman Filter (EKF) and the Adaptive Particle Filter (APF) are executed in parallel. Once divergence is detected, the APF is immediately activated for strong auxiliary correction: the state estimate of the output is calculated in real time and in parallel using the APF filter. State of EKF By resetting or imposing strong constraints, the synergy of the two-layer filtering can be achieved.

[0071] (2) Adaptive Particle Filter (APF) path: This embodiment makes core improvements to particle filtering, and its execution flow includes the following steps: Step S25: Dynamically initialize the particle set based on the GNSS signal quality, determine the total number of particles, and sample based on the prediction results of EKF.

[0072] The total number of particles N is dynamically determined based on the visible number of GNSS satellites, sat_count, to match computing resources with scene requirements. The specific rules are as follows: when sat_count ≥ 6, N = 50; when 4 ≤ sat_count < 6, N = 120; when sat_count < 4, N = 200.

[0073] The particle set uses the aforementioned EKF output state prediction values Covariance Based on Gaussian diffusion sampling, i.e. As the mean, with 1.2 * Sample the variance and covariance, and set initial weights. .

[0074] Step S26: Perform particle prediction, with each particle propagating forward based on IMU data and the state transition model.

[0075] Each particle is based on preprocessed IMU data (accelerometer). With angular velocity The same state transition model as EKF is used for forward propagation, and process noise is superimposed to obtain the predicted particle set. .

[0076] Step S27: Update particle weights based on Gaussian mixture model (GMM), calculate observation likelihood, and normalize weights to obtain APF state output.

[0077] Based on preprocessed GNSS / barometer data, the weight of each particle is calculated. The core improvement of this embodiment lies in using a Gaussian mixture model (GMM) to calculate the observation likelihood to update the weights. Specifically, two Gaussian components (C=2) are used, and their mixture weights, mean, and covariance parameters (e.g., ...) are calculated. =0.6, =0.4, =0.01, =0.03) can be dynamically adjusted according to the observation quality.

[0078] particle The weight update formula is: in, Indicates For the mean, The value of the Gaussian probability density function of the covariance. For GNSS observation vectors, For the corresponding observation matrix, Let be the covariance of the c-th Gaussian component.

[0079] After weight normalization, the fused state output of the APF is calculated. .

[0080] Step S28: Perform resampling judgment and resampling. To avoid particle degradation, trigger hierarchical resampling when the number of effective particles is insufficient.

[0081] Calculate the number of effective particles .when When the [0,1] interval is divided into N segments, random sampling is performed within each segment, new particles are selected based on the cumulative distribution of particle weights, a new particle set is formed, and the weights are reset to 1 / N.

[0082] It should be noted that the adaptability of the Adaptive Particle Filter (APF) in this embodiment is not only reflected in the dynamic configuration of the total number of particles based on the number of satellites, but also extended to a multi-mode operation mechanism. Combined with... Figure 3 As can be seen, the multi-mode operation mechanism provided in this embodiment dynamically adjusts the operational intensity and core functions of the APF based on the divergence state of the GNSS satellite visibility count (sat_count) and the EKF, in order to achieve the optimal match between computing resources and scenario requirements. ① Lightweight Auxiliary Mode: When sat_count≥6 and EKF does not diverge, APF operates with N=50 particles. Its core function is to finely correct the residual nonlinear error in the improved EKF estimation and provide highly reliable auxiliary state input for fusion decision-making.

[0083] ② Standard Balanced Mode: When 4≤sat_count<6 and EKF does not diverge, APF operates with N=120 particles, achieving a balance between positioning accuracy and computational robustness, and enhancing the adaptability of IMU data under moderate signal quality.

[0084] ③ Strong Auxiliary Dominant Mode: This mode is triggered in two situations: a) when EKF is detected to be diverging; b) when sat_count < 4 (regardless of the EKF state). In this mode, APF operates with the maximum number of particles N = 200. Its core function is upgraded to: in situation a, directly outputting its state estimate. Used to reset or strongly constrain divergent EKF states; in case b, when GNSS observation quality is extremely poor, positioning calculation is mainly based on IMU data and historical trajectory information, thereby ensuring the basic continuity of the system under the worst conditions.

[0085] Through the aforementioned multi-mode adaptive mechanism, the APF can intelligently switch between three roles: "assisted fine-tuning", "balanced enhancement" and "dominant protection", forming a deeply collaborative and functionally complementary estimation system with the basic filter layer (EKF).

[0086] (3) Two-layer collaborative mechanism: EKF and APF do not operate completely independently. When the divergence detection mechanism in the EKF path (step S24) is triggered, the system will immediately initiate a cooperative correction process. At this time, the output weights of the APF path are significantly enhanced, and its state estimation... It can be used to directly correct or partially reset the state of the EKF, thus forming a collaborative estimation system where the EKF dominates efficient conventional estimation and the APF provides anti-divergence protection. The outputs of the two paths ( and Together, they provide input for subsequent integration decisions.

[0087] Step S3: Based on the preprocessed sensor data, calculate the multi-dimensional positioning reliability that reflects the geometric accuracy of the GNSS signal, the stability of the IMU data, and the high consistency between the barometer and the GNSS.

[0088] This step forms the basis of the intelligent decision-making in this embodiment, and its innovation lies in constructing a multi-dimensional, comprehensive health assessment model. It quantifies the overall reliability of the system by integrating three independent indicators. The specific calculation method includes the following steps: Step S31: Calculate GNSS geometric accuracy confidence level .

[0089] This confidence component is used to assess the impact of satellite spatial distribution on positioning accuracy. In this embodiment, it is calculated based on the Position Accuracy Attenuation Factor (PDOP) provided by the GNSS submodule. A higher PDOP value indicates a worse satellite geometry, a greater potential positioning error, and a lower confidence level. The calculation formula is as follows: in, The first normalization constant is used in this embodiment. The preferred value is 10.

[0090] Step S32: Calculate the IMU stability reliability .

[0091] This reliability component is used to assess the health status and measurement reliability of the inertial sensor. In this embodiment, it is calculated based on the mean zero-bias value of the gyroscope in the IMU module. Excessive amplitude or fluctuation in the zero-bias indicates that the IMU may be affected by temperature drift or external interference, leading to decreased data reliability. The calculation formula is as follows: Among them, QUOTE This represents the mean zero bias of the gyroscope estimated in real time during step S12; The second normalization constant is used in this embodiment. The preferred value is 0.01 dps, but it can also be set according to the sensor specifications.

[0092] Step S33: Calculate sensor consistency reliability .

[0093] This reliability component is used to examine the consistency of data between different location sources in order to detect potential anomalies. Sensor consistency reliability. The calculation formula is: in, The altitude difference between the barometer and the GNSS calculated in step S13; The third normalization constant is preferably 5 meters in this embodiment.

[0094] Step S34: Synthesize multidimensional location reliability.

[0095] The reliability components of the three dimensions mentioned above are weighted and fused to obtain the final multidimensional location reliability confidence, which is normalized to the [0,1] interval. The formula for calculating multidimensional location reliability confidence is as follows: in, The predetermined weighting coefficients, and satisfying This embodiment uses , , The weighting configuration. This comprehensive reliability score intuitively and quantitatively reflects the overall reliability level of the entire positioning system at the current moment.

[0096] Step S4: Based on the multi-dimensional positioning confidence, dynamically allocate the fusion weights of EKF positioning output, APF positioning output, and historical positioning data.

[0097] This step is the core of the dynamic fusion decision-making process. Its purpose is to adaptively adjust the contribution weights among the Extended Kalman Filter (EKF) output, the Adaptive Particle Filter (APF) output, and historical positioning data based on the real-time health status of the system reflected by the multi-dimensional positioning confidence calculated in step S3. This provides a precise basis for subsequent optimal fusion. This step specifically includes the following sub-steps: Step S41: Weight allocation rules based on two-dimensional query.

[0098] The dynamic allocation of weights follows a set of pre-defined, non-linear decision rules. These rules use the comprehensive positioning confidence output in step S3 and the GNSS satellite visibility count (sat_count) obtained in step S1 preprocessing as two key input dimensions. Through table lookup or logical judgment, a set of optimal weight coefficients is output. These correspond to the fusion weights of EKF positioning output, APF positioning output, and historical positioning data, respectively. .

[0099] This embodiment provides a specific weight allocation rule: When the system is in a high-reliability state, i.e., when sat_count ≥ 6 and Confidence ≥ 0.8, set =0.8, =0.1, 0.1. In this state, the system highly trusts the EKF estimation based on high-quality GNSS signals, and therefore assigns it a high weight, while moderately retaining the weights of APF and historical trajectories can maintain the system's robustness.

[0100] When the system is in a low reliability or signal impairment state, i.e., when sat_count < 4 or Confidence < 0.3, set =0.1, =0.6, 0.3. In this state, the reliability of EKF decreases due to its reliance on GNSS observations, so its weight is significantly reduced. At the same time, the weight of APF, which does not rely on instantaneous GNSS observations, is greatly increased to dominate the estimation, and the weight of historical trajectories is increased to take advantage of motion continuity to smooth the output.

[0101] Step S42: Generation of historical location data.

[0102] Historical positioning data is used to provide short-term inertial constraints on the carrier's motion, preventing abrupt changes in results. It is not simply a cache of raw data, but rather generated by weighted smoothing of the final positioning results output by the system in the previous processing cycle and even earlier cycles. In this embodiment, it is preferably generated by weighted smoothing of the final positioning results output from the first two frames. Let the final fusion state at time k be denoted as... Then historical location data One specific generation method is as follows: .

[0103] in: and They are respectively and The final positioning result at any given moment.

[0104] Step S43: Output and system linkage.

[0105] This step ultimately outputs a dynamically determined combination of fusion weights. This weighted combination will be directly transmitted to the subsequent step S5 for weighted fusion calculation to obtain the final localization result. Furthermore, the high-confidence states determined in this step (such as a confidence level consistently above 0.9) will simultaneously trigger low-power control logic, providing a basis for system-level energy efficiency optimization. This demonstrates the algorithm's ability to coordinate and schedule hardware resources.

[0106] Step S5: Based on the fusion weights, perform weighted fusion of EKF positioning output, APF positioning output, and historical positioning data to output the final positioning result.

[0107] This step is the terminal of the location information processing chain, responsible for performing the final arbitration and output. Its core lies in optimally synthesizing the heterogeneous estimates from different filters and historical information based on the dynamic fusion weights determined in step S4, generating a unique, continuous, and highly reliable location result, and completing the system's closed-loop management. This step specifically includes the following sub-steps: Step S51: Weighted fusion calculation.

[0108] This step receives the EKF positioning output from step S2. APF positioning output and the historical location data generated in step S4 Simultaneously, the dynamic fusion weights from step S4 are received. .

[0109] The final fused localization state vector is calculated using a linear weighted fusion algorithm. The calculation formula is as follows: in, , , and It is a multi-dimensional vector containing state variables such as position, velocity, and attitude angles. This calculation process realizes intelligent optimization and smoothing of multi-source positioning information within a unified mathematical framework.

[0110] Step S52: Data encoding and output.

[0111] To meet the demands of high-precision industrial applications, the fused floating-point positioning data needs to be standardized and encoded for reliable transmission via the digital bus. The encoding rules in this step strictly correspond to the CAN bus output format defined by the aforementioned communication module.

[0112] Specifically, The double-precision position components (such as the eastward position x) are magnified by 10. 7 After multiplying, convert to a 64-bit signed integer (int64T), and convert the attitude angle components (such as roll angle) to... After being magnified 100 times, the data is converted into a 16-bit signed integer (int16T). The encoded data is then output in real-time via the CAN bus according to a predefined CAN identifier (ID) and frame structure, ensuring 10... -7 The displacement accuracy in meters and the attitude angle accuracy in 0.01 radians are retained losslessly during transmission.

[0113] Step S53: System closed-loop management and optimization.

[0114] This step completes two key closed-loop operations to maintain continuous system operation and optimize its state: (1) Historical data update: The final fusion result obtained in this calculation is updated. Write to the historical data buffer, overwriting the old data. This operation provides the latest input for the generation of historical trajectories in step S4 of the next processing cycle, forming a core data closed loop of "fusion → output → feedback → re-fusion".

[0115] (2) Low-power control trigger: Continuously monitor the overall positioning confidence used to calculate the weights. When the confidence value is consistently higher than a stable threshold (e.g., confidence ≥ 0.9 for 5 consecutive frames), the current positioning status is determined to be extremely stable (sat_count ≥ 6, GNSS signal is good). At this time, the core processing module sends an instruction to the low-power control module to perform a series of operations, including reducing the sampling frequency of the GNSS submodule (from 10Hz to 5Hz) and reducing the CPU operating frequency, so that the system enters the low-power maintenance mode and avoids the positioning accuracy from being reduced due to shutting down the GNSS submodule.

[0116] like Figures 1 to 5 As shown, the high-precision positioning terminal and method provided in this embodiment achieve significant breakthroughs over existing technologies at multiple levels by constructing a complete technical closed loop from data source to decision output, thereby obtaining excellent comprehensive positioning performance in complex environments, as specifically demonstrated below: Firstly, at the data foundation level, this embodiment fundamentally improves the quality and consistency of input data through a multi-sensor collaborative preprocessing architecture. Unlike existing technologies that process sensor data in isolation, this embodiment innovatively integrates intelligent cleaning of GNSS data (RANSAC anomaly removal), dynamic zero bias and real-time compensation for installation errors in IMU data, and temperature compensation and cross-source altitude verification of barometer data. This collaborative mechanism effectively eliminates outliers and systematic errors in the raw data, ensuring that the subsequent core algorithms receive clean, reliable, and physically consistent high-quality data, laying a solid data foundation for high-precision positioning.

[0117] Secondly, at the core algorithm level, a dual-layer intelligent filtering architecture consisting of "improved EKF" and "adaptive APF" achieves adaptive and accurate estimation for both linear and nonlinear scenarios. On one hand, the 15-dimensional state vector EKF comprehensively models motion and error states and dynamically adjusts the observation model based on signal quality (SNR, elevation angle), improving estimation accuracy and robustness to signal attenuation under linear conditions. On the other hand, the APF dynamically configures the particle size according to the number of satellites and uses a Gaussian mixture model (GMM) to fit complex observation likelihoods, thereby maintaining stable tracking under nonlinear scenarios such as severe signal fluctuations or strong carrier maneuvers. The two work together through a divergent detection mechanism to ensure the continuity and reliability of state estimation across all scenarios.

[0118] Thirdly, at the system decision-making level, this embodiment achieves intelligent smoothing and high continuity of positioning output through a dynamic fusion mechanism based on multi-dimensional reliability. This embodiment abandons the fixed-weight fusion strategy and creatively constructs a three-dimensional positioning reliability model that fuses GNSS geometric accuracy, IMU stability, and the high consistency between the barometer and GNSS signals. Based on this reliability and the real-time satellite count, the system dynamically and optimally allocates the fusion weights of EKF output, APF output, and historical trajectories. This allows the system to smoothly and seamlessly switch the dominant algorithm source during the gradual or abrupt change in GNSS signal from good to lost, significantly suppressing jumps and drifts in positioning results and achieving a positioning continuity of up to 99.5%.

[0119] Fourth, at the engineering implementation level, this embodiment endows the solution with high practicality and generalization capabilities through scene-adaptive strategies, module-level low-power control, and standardized data encoding output. The system can automatically identify various scenarios such as static, high-speed, and turning scenarios and load the optimal parameter set, adapting to different platforms such as vehicle-mounted, drone, and surveying applications without secondary development. Simultaneously, the innovative three-level low-power control strategy reduces standby power consumption to extremely low levels while ensuring navigation continuity, significantly extending the battery life of mobile devices. Furthermore, the "floating-point to fixed-point" CAN bus encoding scheme, specifically designed for industrial control, ensures that high-precision positioning results can be reliably transmitted with low latency and no accuracy loss, meeting the stringent real-time requirements of critical applications such as autonomous driving.

[0120] In summary, through the aforementioned interconnected technological innovations, this invention systematically solves the bottlenecks of existing solutions in terms of data quality, algorithm adaptability, decision intelligence, and engineering practicality, ultimately realizing a positioning solution that can provide high precision, high continuity, high reliability, and low power consumption in complex dynamic environments.

[0121] The above are merely specific embodiments of the present invention, but the design concept of the present invention is not limited thereto. Any non-substantial modifications made to the present invention using this concept shall be considered as infringing upon the protection scope of the present invention.

Claims

1. A high-precision positioning method based on multimodal fusion filtering, characterized in that: Includes the following steps: Step S1: Acquire sensor data collected in real time by the GNSS submodule, IMU submodule and barometer submodule, and perform collaborative preprocessing on the sensor data; Step S2: Based on the preprocessed sensor data, state estimation is performed using Extended Kalman Filter (EKF) and Adaptive Particle Filter (APF) respectively to obtain the corresponding EKF positioning output and APF positioning output; Step S3: Based on the preprocessed sensor data, calculate the multi-dimensional positioning reliability that reflects the geometric accuracy of the GNSS signal, the stability of the IMU data, and the high consistency between the barometer and the GNSS. Step S4: Based on the multi-dimensional positioning confidence, dynamically allocate the fusion weights of the EKF positioning output, the APF positioning output, and the historical positioning data; Step S5: Based on the fusion weight, perform weighted fusion on the EKF positioning output, the APF positioning output, and the historical positioning data, and output the final positioning result.

2. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: The collaborative preprocessing method in step S1 includes the following steps: Step S11: Perform outlier removal and sliding window smoothing on the GNSS data based on the RANSAC algorithm; Step S12: Perform dynamic zero-bias calibration based on static time period detection and installation error compensation based on fixed installation deviation angle on the IMU data; Step S13: Perform temperature compensation coefficient-based preprocessing on the barometer data. Altitude compensation, and calculate the barometer height after compensation. With GNSS altitude The difference .

3. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: In step S2, the Extended Kalman Filter (EKF) is based on a 15-dimensional state vector. It is used for online estimation of position, velocity, attitude angle, and sensor zero bias, and is defined as follows: in: These represent the directions of east, north, and sky. These are the velocities in the corresponding directions; These are roll angle, pitch angle, and yaw angle, respectively. These are the accelerometer zero bias values; These are the zero bias of the gyroscope.

4. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: In step S2, the observation noise covariance of the extended Kalman filter (EKF) The adjustment formula is based on the real-time average signal-to-noise ratio of the GNSS signal and the dynamic adjustment of the satellite elevation angle: in: The baseline covariance; These are correction coefficients based on the mean signal-to-noise ratio. This is a correction factor based on the satellite elevation angle.

5. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: In step S2, the total number N of particles in the adaptive particle filter (APF) is dynamically determined based on the GNSS satellite visibility count (sat_count), according to the following rules: When sat_count ≥ 6, N = 50; When 4 ≤ sat_count < 6, N = 120; When sat_count < 4, N = 200.

6. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: Step S3 includes the following sub-steps: Step S31: Calculate the GNSS geometric accuracy reliability based on the GNSS signal position accuracy attenuation factor (PDOP). Step S32: IMU-based gyroscope zero-bias mean Calculate IMU stability reliability ; Step S33: Based on the altitude difference between the barometer and GNSS Calculate sensor consistency reliability Step S34: Assess the GNSS geometric accuracy reliability. IMU stability reliability Consistency reliability with sensors By performing a weighted summation, the multi-dimensional location confidence is obtained: in: The predetermined weighting coefficients, and satisfying .

7. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: In step S2, the extended Kalman filter (EKF) includes a divergence detection mechanism. When divergence is detected, the adaptive particle filter (APF) is triggered to perform strong auxiliary correction, and the APF is used to locate and correct or reset the state estimate of the EKF.

8. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: In step S4, the dynamic allocation of fusion weights takes the comprehensive positioning confidence and GNSS satellite visibility count (sat_count) as inputs, and outputs the fusion weights corresponding to the EKF positioning output, APF positioning output, and historical positioning data. The weighting rules are as follows: When sat_count ≥ 6 and Confidence ≥ 0.8, =0.8, =0.1, 0.1; When sat_count < 4 or Confidence < 0.3, =0.1, =0.6, 0.

3.

9. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: In steps S4 and S5, the historical positioning data is generated by weighted smoothing of the final positioning results of the first two frames, and its calculation formula is as follows: in: Historical location data; and They are respectively and The final positioning result at any given moment.

10. The high-precision positioning method based on multimodal fusion filtering as described in claim 1, characterized in that: In step S5, the final positioning result is output after standardized encoding and via the communication bus; the standardized encoding method is to amplify the floating-point position component by 10. 7 After being multiplied, the data is converted to int64_T type. The floating-point attitude angle component is amplified by 100 times and then converted to int16_T type. The communication bus includes a first CAN bus and a second CAN bus. The first CAN bus transmits the encoded position, velocity and attitude angle data at a frequency of 50Hz, and the second CAN bus transmits the multi-dimensional positioning confidence data at a frequency of 10Hz.

Images