Multi-source data synchronization inertial integrated navigation method, device, equipment and medium

Abstract

Embodiments of the present disclosure disclose a multi-source data synchronization inertial integrated navigation method, device, equipment and medium. A specific embodiment of the method comprises: time stamping and processing each navigation sensor data, and synchronously compensating and processing each obtained asynchronous observation data to obtain each synchronous observation data; dynamically evaluating the confidence of each synchronous observation data to obtain each dynamic confidence factor; adaptively compensating and adjusting each dynamic confidence factor and the statistical characteristics of each historical innovation sequence to obtain each target observation noise matrix; fusion solving and processing each synchronous observation data to obtain a navigation state estimation value and each current innovation sequence; updating the statistical characteristics of each historical innovation sequence; and controlling the motion of a carrier based on the navigation state estimation value. The embodiment provides key technical support for application fields such as automatic driving which have strict requirements on navigation performance.

Description

Technical Field

[0001] The embodiments of this disclosure relate to the field of computer technology, and more specifically to multi-source data synchronous inertial navigation methods, apparatus, devices, and media. Background Technology

[0002] With the rapid development of autonomous driving, mobile robots, and other fields, the demand for high-precision and high-reliability navigation systems is becoming increasingly urgent. Inertial Navigation Systems (INS) rely on Inertial Measurement Units (IMUs) for dead reckoning, continuously outputting the attitude, velocity, and position information of the vehicle. However, their errors accumulate over time, making it difficult to independently meet the requirements of long-term high-precision navigation. Global Navigation Satellite Systems (GNSS) can provide long-term stable absolute position information and are often combined with INS to suppress their accumulated errors. However, in complex environments such as urban canyons and tunnels, GNSS signals are easily blocked or interfered with, leading to a sharp decline in positioning accuracy. Therefore, existing technologies typically introduce auxiliary navigation sources such as Visual Odometry (VO) and Ultra-Wideband (UWB) positioning systems to improve the accuracy and robustness of the navigation system through multi-source information fusion. Related attempts mainly focus on loosely or tightly coupled fusion of data from various sensors and the use of Kalman filtering for state estimation.

[0003] However, the above approach often presents the following technical challenges: First, different navigation sensors typically have independent hardware clocks and different data output frequencies, leading to time-synchronization issues in multi-source data. Simple timestamp alignment or interpolation introduces non-negligible time synchronization errors. At high speeds, microsecond-level time deviations, after integration, translate into meter-level spatial positioning errors, severely limiting the accuracy ceiling of fused navigation. Second, the reliability and signal-to-noise ratio of each navigation source are time-varying in complex environments. For example, GNSS accuracy varies with the number and geometric distribution of visible satellites; visual odometry suffers from reduced feature tracking quality when texture is missing or illumination changes drastically; and UWB generates significant ranging errors in non-line-of-sight environments. Traditional fixed-weight fusion strategies cannot dynamically respond to these changes, resulting in decreased or even divergent system accuracy when a data source momentarily fails. Furthermore, existing methods lack effective online detection and compensation mechanisms for sudden abnormal observations such as UWB non-line-of-sight errors and visual mismatches. Abnormal data directly participating in filtering updates contaminates state estimation, causing navigation solutions to jump and be difficult to recover quickly. Furthermore, the accumulation of the above problems will ultimately affect the control of the carrier's motion based on the navigation state estimate. How to effectively coordinate high-precision and high-reliability navigation state estimates with the actuator to achieve accurate and stable control of the carrier's motion also constitutes a key technical obstacle.

[0004] The information disclosed in this background section is only intended to enhance the understanding of the background of the inventive concept, and therefore may contain information that does not constitute prior art known to those skilled in the art. Summary of the Invention

[0005] The summary portion of this disclosure is intended to provide a brief overview of the concepts, which will be described in detail in the detailed description portion. This summary portion is not intended to identify key or essential features of the claimed technical solutions, nor is it intended to limit the scope of the claimed technical solutions.

[0006] Some embodiments of this disclosure provide a multi-source data synchronous inertial navigation method, apparatus, electronic device, and computer-readable medium to solve one or more of the technical problems mentioned in the background section above.

[0007] In a first aspect, some embodiments of this disclosure provide a multi-source data synchronous inertial navigation method, comprising: timestamping the collected navigation sensor data to obtain timestamped asynchronous observation data; performing synchronization compensation processing on the asynchronous observation data to obtain synchronous observation data; dynamically evaluating the confidence level of the synchronous observation data to obtain dynamic confidence factors; adaptively compensating and adjusting the statistical characteristics of the dynamic confidence factors and historical news sequences to obtain target observation noise matrices; performing fusion calculation processing on the synchronous observation data based on the target observation noise matrices to obtain navigation state estimates and current news sequences; updating the statistical characteristics of the historical news sequences based on the current news sequences; and controlling the motion of the carrier based on the navigation state estimates.

[0008] Secondly, some embodiments of this disclosure provide a multi-source data synchronous inertial navigation system, comprising: a marking unit configured to timestamp collected navigation sensor data to obtain timestamped asynchronous observation data; a synchronization compensation unit configured to perform synchronization compensation on the asynchronous observation data to obtain synchronous observation data; a confidence dynamic evaluation unit configured to perform confidence dynamic evaluation on the synchronous observation data to obtain dynamic confidence factors; an adaptive compensation adjustment unit configured to adaptively compensate and adjust the statistical characteristics of the dynamic confidence factors and historical news sequences to obtain target observation noise matrices; a fusion calculation unit configured to perform fusion calculation on the synchronous observation data based on the target observation noise matrices to obtain navigation state estimates and current news sequences; an update unit configured to update the statistical characteristics of the historical news sequences based on the current news sequences; and a control unit configured to control the motion of a carrier based on the navigation state estimates.

[0009] Thirdly, some embodiments of this disclosure provide an electronic device, including: one or more processors; and a storage device having one or more programs stored thereon, wherein when the one or more programs are executed by the one or more processors, the one or more processors implement the method described in any implementation of the first aspect above.

[0010] Fourthly, some embodiments of this disclosure provide a computer-readable medium having a computer program stored thereon, wherein the program, when executed by a processor, implements the method described in any of the implementations of the first aspect above.

[0011] Fifthly, some embodiments of this disclosure provide a computer program product, including a computer program that, when executed by a processor, implements the method described in any of the implementations of the first aspect above.

[0012] The above embodiments of the present invention have the following beneficial effects: Through the multi-source data synchronization inertial navigation method of the present invention, high-precision time synchronization, adaptive fusion, and anomaly error suppression of multi-source navigation data can be achieved, significantly improving the accuracy, robustness, and real-time performance of the integrated navigation system in complex environments, and ultimately achieving precise and reliable control of the carrier's motion. Specifically, traditional multi-source integrated navigation methods (such as existing schemes relying on simple timestamp alignment, fixed-weight fusion, and threshold-based anomaly removal) may encounter problems such as decreased positioning accuracy due to time synchronization errors, divergence in state estimation caused by fusion weight mismatch, and filter jumps caused by abnormal observation contamination when facing sensor clock differences, time-varying signal quality, and sudden anomalies. If only conventional loosely coupled or tightly coupled fusion is relied upon, microsecond-level time differences may be converted into meter-level errors, low-quality data may have excessively high weights, and outlier interference may be difficult to recover quickly, ultimately leading to navigation failure and control mismanagement. Based on this, the multi-source data synchronization inertial navigation method of the present invention first timestamps the collected navigation sensor data to obtain asynchronous observation data with timestamps. Therefore, high-precision absolute timestamps are uniformly marked for sensor data from different clock sources and sampling frequencies, eliminating clock differences at the data source and providing a foundation for subsequent accurate synchronization, avoiding alignment errors caused by hardware clock asynchrony. Then, synchronization compensation processing is performed on the aforementioned asynchronous observation data to obtain synchronized observation data. Based on timestamps and high-frequency inertial data, the displacement of the carrier caused by time differences is accurately compensated by calculating the time difference and extrapolating the motion trajectory, mapping asynchronous data to a unified fusion time. This effectively solves the meter-level spatial positioning error caused by microsecond-level time differences in high-speed dynamic scenarios, achieving true high-precision time synchronization. Next, dynamic confidence evaluation processing is performed on the aforementioned synchronized observation data to obtain various dynamic confidence factors. Thus, for different navigation sources such as GNSS, visual odometry, and UWB, multi-dimensional indicators such as the number of visible satellites, the number of feature points, and signal strength are selected for real-time reliability evaluation, generating dynamic factors that quantify the real-time reliability of sensors. This provides an accurate quality basis for subsequent adaptive fusion, overcoming the shortcomings of fixed weights that cannot respond to environmental changes. Subsequently, adaptive compensation and adjustment processing is performed on the statistical characteristics of each dynamic confidence factor and each historical news sequence to obtain the target observation noise matrix. Thus, on the one hand, the baseline observation noise matrix is ​​scaled according to the dynamic confidence factor to achieve confidence-driven adaptive weight adjustment; on the other hand, anomalies are detected based on the statistical characteristics of the historical news sequences, and penalties are imposed on anomalous observation sources. The penalized noise matrix is ​​used as the target matrix. This dual mechanism ensures that the observation noise matrix can reflect the true quality of each data source in real time and accurately, and effectively suppresses the impact of sudden anomalies.Then, based on the noise matrices of each target observation, the synchronous observation data are fused and processed to obtain navigation state estimates and current information sequences. The adjusted noise matrices are then substituted into a Kalman filter to perform optimal fusion of the multi-source synchronous data, obtaining a high-precision state estimate. Simultaneously, the information sequences are output for feedback, achieving a dynamic closed loop in the fusion process. Next, based on the current information sequences, the statistical characteristics of the historical information sequences are updated. This integrates the current information with historical statistics, enabling historical statistical characteristics to dynamically track data changes and providing a real-time updated benchmark for anomaly detection in the next cycle, forming an adaptive continuous optimization mechanism. Finally, based on the navigation state estimates, the vehicle's motion is controlled. This transforms the high-precision, high-reliability navigation state into specific control commands, driving actuators such as steering, throttle, and braking, achieving a complete closed loop from perception fusion to physical control, ensuring accurate and stable movement of the vehicle in complex environments. Furthermore, because this method incorporates dynamic perception and closed-loop feedback mechanisms in core aspects such as data synchronization, confidence assessment, adaptive adjustment, and anomaly compensation, it can effectively cope with complex operating conditions such as sensor clock differences, signal quality fluctuations, and sudden anomalies. It possesses an inherent ability to suppress low-quality or abnormal data, thereby enhancing the system's robustness and generalization in real-world scenarios. Simultaneously, by organically combining time synchronization, adaptive fusion, and anomaly handling, the entire navigation system achieves an overall improvement in accuracy, reliability, and real-time performance, providing crucial technical support for applications with stringent navigation performance requirements, such as autonomous driving and mobile robots. Attached Figure Description

[0013] The above and other features, advantages, and aspects of the embodiments of this disclosure will become more apparent from the accompanying drawings and the following detailed description. Throughout the drawings, the same or similar reference numerals denote the same or similar elements. It should be understood that the drawings are schematic, and elements are not necessarily drawn to scale.

[0014] Figure 1 This is a flowchart of some embodiments of the multi-source data synchronous inertial integrated navigation method according to the present disclosure; Figure 2 These are schematic diagrams illustrating the structure of some embodiments of the multi-source data synchronous inertial navigation device according to this disclosure; Figure 3 This is a schematic diagram of the structure of an electronic device suitable for implementing some embodiments of the present disclosure. Detailed Implementation

[0015] Embodiments of this disclosure will now be described in more detail with reference to the accompanying drawings. While some embodiments of this disclosure are shown in the drawings, it should be understood that this disclosure can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of this disclosure. It should be understood that the accompanying drawings and embodiments of this disclosure are for illustrative purposes only and are not intended to limit the scope of protection of this disclosure.

[0016] It should also be noted that, for ease of description, only the parts relevant to the invention are shown in the accompanying drawings. Unless otherwise specified, the embodiments and features described in this disclosure can be combined with each other.

[0017] It should be noted that the concepts of "first" and "second" mentioned in this disclosure are used only to distinguish different devices, modules or units, and are not used to limit the order of functions performed by these devices, modules or units or their interdependencies.

[0018] It should be noted that the terms "a" and "a plurality of" used in this disclosure are illustrative rather than restrictive, and those skilled in the art should understand that, unless otherwise expressly indicated in the context, they should be understood as "one or more".

[0019] The names of messages or information exchanged between multiple devices in the embodiments of this disclosure are for illustrative purposes only and are not intended to limit the scope of such messages or information.

[0020] This disclosure will now be described in detail with reference to the accompanying drawings and embodiments.

[0021] Figure 1 A flowchart 100 is shown, illustrating some embodiments of a multi-source data synchronization inertial navigation method according to the present disclosure. This multi-source data synchronization inertial navigation method includes the following steps: Step 101: Timestamp the collected navigation sensor data to obtain asynchronous observation data with timestamps.

[0022] In some embodiments, the execution entity of the multi-source data synchronous inertial navigation method (e.g., a computing device with data processing capabilities such as an onboard computing unit, flight controller, or robot main control computer) can timestamp the collected navigation sensor data to obtain timestamped asynchronous observation data. Navigation sensor data refers to the raw measurement messages or signals directly output by various navigation sensors (such as inertial measurement units, global navigation satellite system receivers, visual odometry, ultra-wideband positioning tags, etc.), typically containing information such as position, velocity, attitude, distance, and image, but without unified time calibration. Timestamping refers to the process of attaching or associating time information based on a unified time reference to the raw sensor data. Timestamped asynchronous observation data refers to a set of data where each data point is assigned a time identifier under a unified time reference, but due to the different sampling times of each sensor, it exhibits a non-uniform distribution on the time axis. In practice, the aforementioned execution entities can receive raw data output from various navigation sensors via hardware interfaces (such as Controller Area Network (CAN), Ethernet, serial peripheral interfaces, etc.) and use a high-precision synchronous clock source (such as a network clock based on the IEEE 1588 Precision Time Protocol or a local clock trained by a 1PPS pulse provided by a Global Navigation Satellite System (GNSS) receiver) to mark these data with a unified system absolute time, thereby completing the timestamp marking process. For example, in an autonomous vehicle, the inertial measurement unit may output acceleration and angular velocity data at a frequency of 200Hz, the GNSS receiver outputs position information at 5Hz, and the visual odometry outputs image frames at 30Hz. The arrival times of these data on the execution entity are different. By attaching a microsecond-level timestamp based on the PTP master clock to each data message, timestamped asynchronous observation data can be formed.

[0023] In some optional implementations of certain embodiments, the aforementioned execution entity may perform timestamp processing on the collected navigation sensor data through the following steps to obtain timestamped asynchronous observation data: Step one involves acquiring the acquisition times corresponding to the data from each navigation sensor through various observation sources. An observation source refers to a physical device or software module that provides navigation measurement data. Each observation source has an independent internal clock and data output channel, capable of continuously generating raw observation data reflecting the vehicle's motion state or environmental information. The acquisition time refers to the instantaneous moment when the sensor actually samples the physical quantity. It is usually recorded by the observation source's internal clock or locked by an external hardware trigger signal, and its accuracy directly affects the accuracy of subsequent time synchronization. In practice, the aforementioned execution entity can extract time information as the acquisition time from the received data frames through data communication links established with each observation source. When generating data messages, the observation source typically embeds the current count value of its internal clock or time information based on a specific time base into the message header or metadata. The execution entity parses these messages to obtain the acquisition time under the local clock of that observation source. For example, for a Global Navigation Satellite System (GNSS) receiver, the output NMEA-0183 protocol message typically includes a UTC time field (such as the timestamp in $GPRMC). This time is generated by the receiver's internal clock at the positioning calculation time, and the execution entity can obtain the acquisition time by parsing this field. For a visual odometry (VOA), the camera module can output a hardware trigger signal at the end of the exposure. The execution entity's field-programmable gate array (FPGA) captures this signal and records a high-precision counter value, which, after conversion, becomes the acquisition time. The aforementioned observation sources include GNSS receivers, visual odometry, and ultra-wideband positioning systems (UWBS). A GNSS receiver is a dedicated radio receiving device capable of receiving radio frequency signals transmitted by at least one Global Navigation Satellite System (GNSS) (such as GPS, BeiDou, GLONASS, Galileo, etc.), and determining the position, velocity, and precise time of the receiving antenna in three-dimensional space in real time by calculating pseudorange, carrier phase, and other observation values. Visual odometry is a visual navigation system that continuously acquires image sequences of the environment surrounding a vehicle, uses computer vision technology to extract and track feature points in the images, and estimates the relative motion of the camera itself between adjacent frames to recursively calculate the vehicle's position and attitude changes. Ultra-wideband (UWB) positioning systems utilize nanosecond-level extremely narrow pulse signals for wireless communication and ranging. They typically consist of multiple fixed base stations with known locations and mobile tags mounted on the vehicle. Distance measurement with centimeter-level accuracy is achieved by measuring the flight time of the signal between the base stations and the tags, and the three-dimensional coordinates of the tags can be calculated using polygonal positioning algorithms.

[0024] Step two involves adding the corresponding system absolute timestamps for each acquisition time to the navigation sensor data, resulting in timestamped asynchronous observation data. The system absolute timestamp refers to a globally unique and comparable time marker generated based on a unified time base (such as Coordinated Universal Time or the system master clock time maintained by a high-precision clock source), typically expressed with nanosecond or microsecond precision. Establishing system absolute timestamps requires the support of a high-precision synchronous clock source, such as the IEEE 1588 precision time protocol master clock, a local clock trained with 1PPS pulses from a Global Navigation Satellite System receiver, or an atomic clock. Asynchronous observation data refers to a set of data that has been timestamped but not yet time-aligned. Its characteristics include a non-uniform distribution of data on the time axis, inconsistent time intervals between adjacent data points, and non-one-to-one correspondence between data from different observation sources at the same timestamp. In practice, after acquiring the acquisition time under the local clock of each observation source, the executing entity needs to convert it to a unified system absolute time. This is typically achieved through a pre-established clock mapping: the executing entity continuously monitors the offset and drift rate of each observation source clock relative to the system absolute clock and establishes a clock correction model. For each acquisition moment, the correction model is applied to calculate the corresponding system absolute time, and then this absolute timestamp is appended to the raw data. For example, the executing entity maintains a local clock synchronized based on the PTP protocol as the system absolute time reference. For the ranging value output by the ultra-wideband positioning system, its original message may only contain the tag ID and time of flight, but the ultra-wideband module can output a synchronization pulse signal at the same time as sending the ranging pulse. The executing entity's hardware timer captures the precise system absolute time of this pulse as the acquisition moment of the ranging value. The executing entity encapsulates the system absolute timestamp and the raw ranging value into a new data structure, such as a binary message containing a timestamp field and a ranging value field, and stores it in a circular buffer for subsequent processing. Finally, each navigation sensor data is assigned a unique system absolute timestamp based on the same time reference, forming asynchronous observation data with timestamps. Although these data have precise and uniform timestamps, they are still asynchronous and non-uniformly distributed on the time axis due to the different sampling frequencies and sampling times of each observation source. This provides accurate input for subsequent synchronization compensation processing.

[0025] Step 102: Perform synchronization compensation processing on each asynchronous observation data to obtain each synchronous observation data.

[0026] In some embodiments, the execution entity can perform synchronization compensation processing on the asynchronous observation data to obtain synchronous observation data. Synchronization compensation processing refers to calculating the time difference between the timestamp carried by the asynchronous data and a preset fusion calculation time, and using the high-frequency motion information output by the inertial measurement unit to deduce the displacement change of the carrier within this time difference, thereby correcting the original asynchronous observation values ​​to make them equivalent to the processing process acquired at a unified fusion calculation time. Synchronous observation data refers to the virtual observation values ​​corresponding to each observation source at the preset fusion calculation time after the aforementioned time synchronization and displacement compensation. These data are strictly aligned on the time axis and can be directly used for subsequent multi-source fusion calculations. In practice, the execution entity can first preset a series of fusion calculation times in memory (e.g., a time sequence generated at fixed intervals of 50 milliseconds), and then process each asynchronous observation data sequentially, correcting it to the next fusion calculation time closest to the observation data through the following four steps, thereby obtaining a set of synchronously aligned observation data. For example, for a global navigation satellite system position observation arriving at 100.123 seconds, the executing entity synchronously compensates it to the next fusion calculation time of 100.150 seconds, generating the synchronous observation position corresponding to that time.

[0027] In some optional implementations of certain embodiments, the aforementioned execution entity can perform synchronization compensation processing on the asynchronous observation data through the following steps to obtain the synchronous observation data: Step 1: Based on the timestamps corresponding to the asynchronous observation data and the preset fusion calculation time, generate various time difference values. The timestamp refers to the system absolute timestamp marked for each asynchronous observation data in step 101, which precisely represents the actual physical observation time of the data. The preset fusion calculation time refers to the unified time point to which all observation source data ultimately needs to be aligned, pre-set during the system design phase or before operation. These time points typically appear cyclically at fixed time periods (e.g., 10 milliseconds, 50 milliseconds, or 100 milliseconds) and serve as the reference time for the Kalman filter to update its state. The time difference value refers to the time interval between the actual observation time and the target fusion calculation time; its magnitude determines the amount of carrier displacement that needs to be compensated. In practice, the execution entity can extract the system absolute timestamp from the asynchronous observation data, simultaneously obtain the currently waiting or upcoming preset fusion calculation time, and then subtract the two to obtain the time difference value. For example, assuming the system's fusion calculation period is set to 50 milliseconds, and the fusion calculation time sequence is millisecond 0, millisecond 50, millisecond 100, millisecond 150, and so on, if the timestamp of a certain asynchronous observation data shows it as millisecond 123, then its next fusion calculation time is millisecond 150, and the time difference between the two is 27 milliseconds. The executing entity associates and stores this 27-millisecond time difference with the asynchronous observation data, using it as an input parameter for subsequent steps.

[0028] Step two involves acquiring the corresponding driving input data for each asynchronous observation based on the aforementioned time differences. The driving input data refers to the raw motion measurement values ​​output by the inertial measurement unit (IMU) within the time period covered by the corresponding time difference. An IMU typically includes three gyroscopes and three accelerometers. The gyroscopes output the angular increment (i.e., change in angle) of the carrier within the sampling interval, while the accelerometers output the velocity increment (i.e., change in velocity) of the carrier within the sampling interval. The output frequencies of these measurements (e.g., 200 Hz, 500 Hz) are much higher than the fusion calculation cycle, enabling a detailed description of the carrier's angular and linear motion within a short timeframe. This is the direct driving source for extrapolating high-frequency motion trajectories. In practice, after obtaining the time difference of a certain asynchronous observation, the execution entity needs to retrieve all IMU data whose timestamps fall within the interval from the observation time to the observation time plus the time difference from the IMU's data buffer. These data are arranged chronologically, with each data point containing a set of angular increment values ​​and a set of velocity increment values. The executing entity organizes this data sequentially and encapsulates it into the driving input data corresponding to the asynchronous observation data. For example, for the aforementioned 27-millisecond time difference, if the output frequency of the inertial measurement unit (IMU) is 200 Hz (i.e., outputting a set of data every 5 milliseconds), the driving input data may contain 6 sets of IMU data from the 123rd millisecond to the 150th millisecond (the exact number depends on the precise alignment of the timestamps). The executing entity then compiles this data into a list for use in the next step of trajectory extrapolation.

[0029] Step 3: Based on the aforementioned driving input data, generate the corresponding high-frequency motion trajectories for each asynchronous observation data. The high-frequency motion trajectory refers to the continuous motion process that starts from the actual observation moment's vehicle motion state, uses the angle and velocity increments in the driving input data as drivers, and gradually deduces the vehicle's attitude, velocity, and position changes within each tiny time step using a high-frequency numerical integration method, ultimately reaching the fusion solution moment. The vehicle's motion state includes attitude (i.e., the orientation of the vehicle coordinate system relative to the navigation coordinate system), velocity (the vehicle's three-dimensional velocity in the navigation coordinate system), and position (the vehicle's three-dimensional coordinates in the navigation coordinate system). The high-frequency numerical integration method is a mathematical method that iteratively calculates subsequent states from known initial states and input quantities; a commonly used method is the fourth-order Runge-Kutta method. In practice, the execution entity first uses the inertial navigation solution results at the asynchronous observation moment as the initial state. This initial state is accumulated in previous fusion solution cycles and includes the attitude matrix, velocity vector, and position vector at that moment. Then, the execution entity sequentially processes each set of angle and velocity increments in the driving input data. For each set of inputs, the execution agent substitutes them into the attitude update algorithm, velocity update algorithm, and position update algorithm, and gradually updates the carrier's attitude, velocity, and position through multiple iterations. This process is repeated with an integration step size smaller than the output period of the inertial measurement unit (IMU) (e.g., 1 millisecond) until the integration progresses to the fusion solution time. Throughout the integration process, the state values ​​calculated at each intermediate time point constitute a high-frequency motion trajectory. For example, for a time difference of 27 milliseconds and an integration step size of 1 millisecond, the execution agent needs to iterate 27 times to obtain a carrier state sequence every 1 millisecond from the 123rd millisecond to the 150th millisecond. This sequence describes the carrier's fine motion trajectory during this period.

[0030] Step four involves compensating and correcting the asynchronous observation data based on the aforementioned high-frequency motion trajectories to obtain synchronous observation data. This compensation and correction process uses the difference between the endpoint state and the starting state of the high-frequency motion trajectory to correct the original asynchronous observations, making them equivalent to the results obtained from observations performed at the fusion calculation time. Specifically, for position-related observations (such as the position output by the Global Navigation Satellite System), the compensation amount is the endpoint position minus the starting position; for velocity-related observations (such as the velocity output by the Global Navigation Satellite System or the velocity output by the visual odometry), the compensation amount is the endpoint velocity minus the starting velocity. The synchronous observation data are the observations obtained after the above corrections, corresponding to the fusion calculation time. In practice, the executing entity first extracts the starting state (i.e., the attitude, velocity, and position at the actual observation time) and the endpoint state (i.e., the attitude, velocity, and position at the fusion calculation time) from the generated high-frequency motion trajectory. Then, the appropriate compensation method is selected based on the type of asynchronous observation data: If the original observation value is position, the executing entity subtracts the starting point's position from the end point's position to obtain the position change, and then adds this change to the original position observation value to obtain the synchronous observation position; if the original observation value is velocity, the executing entity subtracts the starting point's velocity from the end point's velocity to obtain the velocity change, and then adds this change to the original velocity observation value to obtain the synchronous observation velocity; if the original observation value is distance (such as the ranging value between the tag and the base station output by an ultra-wideband positioning system), the executing entity needs to calculate the carrier's displacement vector based on the position difference between the starting and ending points, and then project this displacement vector onto the line connecting the base station and the tag to compensate for the original distance value. Finally, the executing entity associates the compensated and corrected observation values ​​with the corresponding fusion calculation time to form synchronous observation data, and stores it in the synchronous observation data queue, waiting for subsequent fusion calculation processing. After all the asynchronous observation data from all observation sources have undergone the above compensation and correction, a set of synchronous observation data that are strictly aligned in time can be obtained.

[0031] Step 103: Perform a dynamic confidence assessment on each synchronous observation data to obtain each dynamic confidence factor.

[0032] In some embodiments, the aforementioned execution entity can perform dynamic confidence assessment on the various synchronous observation data to obtain various dynamic confidence factors. Dynamic confidence assessment refers to the process of selecting multiple assessment indicators that reflect the current reliability of each navigation source (observation source) based on its real-time operating status and signal quality, and generating a value that dynamically characterizes the current reliability of the observation source through quantification, scoring, and fusion processing of these indicators. The dynamic confidence factor is the final generated value between 0 and 1 (or after normalization). A larger value indicates that the current observation value of the observation source is more reliable, while a smaller value indicates that it is less reliable or of lower quality. This factor will be used to dynamically adjust the observation noise matrix of the Kalman filter in subsequent steps. In practice, after obtaining the various synchronous observation data, the aforementioned execution entity extracts the relevant measured values ​​of quality indicators from the synchronous observation data of each observation source according to its type, and then transforms and fuses these indicators according to a preset assessment model, ultimately outputting a dynamic confidence factor for each observation source. For example, for a Global Navigation Satellite System receiver, the operator can extract the number of visible satellites, accuracy factor, and average signal-to-noise ratio from its synchronous observation data, and then calculate the corresponding dynamic confidence factor based on a pre-calibrated mapping relationship.

[0033] In some optional implementations of certain embodiments, the aforementioned executing entity may perform dynamic confidence assessment on the aforementioned synchronous observation data through the following steps to obtain various dynamic confidence factors: Step 1: For the synchronous observation data corresponding to the Global Navigation Satellite System receiver, perform the following steps: Sub-step one involves obtaining the measured values ​​of the number of visible satellites, the accuracy factor, and the average signal-to-noise ratio. The measured number of visible satellites refers to the number of satellites whose signals the Global Navigation Satellite System (GNSS) receiver can receive and use for positioning calculations at the current moment. This value is usually directly output by the receiver and is a fundamental indicator of positioning availability. The measured accuracy factor is a dimensionless value reflecting the impact of the current satellite geometric distribution on positioning accuracy. It typically includes the position accuracy factor (PDOP), horizontal accuracy factor (HDOP), and vertical accuracy factor (VDOP). A smaller value indicates a better satellite geometric distribution and higher positioning accuracy. The measured average signal-to-noise ratio (SNR) is the average of the signal-to-noise ratios (SNR) of all visible satellite signals currently tracked by the receiver. SNR is a commonly used indicator of satellite signal strength, usually expressed in dB-Hz. A higher value indicates better signal quality. In practice, when the aforementioned implementing entity receives synchronous observation data output from the Global Navigation Satellite System (GNSS) receiver, it can parse the current measured values ​​of the three indicators from the standard message format of the data (such as the GGA, GSA, and GSV statements in the NMEA-0183 protocol). For example, the PDOP value can be parsed from the GSA statement, the signal-to-noise ratio of each satellite can be obtained from the GSV statement and its average value can be calculated, and the number of satellites participating in the positioning can be obtained from the GGA statement. The implementing entity temporarily stores these measured values ​​for subsequent sub-confidence calculations.

[0034] Sub-step two involves generating a sub-confidence score for the number of visible satellites based on the measured values. This sub-confidence score refers to the local reliability of the observation source assessed solely based on the number of visible satellites, typically a value between 0 and 1. In practice, the executing entity can compare the measured number of visible satellites with several preset threshold intervals. These threshold intervals are derived from statistical analysis of extensive experimental data and reflect the correlation between the number of visible satellites and positioning error. For example, the preset threshold intervals are: 1.0 for a sub-confidence score when the number of visible satellites is greater than or equal to 12; 0.1 for a sub-confidence score when the number of visible satellites is less than or equal to 4; and a linear interpolation method is used to calculate the sub-confidence score when the number is between 4 and 12. The executing entity calculates the corresponding sub-confidence score for the number of visible satellites based on which interval the current measured value falls into, using a lookup table or interpolation formula.

[0035] Sub-step three involves generating the precision factor sub-confidence score based on the measured precision factor values. The precision factor sub-confidence score refers to the local confidence level of the observation source assessed solely based on the precision factor. In practice, the executing entity compares the measured precision factor values ​​with several preset precision factor threshold intervals. For example, the preset threshold intervals are: when PDOP is less than or equal to 2, the precision factor sub-confidence score is 1.0; when PDOP is greater than or equal to 5, the precision factor sub-confidence score is 0.1; and when PDOP is between 2 and 5, it is calculated using linear interpolation. The executing entity calculates the corresponding precision factor sub-confidence score based on the measured PDOP values.

[0036] Sub-step four involves generating an average signal-to-noise ratio (SNR) sub-confidence score based on the measured average SNR value. The average SNR sub-confidence score refers to the local confidence level of the observed source assessed solely based on the average SNR. In practice, the executing entity compares the measured average SNR value with several preset SNR threshold intervals. For example, the preset threshold intervals are: an average SNR sub-confidence score of 1.0 when the average SNR is greater than or equal to 50 dB-Hz; an average SNR sub-confidence score of 0.1 when the average SNR is less than or equal to 30 dB-Hz; and intermediate values ​​are calculated using linear interpolation. The executing entity then calculates the average SNR sub-confidence score accordingly.

[0037] Sub-step five involves weighted summation and fusion of the aforementioned visible satellite quantity sub-confidence, accuracy factor sub-confidence, and average signal-to-noise ratio sub-confidence to obtain the dynamic confidence factor corresponding to the Global Navigation Satellite System receiver. The weighted summation and fusion process involves multiplying each sub-confidence by its respective weight coefficient and then summing the results to obtain a comprehensive evaluation value. The weight coefficients are predetermined during the system calibration phase by analyzing the correlation between each indicator and the final positioning error, reflecting the different importance of different indicators in assessing overall reliability. In practice, the execution entity reads three pre-set weight values ​​from memory (e.g., visible satellite quantity weight 0.4, accuracy factor weight 0.4, average signal-to-noise ratio weight 0.2), then multiplies each of the three sub-confidences by its corresponding weight, and finally sums the products to obtain the final dynamic confidence factor. For example, if the three sub-confidence levels are 0.8, 0.7, and 0.9 respectively, then the dynamic confidence factor = 0.4 × 0.8 + 0.4 × 0.7 + 0.2 × 0.9 = 0.78. This factor will serve as a reliability measure of the Global Navigation Satellite System receiver within the current fusion calculation cycle and will be used in subsequent steps.

[0038] Step two, for the synchronous observation data corresponding to the visual odometry, perform the following steps: Sub-step one involves obtaining the measured values ​​of the number of successfully tracked image feature points and the average reprojection error. The measured value of the number of successfully tracked image feature points refers to the number of feature points in the current frame that the visual odometry can successfully match and track from the previous frame. Feature points are corner points or spots in an image that possess significant texture information; their quantity reflects the richness of the image texture and the operational stability of the visual odometry. The measured value of the average reprojection error refers to the average pixel distance between the reprojected 3D map points onto the image plane based on the currently estimated camera pose and the original detected feature point positions. The smaller this error, the more accurate the current pose estimation. In practice, the aforementioned execution entity extracts the current measured values ​​of these two indicators from the synchronous observation data output by the visual odometry. For example, the visual odometry software module typically outputs the number of tracked feature points and the average reprojection error in the current frame along with the pose estimation. The execution entity uses these values ​​as input for subsequent calculations.

[0039] Sub-step two involves generating a sub-confidence score based on the measured number of successfully tracked image feature points. The sub-confidence score refers to the local reliability of visual odometry assessed solely based on the number of feature points. In practice, the executing entity compares the measured number of feature points with two preset threshold values. These two thresholds are called the first threshold (high threshold) and the second threshold (low threshold). When the number of feature points is higher than the first threshold, the sub-confidence score is set to its maximum value of 1.0; when the number of feature points is lower than the second threshold, the sub-confidence score is set to its minimum value of 0.1; when the number of feature points is between the two, linear interpolation is used to calculate the sub-confidence score. For example, if the first threshold is 200, the second threshold is 50, and the current number of feature points is 120, then the sub-confidence score = 0.1 + (120-50) / (200-50)×(1.0-0.1) = 0.52.

[0040] Sub-step three involves generating an error sub-confidence score based on the measured average reprojection error. The error sub-confidence score refers to the local reliability of the visual odometry assessed solely based on the average reprojection error. In practice, the executing entity compares the measured average reprojection error with two preset error thresholds. These two thresholds are called the first error threshold (low threshold) and the second error threshold (high threshold). When the average reprojection error is lower than the first error threshold, the error sub-confidence score is set to its maximum value of 1.0; when the average reprojection error is higher than the second error threshold, the error sub-confidence score is set to its minimum value of 0.1; when the error value is between the two, linear interpolation is used to calculate the error sub-confidence score. For example, if the first error threshold is 0.5 pixels, the second error threshold is 2.5 pixels, and the current average reprojection error is 1.2 pixels, then the error sub-confidence score = 1.0 - (1.2-0.5) / (2.5-0.5)×(1.0-0.1) = 0.685.

[0041] Sub-step four involves performing a first multiplication fusion process on the aforementioned quantity sub-confidence and error sub-confidence to obtain the dynamic confidence factor corresponding to the visual odometry. This first multiplication fusion process involves directly multiplying the two sub-confidences to obtain the final comprehensive confidence. This multiplication relationship embodies the "one-vote veto" principle: a low confidence level in any one sub-sub-confidence will lead to a sharp decrease in the final product, reflecting the extremely low overall reliability of the visual odometry when there are too few feature points or too large reprojection errors. In practice, the executing entity multiplies the quantity sub-confidence and error sub-confidence generated in steps two and three to obtain the dynamic confidence factor corresponding to the visual odometry. For example, if the quantity sub-confidence is 0.52 and the error sub-confidence is 0.685, then the dynamic confidence factor = 0.52 × 0.685 ≈ 0.356. This factor will be used to adjust the noise weight of the visual odometry observations in subsequent steps.

[0042] Step 3: For the synchronous observation data corresponding to the ultra-wideband positioning system, perform the following steps: Sub-step one involves obtaining the measured value of the Received Signal Strength Indicator (RSSI) and the measured value of the rate of change between two adjacent ranging results. The measured RSSI refers to the signal strength indication received by the mobile tag from the base station in the ultra-wideband positioning system, typically measured in dBm. It reflects the attenuation of the signal during propagation; a smaller absolute value (i.e., a larger numerical value) indicates a stronger signal and higher ranging reliability. The measured rate of change between two adjacent ranging results is the ratio of the difference between the current ranging value and the previous ranging value to the time interval, typically measured in meters per second (m / s). It reflects the stability of the ranging value; an excessively large rate of change may indicate non-line-of-sight interference or abnormal tag movement. In practice, the aforementioned execution entity extracts the RSSI value from the synchronous observation data output by the ultra-wideband positioning system and calculates the rate of change between the current ranging value and the previous ranging value. The execution entity typically maintains a cache storing the previous ranging value. Each time a new ranging value is received, the difference is calculated and divided by the time interval to obtain the measured rate of change.

[0043] Sub-step two involves generating a strength sub-confidence score based on the measured RSSI value. The strength sub-confidence score refers to the local reliability of the ultra-wideband positioning system assessed solely based on the RSSI indicator. In practice, the executing entity compares the measured RSSI value with several preset strength threshold intervals. Since RSSI is typically negative (e.g., -90dBm, -80dBm), a smaller absolute value indicates a stronger signal. For example, the preset threshold intervals are: a strength sub-confidence score of 1.0 when RSSI is greater than or equal to -80dBm; a strength sub-confidence score of 0.1 when RSSI is less than or equal to -100dBm; and intermediate values ​​are calculated using linear interpolation. The executing entity calculates the strength sub-confidence score based on the current RSSI value.

[0044] Step three involves generating a rate-of-change sub-confidence score based on the measured values ​​of the rate of change between two consecutive ranging results. The rate-of-change sub-confidence score refers to the local reliability of the ultra-wideband positioning system assessed solely based on the ranging rate of change. In practice, the executing entity first compares the measured rate of change with a preset rate-of-change threshold. The rate-of-change threshold is an empirical value, such as 3 meters per second, representing the maximum reasonable rate of change of the ranging value under normal movement conditions. If the measured rate of change is lower than or equal to this threshold, the ranging is considered stable, and the rate-of-change sub-confidence score is set to its maximum value of 1.0. If the measured rate of change is higher than this threshold, it indicates that there may be abnormal fluctuations in the ranging. In this case, a decay function is needed to calculate the sub-confidence score, so that the higher the rate of change, the lower the sub-confidence score. For example, the decay function can be defined as an exponential decay form: Rate-of-change sub-confidence score = exp(-α × (measured rate of change - rate-of-change threshold)), where α is the decay coefficient. The executing entity outputs the calculated rate-of-change sub-confidence score.

[0045] Sub-step four involves performing a second multiplication and fusion process on the aforementioned intensity sub-confidence and rate of change sub-confidence to obtain the dynamic confidence factor corresponding to the ultra-wideband positioning system. This second multiplication and fusion process also employs a multiplication method to comprehensively reflect the impact of signal strength and ranging stability on overall reliability. In practice, the executing entity multiplies the intensity sub-confidence and rate of change sub-confidence to obtain the dynamic confidence factor corresponding to the ultra-wideband positioning system. For example, if the intensity sub-confidence is 0.8 and the rate of change sub-confidence is 0.6, then the dynamic confidence factor = 0.8 × 0.6 = 0.48. This factor will be used in subsequent steps to adjust the noise weights of the ultra-wideband positioning system observations.

[0046] Step 104: Adaptively compensate and adjust the statistical characteristics of each dynamic confidence factor and each historical information sequence to obtain the observation noise matrix of each target.

[0047] In some embodiments, the execution entity can adaptively compensate and adjust the statistical characteristics of each dynamic confidence factor and each historical news sequence to obtain each target observation noise matrix. Adaptive compensation and adjustment refers to a fusion weight determination process that combines active adjustment based on real-time confidence and anomaly compensation based on historical statistical characteristics. This process first uses dynamic confidence factors to scale a preset benchmark observation noise matrix to achieve adaptive matching of the observation noise; then, it uses the statistical characteristics of the historical news sequences of each observation source to perform anomaly detection and applies additional penalties to the detected anomaly observation sources, ultimately obtaining the target observation noise matrix used for the current fusion solution cycle. The target observation noise matrix refers to the observation noise covariance matrix that is finally determined after the above adaptive compensation and adjustment and will be substituted into the Kalman filter observation update equation; it accurately reflects the actual confidence level of each observation source at the current moment. In practice, after obtaining each dynamic confidence factor and the statistical characteristics of each historical news sequence, the execution entity processes them sequentially according to the following steps, ultimately generating a corresponding target observation noise matrix for each observation source. These matrices will be used for subsequent fusion solutions.

[0048] In some optional implementations of certain embodiments, the aforementioned execution entity can adaptively compensate and adjust the statistical characteristics of each dynamic confidence factor and each historical information sequence through the following steps to obtain each target observation noise matrix: Step one involves scaling the preset benchmark observation noise matrices based on the aforementioned dynamic confidence factors, resulting in pre-adjusted observation noise matrices. The benchmark observation noise matrix is ​​the observation noise covariance matrix pre-set during system initialization based on the factory nominal accuracy or empirical values ​​of each observation source. It represents a priori estimate of the measurement uncertainty of that observation source under ideal conditions. Scaling refers to multiplying all elements of the benchmark observation noise matrix by a scaling factor, thereby enlarging or reducing the noise matrix as a whole. The scaling factor is determined by the dynamic confidence factors through a preset scaling function. The pre-adjusted observation noise matrix is ​​the observation noise matrix obtained after scaling with the confidence factors, before undergoing anomaly compensation processing; it already reflects the weight adjustments based on real-time signal quality. In practice, the aforementioned execution entity first needs to preset a benchmark observation noise matrix for each observation source. For example, for position observations by a Global Navigation Satellite System (GNSS) receiver, if its nominal horizontal positioning accuracy is 3 meters (standard deviation) and vertical accuracy is 5 meters, and assuming that the errors in each direction are uncorrelated, the reference observation noise matrix can be set as a 3×3 diagonal matrix, with diagonal elements of 3², 3², and 5². Then, the executing entity calculates a scaling factor based on the current dynamic confidence factor of the observation source using a predetermined scaling function. This scaling function is typically designed such that: when the confidence factor is at its maximum value of 1, the scaling factor is at its minimum value of 1 (i.e., complete trust, no noise amplification); when the confidence factor decreases, the scaling factor increases non-linearly; when the confidence factor approaches 0, the scaling factor approaches a preset maximum value (e.g., 100), indicating almost no trust. For example, the scaling function can be defined as S = 1 + (K-1)×(1-c)^n, where c is the confidence factor, K is the maximum scaling factor (e.g., 100), and n is the non-linear exponent (e.g., 3). If the dynamic confidence factor c of an observation source is 0.7, then the scaling factor S = 1 + 99 × 0.3³ ≈ 3.67. The executing entity multiplies this scaling factor by each element of the reference observation noise matrix to obtain the preliminarily adjusted observation noise matrix. For example, if the reference matrix is ​​diag(9,9,25), multiplying it by 3.67 yields diag(33.03,33.03,91.75). This preliminarily adjusted matrix reflects the noise level under the current signal quality.

[0049] Step two, for each observation source, perform the following steps: Sub-step one: Based on the statistical characteristics of the historical innovation sequences corresponding to the aforementioned observation sources, generate the sample mean and sample covariance matrix for each observation source. The statistical characteristics of the historical innovation sequences refer to the feature quantities obtained by statistically analyzing the innovation sequences of the observation source over a past period, typically calculated from the most recent innovation values ​​stored in a sliding window. The innovation sequence refers to the sequence of differences between the actual observed values ​​and the predicted values ​​based on the predicted state in Kalman filtering. The sample mean is the arithmetic mean of these historical innovations, reflecting the systematic bias of the innovations. The sample covariance matrix is ​​the covariance matrix of these historical innovations, reflecting the fluctuation range and correlation of the innovations. In practice, the execution entity maintains a fixed-length sliding window for each observation source, storing the innovation sequences generated in the most recent N fusion calculation cycles for that observation source (e.g., N=30). When entering the current fusion calculation cycle, the execution entity reads all the innovation data within the sliding window corresponding to that observation source from memory and then calculates the sample mean and sample covariance matrix of these data. The sample mean is calculated by averaging each innovation component separately; the sample covariance matrix, on the other hand, needs to consider the correlation between the components. For example, for a three-dimensional location innovation, the sample mean is a three-dimensional vector, and the sample covariance matrix is ​​a 3×3 matrix, where the diagonal elements are the variances of the innovations in each direction, and the off-diagonal elements are the covariances of the innovations in different directions. These statistics will serve as the baseline characteristics for the normal operation of the observation source and will be used for subsequent anomaly detection.

[0050] Sub-step two involves generating anomaly detection results for the observed sources based on the aforementioned sample mean and sample covariance matrix. The anomaly detection result refers to the conclusion used to determine whether there are anomalous errors in the current observation, typically expressed as a Boolean value (yes / no anomaly) or anomaly level. This result is generated by comparing the current innovation with historical statistical characteristics to quantify the degree to which the current innovation deviates from the normal pattern. In practice, after obtaining the sample mean and sample covariance matrix, the executing entity also needs to obtain the current innovation sequence to be detected. It should be noted that since step 104 occurs before the Kalman filter update in the current fusion solution cycle, the current innovation sequence for the current cycle has not yet been generated. Therefore, the current innovation sequence used here actually refers to the latest one from the historical innovation sequences generated in the previous fusion solution cycle and already stored. The executing entity reads the latest innovation sequence from the historical innovation sequence sliding window of the observed source as the current innovation to be detected. Then, the executing agent calculates the deviation of the current innovation from historical statistical characteristics, for example, by calculating the Mahalanobis distance (a distance metric that considers the covariance structure) between the current innovation vector and the sample mean, and compares this distance with a preset dynamic detection threshold. The dynamic detection threshold is usually determined based on a chi-square distribution table, with its degrees of freedom equal to the dimension of the innovation vector, and its significance level is preset according to the system's requirements for false positive and false negative rates (e.g., 0.01). If the calculated Mahalanobis distance exceeds this threshold, it is preliminarily judged that the current innovation may be anomalous. However, to increase the robustness of detection, a strategy of confirming anomaly only after multiple consecutive exceedances is typically adopted. For example, the executing agent records the number of consecutive exceedances for each observation source; when the number of consecutive exceedances reaches a preset number (e.g., 3 times), a detection result indicating anomaly is generated; otherwise, a detection result indicating no anomaly is generated. This detection result will be used to determine whether to impose a penalty on the noise matrix.

[0051] Sub-step three involves penalizing the pre-adjusted observation noise matrix corresponding to the observed source in response to the anomaly detection results. The penalized observation noise matrix is ​​then designated as the target observation noise matrix. Penalization refers to further amplifying the observation noise matrix to significantly reduce its weight in the Kalman filter when a persistent anomaly occurs, thus preventing abnormal data from contaminating the state estimation. The penalized observation noise matrix is ​​the noise matrix amplified by the penalty factor. In practice, when the anomaly detection results generated in sub-step two determine that an observed source is abnormal, the execution entity immediately triggers the penalty mechanism. First, the execution entity obtains the pre-adjusted observation noise matrix used by the observed source in the current period. Then, a penalty factor greater than 1 is dynamically determined based on the severity of the anomaly (e.g., the number of consecutive exceedances). The penalty factor can be designed to grow exponentially with the number of consecutive exceedances, for example, penalty factor = 1.5^(number of consecutive exceedances). The penalty factor typically has an upper limit, such as a maximum of 10, to prevent the noise matrix from being infinitely amplified due to a single anomaly, thus completely blocking the observed source. Simultaneously, when the number of consecutive out-of-limit occurrences of an observation source returns to zero (i.e., returns to normal), the penalty factor is reset to 1, and no further penalty is applied. The condition for resetting the number of consecutive out-of-limit occurrences to zero can be: when an observation source is not detected as abnormal multiple times (e.g., 3 times), its consecutive out-of-limit occurrences are reset to 0. Next, the executing entity multiplies the penalty factor by each element of the initially adjusted observation noise matrix to obtain the penalized observation noise matrix. Finally, the executing entity determines this penalized observation noise matrix as the target observation noise matrix used by the observation source in the current fusion solution cycle. For example, if an observation source has exceeded the limit 3 times consecutively, the penalty factor = 1.5^3 = 3.375, and the initially adjusted observation noise matrix is ​​R_adj, then the target observation noise matrix R_target = 3.375 × R_adj. In this way, the weight of this observation source in the Kalman filter update will be significantly reduced.

[0052] Sub-step four: In response to the anomaly detection results indicating the absence of anomalies, the preliminarily adjusted observation noise matrix corresponding to the observed source is determined as the target observation noise matrix. That is, for observation sources where no anomalies are detected, no penalty is applied; their preliminarily adjusted observation noise matrix is ​​directly used as the final target observation noise matrix. Through these steps, the executing entity generates a target observation noise matrix for each observation source that considers both real-time signal quality (scaled by a dynamic confidence factor) and historical anomaly patterns (penalized based on innovation statistics). These matrices will be used in the fusion and resolution processing in step 105.

[0053] Step 105: Based on the noise matrix of each target observation, perform fusion processing on each synchronous observation data to obtain the navigation state estimate and each current information sequence.

[0054] In some embodiments, the aforementioned execution entity can perform fusion processing on the aforementioned synchronous observation data based on the aforementioned target observation noise matrices to obtain navigation state estimates and various current information sequences. The fusion processing refers to the process of optimally fusing multiple time-synchronized observation source data (i.e., various synchronous observation data) with the prediction information of the inertial navigation system using a Kalman filter and its variants, thereby obtaining a more accurate and reliable estimate of the vehicle's motion state. The navigation state estimate refers to the motion state parameters of the vehicle at the current moment obtained after fusion processing, typically including the vehicle's three-dimensional position, three-dimensional velocity, three-dimensional attitude, and relevant error terms of the inertial measurement unit (such as gyroscope bias, accelerometer bias, etc.). The current information sequence refers to the difference vector between the synchronous observation data of each observation source and its corresponding predicted observation value during this fusion processing. This difference reflects the degree of deviation between the actual observation and the expected observation and will be used for anomaly detection and historical statistical characteristic updates in the next cycle. In practice, after obtaining the target observation noise matrices generated in step 104 and the synchronous observation data generated in step 102, the aforementioned execution entity performs iterative calculations according to the prediction-update process of standard Kalman filtering. Kalman filtering is a recursive estimation algorithm that continuously fuses new observation information by alternately executing two steps: state prediction (based on the system model) and observation update (based on actual observations), thereby achieving an optimized estimate of the system state. The following six steps describe this process in detail.

[0055] In some optional implementations of certain embodiments, the aforementioned execution entity may perform fusion processing on the aforementioned synchronous observation data based on the aforementioned target observation noise matrices through the following steps to obtain navigation state estimates and various current information sequences: Step one: Obtain the navigation state estimate corresponding to the previous fusion calculation cycle. This estimate refers to the vehicle motion state parameters that were finally determined and stored after complete prediction and update steps at the previous fusion calculation moment. This estimate includes the vehicle's optimal position, velocity, attitude, and various error states at the previous moment, serving as the initial basis for the current cycle's state prediction. In practice, the execution entity reads the navigation state estimate saved at the end of the previous fusion calculation cycle from memory. In the first cycle after system startup, this value may come from the initial state set during the system initialization phase (e.g., through static alignment or external input). The execution entity uses this estimate as the starting point for the current cycle's state prediction. For example, the vehicle's estimated position in the previous cycle was 100.0 meters east, 50.0 meters north, and 2.0 meters up; its speed was 5.0 meters per second east, 0.0 meters per second north, and 0.0 meters per second up; its attitude angles were 30 degrees yaw, 0 degrees pitch, and 0 degrees roll; and its gyro bias estimate was 0.01 degrees per second. These values ​​are then incorporated into the calculation process for the current cycle.

[0056] Step two involves performing state prediction processing on the navigation state estimate corresponding to the previous fusion solution cycle to obtain the predicted state and the state prediction covariance matrix. State prediction processing refers to the process of calculating the predicted state of the carrier at the current moment using the system's state transition model (i.e., the equations describing the carrier's motion, usually based on the angular and velocity increments output by the inertial measurement unit) and the state estimate from the previous cycle. The predicted state refers to the estimated carrier state at the current moment obtained after state prediction processing, but without incorporating current observation information for correction. The state prediction covariance matrix is ​​a matrix that quantifies the uncertainty of the predicted state; it reflects the accumulated error based on the system model. Its diagonal elements represent the prediction variance of each state quantity, while the off-diagonal elements reflect the correlation of prediction errors between different state quantities. In practice, the execution entity first reads all inertial measurement data (angular and velocity increments) from the inertial measurement unit's data cache from the previous fusion solution moment to the current fusion solution moment. Then, it uses this data to drive the state transition model to perform integration on the navigation state estimate from the previous cycle. The specific integration method is similar to the method for deriving the high-frequency motion trajectory in step 102, but the goal of integration here is to obtain the complete state transition from the previous moment to the current moment, rather than just short-term compensation for a certain asynchronous observation. After the state transition is completed, the executing entity also needs to calculate the state prediction covariance matrix, which usually requires the system's process noise matrix (describing the uncertainty of the model itself) and the state transition matrix (describing the propagation relationship between state variables). For example, the executing entity integrates the position, velocity, and attitude from the previous moment to the current moment using a strapdown inertial navigation algorithm, and simultaneously calculates the covariance matrix of the predicted position, velocity, and attitude values ​​based on the noise characteristics of the inertial measurement unit and the integration time. Finally, a set of predicted states (such as predicted position, predicted velocity, and predicted attitude) and a corresponding prediction covariance matrix are obtained.

[0057] Step 3: Based on the aforementioned target observation noise matrices and state prediction covariance matrices, generate the Kalman gains for each observation source. The Kalman gain is a matrix that determines the weight distribution between the predicted state and the actual observed values ​​during the observation update process. The magnitude of the Kalman gain depends on the uncertainty of the prediction (reflected by the state prediction covariance matrix) and the uncertainty of the observation (reflected by the target observation noise matrix): if the prediction is very accurate (small covariance) but the observation noise is large, the Kalman gain will be small, making the update more dependent on the prediction; conversely, if the prediction uncertainty is large but the observation noise is small, the Kalman gain will be large, making the update more dependent on the observation. Since each observation source has a different observation model and a different target observation noise matrix, it is necessary to calculate the corresponding Kalman gain for each observation source separately. In practice, the execution entity first obtains the observation matrix corresponding to each observation source. The observation matrix is ​​a mathematical expression describing the linear or nonlinear relationship between the observed values ​​(such as position, velocity, and distance) and the system state vector. The observation matrix is ​​predetermined based on the measurement model of the observation source. For example, for a global navigation satellite system receiver that directly outputs position observations, if the system state vector contains three-dimensional position components, the observation matrix can be a selection matrix, whose function is to extract the position components from the state vector. For an ultra-wideband positioning system that outputs range observations, the observation matrix is ​​nonlinear and needs to be linearized according to the geometric relationship between the carrier position and the base station position, usually in the form of a Jacobian matrix. These observation matrices are pre-defined in the system design phase based on sensor characteristics and state vector definitions, and stored in memory for real-time retrieval. Then, the execution entity uses the standard Kalman filter gain calculation formula, substituting the state prediction covariance matrix, the observation matrix, and the target observation noise matrix corresponding to the observation source, to calculate the Kalman gain of the observation source. This calculation process involves matrix multiplication and inversion operations. For example, for a certain observation source, if the state prediction covariance matrix is ​​P, the observation matrix is ​​H, and the target observation noise matrix is ​​R, then the Kalman gain K = P × H^T × (H × P × H^T + R)^(-1). The execution entity repeats this calculation for each available observation source to obtain a set of Kalman gain matrices.

[0058] Step four involves generating predicted observation values ​​based on the aforementioned predicted states and pre-defined observation matrices. These predicted observation values ​​are calculated based on the current predicted state and the observation model of the observation source, representing the values ​​that should be observed at the current moment. In other words, they are estimates of the actual observed values ​​based on the state prediction. In practice, the executing entity calculates the predicted observation values ​​for each observation source using the observation matrix of that source and the predicted state generated in step two. For example, for position observations of a Global Navigation Satellite System receiver, the predicted observation values ​​are directly extracted from the predicted state, representing the predicted position (east, north, and celestial coordinates). For distance observations of an ultra-wideband positioning system, the predicted observation values ​​require calculating the predicted distance between the carrier and the base station based on the predicted carrier position in the predicted state and the known base station position. The executing entity compares the predicted observation values ​​for each observation source with subsequent actual observation values ​​to calculate information and update observations.

[0059] Step 5: Based on the Kalman gains and synchronous observation data mentioned above, the predicted state is updated to obtain the navigation state estimate corresponding to the current fusion solution cycle. The observation update process refers to correcting the predicted state using actual observations (i.e., synchronous observation data) and Kalman gains to obtain a more accurate state estimate. This is the core step of Kalman filtering, achieving the fusion of observation and prediction information. In practice, the execution entity processes each available observation source sequentially, or processes the data from all observation sources sequentially. For each observation source, the execution entity first calculates the innovation (described in detail in the next step), then multiplies the Kalman gain by the innovation to obtain a correction amount, which is then added to the predicted state to obtain the updated state. If multiple observation sources are available simultaneously, a sequential update method can be used: first update the predicted state with the first observation source to obtain an intermediate state; then update this intermediate state with the second observation source, and so on. After all observation sources have been processed, the final state obtained is the navigation state estimate corresponding to the current fusion solution cycle. This estimate incorporates information from all available observation sources, resulting in higher accuracy than when using any single observation source alone. For example, the agent first updates the position and velocity using observations from a Global Navigation Satellite System receiver, then further refines the velocity and attitude using observations from a visual odometry system, finally obtaining the fused position, velocity, and attitude estimates, while simultaneously updating the state covariance matrix to reflect the updated uncertainties.

[0060] Step Six: Based on the aforementioned synchronous observation data and the aforementioned predicted observation values, generate various current news sequences. The current news sequence refers to the difference vector between the actual observed value and the predicted observation value for each observation source during this fusion calculation. News reflects the degree of inconsistency between actual observations and predictions and is an important output of the Kalman filter. It is used both to measure the reasonableness of the observations and to update the statistical characteristics of historical news sequences in subsequent cycles. In practice, the executing entity calculates the news for each observation source simultaneously with or after generating the predicted observation values ​​in Step Four and processing the observation data in Step Five: News = Synchronous Observation Data - Predicted Observation Value. For example, for the position observations of a Global Navigation Satellite System receiver, the news is a three-dimensional vector representing the difference between the observed and predicted position values ​​in the east, north, and sky directions, respectively. The executing entity stores these news vectors according to the observation source, forming various current news sequences. These news sequences will be used in Step 106 to update the statistical characteristics of various historical news sequences, thereby providing a basis for anomaly detection in the next cycle. Meanwhile, the information itself can also be used to monitor the quality of observations in real time, for example, for anomaly detection in step 104.

[0061] Step 106: Update the statistical characteristics of each historical information sequence based on each current information sequence.

[0062] In some embodiments, the execution entity can update the statistical properties of the historical news sequences based on the current news sequences. The current news sequence refers to the news vector generated in step 105 for each observation source within the current fusion solution period, reflecting the difference between the actual observed value and the predicted value based on the predicted state. The statistical properties of the historical news sequences refer to the features obtained by statistically analyzing the accumulated news sequences of each observation source over a past period (usually in the form of a sliding window), mainly including the sample mean vector and sample covariance matrix of the news sequences. The sample mean vector reflects the average bias level of the news sequences, while the sample covariance matrix describes the fluctuation range of the news sequences and the correlation between their dimensions. Updating involves incorporating the current news sequence into the historical statistical window and recalculating the sample mean and sample covariance matrix, so that the historical statistical properties can track the recent trends of the news sequences in real time, thereby providing an accurate benchmark for anomaly detection and weight adjustment in the next period. In practice, the aforementioned execution entity maintains a fixed-length sliding window for each observation source, storing the innovation sequences from the most recent fusion solution cycles (e.g., the last 30 cycles). After obtaining the current innovation sequence for the current fusion solution cycle, the execution entity first adds it to the end of the sliding window for the corresponding observation source. If the window is full, the oldest innovation sequence is removed to maintain the window length. Then, based on all the updated innovation sequences within the window, the execution entity recalculates the sample mean vector and sample covariance matrix for that observation source. The sample mean is calculated by taking the arithmetic mean of each innovation component within the window; the sample covariance matrix is ​​calculated by considering the covariance relationships between the components, typically using an unbiased estimation formula. After calculation, the execution entity stores the updated sample mean vector and sample covariance matrix as the statistical properties of the latest historical innovation sequences for that observation source, for use in step 104 of the next cycle. For example, suppose an observation source (such as a Global Navigation Satellite System receiver) maintains a sliding window of length 30, storing the three-dimensional innovation sequence (eastward, northward, and celestial innovations) of the most recent 30 fusion calculation cycles. A new three-dimensional innovation vector, denoted as v_new, is generated in the current cycle. The execution entity adds v_new to the window and removes the oldest innovation vector in the window (assuming it's the first of the 30). The updated window contains the 2nd to 30th old innovations and v_new, for a total of 30 innovation vectors. The execution entity averages these 30 innovation vectors element-wise to obtain a new sample mean vector (e.g., eastward mean 0.02 meters, northward mean -0.01 meters, celestial mean 0.03 meters). Simultaneously, the execution entity calculates a 3×3 sample covariance matrix based on these 30 innovation vectors, obtaining the variance (e.g., eastward variance 0.15 meters²) and covariance (e.g., the covariance between eastward and northward is 0.02 meters²).These updated statistical properties will be saved as the statistical properties of the historical news sequences of this observation source in the next period. Through this continuous update mechanism, the historical statistical properties can dynamically adapt to changes in the news sequences, ensuring that the benchmark for anomaly detection always reflects recent normal observation patterns.

[0063] Building upon the aforementioned multi-source data synchronous inertial navigation method for addressing the high-precision navigation state estimation problem in the background technology, and considering the application scenario—in real-world driving environments such as autonomous vehicles, intelligent connected vehicles, and advanced driver assistance systems—the following technical problems often arise: When a vehicle performs trajectory tracking control based on a high-precision navigation state estimate, traditional control methods typically employ fixed-parameter PID controllers, which cannot adapt to real-time fluctuations in navigation signal quality. For example, when a vehicle enters a tunnel, urban canyon, or area with severe weather, the GNSS signal may temporarily lose lock or degrade, leading to a decrease in the confidence level of the navigation state estimate. However, the fixed-parameter controller still responds with the same gain to the tracking error at this time, potentially amplifying unreliable navigation information into drastic control actions, causing steering jitter, acceleration / deceleration shocks, or even vehicle instability. Furthermore, when the navigation signal recovers, the fixed parameters cannot respond quickly enough to fully utilize the high-quality information, resulting in a slow improvement in tracking accuracy. Based on the following requirements for this application scenario: robust control adaptability to complex and changing road environments, adaptive adjustment capability to cope with fluctuations in navigation signal quality, and long-term stability in maintaining accurate, stable, and safe trajectory tracking under different operating conditions, we have decided to adopt the following solution.

[0064] Step 107: Control the movement of the carrier based on the navigation state estimate.

[0065] In some embodiments, the aforementioned execution entity can control the motion of the vehicle based on the aforementioned navigation state estimate. The navigation state estimate refers to the high-precision motion state parameters of the vehicle at the current moment obtained through fusion calculation in step 105, typically including the vehicle's three-dimensional position, three-dimensional velocity, three-dimensional attitude, and related error terms. Controlling the motion of the vehicle refers to comparing the aforementioned navigation state estimate as feedback information with a preset target trajectory, generating control commands for the actuators through a certain control algorithm, driving actuator actions such as steering, throttle, and braking, enabling the vehicle to accurately track the predetermined trajectory and maintain stable driving. This step realizes a closed loop from perception fusion to physical execution, representing the ultimate value of multi-source navigation information in practical applications. In this embodiment, the aforementioned vehicle can specifically be a car, including but not limited to passenger cars, commercial vehicles, or special vehicles. In practice, the aforementioned execution entity is typically a central control unit (such as the domain controller of an autonomous vehicle) integrating navigation fusion and control functions. It receives high-precision state estimates from the navigation fusion module and, based on the pre-planned target trajectory, calculates specific actuator control quantities according to a preset control strategy, and sends the commands to each actuator through an in-vehicle network (such as a CAN bus). Since the navigation state estimate itself has already incorporated multi-source information and undergone confidence assessment and anomaly handling, the control commands generated based on this can better adapt to complex environments and improve the stability and safety of control.

[0066] In some optional implementations of certain embodiments, the aforementioned execution entity can control the movement of the carrier based on the aforementioned navigation state estimate through the following steps: The first step is to acquire the preset target trajectory of the vehicle. This target trajectory refers to the ideal motion path that the vehicle needs to follow over a future period, pre-calculated and issued by the upper-level planning module. This trajectory is typically represented as a time series, containing a series of discrete points, each corresponding to the target's position, speed, and attitude at a specific moment. It serves as a reference benchmark for the vehicle's motion, and the control system's task is to make the actual motion state as close as possible to this target trajectory. In practice, the aforementioned execution entity can obtain the latest target trajectory for the current period from the planning module through an internal communication interface. For example, when the vehicle is an autonomous vehicle, the planning module might plan a driving trajectory for the next 5 seconds every 100 milliseconds and send it to the control module as a series of waypoints. Each waypoint contains the target's eastward position, northward position, speed, and heading angle at that moment. The execution entity stores this data in the trajectory buffer used in the current control cycle.

[0067] The second step involves generating position tracking error, velocity tracking error, and attitude tracking error based on the aforementioned navigation state estimate and target trajectory. Position tracking error refers to the deviation vector between the vehicle's actual position at the current moment and the target position on the trajectory at the corresponding moment, typically decomposed into lateral position error (perpendicular to the trajectory direction) and longitudinal position error (along the trajectory direction). Velocity tracking error is the difference between the current actual velocity and the target velocity. Attitude tracking error is the angular deviation between the current actual attitude and the target attitude. These error quantities are the core inputs for subsequent control law calculations, reflecting the degree to which the vehicle deviates from the target trajectory. In practice, the executing entity first interpolates the target position, target velocity, and target attitude from the target trajectory based on the current timestamp. Then, it compares the actual position, actual velocity, and actual attitude from the current navigation state estimate with these target values. For example, when the vehicle is a car, the executing entity can perform coordinate transformation based on the car's kinematics model to decompose the position error into the vehicle coordinate system: the lateral position error typically refers to the vertical distance from the car's center of mass to the target trajectory, and the longitudinal position error refers to the difference in projected distance along the trajectory direction. Speed ​​tracking error is obtained directly by subtracting the target longitudinal speed from the actual longitudinal vehicle speed. Attitude tracking error is obtained by subtracting the target heading angle from the actual heading angle.

[0068] The third step involves performing a first adaptive adjustment on the preset lateral control proportional coefficient, lateral control integral coefficient, and lateral control derivative coefficient based on the aforementioned dynamic confidence factors. This yields the lateral adaptive proportional coefficient, lateral adaptive integral coefficient, and lateral adaptive derivative coefficient. The lateral control proportional coefficient, lateral control integral coefficient, and lateral control derivative coefficient refer to the PID controller parameters used for the lateral motion control of the vehicle. The proportional coefficient determines the response strength of the control quantity to the current lateral error, the integral coefficient is used to eliminate steady-state error, and the derivative coefficient is used to predict the error change trend and increase damping. The first adaptive adjustment process refers to the process of dynamically adjusting these control parameters according to the current dynamic confidence factors. When the confidence of the navigation state estimate is low, the control parameters are adjusted to more conservative values ​​to avoid control oscillations or instability due to unreliable state estimates. In practice, the aforementioned execution entity reads the preset lateral control reference parameters from memory. Then, a comprehensive confidence index is calculated based on the various dynamic confidence factors. Next, adjustment factors for each coefficient are generated using a preset adaptive function, and the baseline parameters are multiplied by the corresponding adjustment factors to obtain the lateral adaptive proportional coefficient, lateral adaptive integral coefficient, and lateral adaptive derivative coefficient. For example, when the vehicle is a car and the overall confidence level is low, the steering control action is correspondingly weakened, and the system performs lateral control in a more cautious manner.

[0069] The fourth step involves generating the desired front wheel steering angle based on the lateral position error in the aforementioned position tracking error, as well as the aforementioned lateral adaptive proportional coefficient, lateral adaptive integral coefficient, and lateral adaptive derivative coefficient. The desired front wheel steering angle refers to the angle value that the front wheel steering mechanism is expected to achieve in order for the vehicle to eliminate lateral position and heading deviations. It is the output of the lateral controller, typically calculated based on a PID control law and converted using the vehicle's kinematic or dynamic model. In practice, the aforementioned actuator uses a lateral PID control algorithm. First, the proportional, integral, and derivative terms of the lateral position error are calculated, and these three terms are added together to obtain a desired lateral control quantity. Then, based on the vehicle's kinematic model, this control quantity is converted into the corresponding desired front wheel steering angle. For example, when the vehicle is a car, at low speeds, the desired steering angle ≈ wheelbase × desired curvature. The desired curvature is usually obtained directly from the output of the lateral PID controller, i.e., desired curvature = proportional term + integral term + derivative term. When traveling at high speeds, the vehicle's dynamic characteristics also need to be considered. For example, the desired yaw rate can be converted into the front wheel steering angle using a linear two-degree-of-freedom model, or a more complex dynamic control law can be employed. Those skilled in the art can select an appropriate conversion model based on the actual application scenario; these models are common knowledge in the field of vehicle control. The executing entity uses the calculated desired front wheel steering angle as the command value for steering control.

[0070] The fifth step involves performing a second adaptive adjustment on the preset longitudinal control proportional coefficient, longitudinal control integral coefficient, and longitudinal control derivative coefficient based on the aforementioned dynamic confidence factors. This yields the longitudinal adaptive proportional coefficient, longitudinal adaptive integral coefficient, and longitudinal adaptive derivative coefficient. Here, the longitudinal control proportional coefficient, longitudinal control integral coefficient, and longitudinal control derivative coefficient refer to the PID controller parameters used for the longitudinal motion control of the vehicle. Similar to the third step, the second adaptive adjustment process dynamically adjusts the longitudinal control parameters based on the dynamic confidence factors, enabling the speed control strategy to adapt to changes in the reliability of the navigation state estimate. In practice, similar to the third step, the executing entity reads the preset longitudinal control reference parameters and calculates the adjustment factor based on the comprehensive confidence level to generate the longitudinal adaptive proportional coefficient, longitudinal adaptive integral coefficient, and longitudinal adaptive derivative coefficient.

[0071] Step 6: Based on the longitudinal velocity error in the aforementioned velocity tracking error, as well as the aforementioned longitudinal adaptive proportional coefficient, longitudinal adaptive integral coefficient, and longitudinal adaptive differential coefficient, the desired acceleration is generated. The desired acceleration refers to the longitudinal acceleration value that the carrier is expected to achieve in order for the actual speed of the carrier to track the target speed. It is the output of the longitudinal controller. In practice, the actuator uses a longitudinal PID control algorithm to calculate the proportional, integral, and differential terms of the longitudinal velocity error, and then adds these three terms together to obtain the desired acceleration.

[0072] The seventh step involves mapping the desired acceleration to throttle and brake pressure, generating the desired throttle opening and desired brake pressure. This throttle-brake mapping process converts the unified desired acceleration command into specific throttle opening and brake pressure commands based on the vehicle's longitudinal dynamics and current state. In practice, the actuator needs to pre-define a mapping table or function relationship from the desired acceleration to throttle opening and brake pressure. When the vehicle is a car, this mapping is typically non-linear and closely related to the current vehicle speed. The actuator uses the current vehicle speed and desired acceleration to look up the table or calculate the desired throttle opening and desired brake pressure.

[0073] Step 8 involves applying physical limiting processing to the aforementioned desired front wheel steering angle, desired throttle opening, and desired braking pressure to generate the actual front wheel steering angle, actual throttle opening, and actual braking pressure. This physical limiting processing restricts the calculated desired control values ​​within the physical limits allowed by the corresponding actuator, preventing damage to equipment or dangerous situations caused by exceeding mechanical or electrical limits. In practice, the actuator compares the desired front wheel steering angle with preset maximum and minimum steering angle limits to obtain the actual front wheel steering angle; limits the desired throttle opening to between 0% and 100%; and limits the desired braking pressure to between 0 and the maximum braking pressure. These limited values ​​are the final actual command values ​​sent to the actuator.

[0074] The ninth step involves controlling the steering, throttle, and braking mechanisms on the vehicle based on the actual front wheel steering angle, throttle opening, and braking pressure. The steering mechanism is the electromechanical device that controls the front wheel steering angle; it receives angle commands and drives the wheels to steer. The throttle mechanism controls power output; it receives opening commands and adjusts the output torque of the engine or motor. The braking mechanism is the hydraulic or pneumatic device that generates braking force; it receives pressure commands and adjusts the brake line pressure. In practice, when the vehicle is a car, these actuators send the actual front wheel steering angle command to the steering control unit, the actual throttle opening command to the engine management unit or motor controller, and the actual braking pressure command to the braking control unit via the vehicle network. These underlying controllers further drive the corresponding physical actuators, ultimately achieving the vehicle's steering, acceleration, and deceleration movements, thus completing precise control of the vehicle's movement. Through this series of control actions, the vehicle can travel stably and safely along a preset target trajectory.

[0075] Steps one through nine of this disclosure are an inventive point of this disclosure, solving the technical problem that "existing carrier motion control lacks an adaptive mechanism that can dynamically adjust control parameters according to the quality of navigation signals in the closed loop from high-precision navigation state estimation to the actuator, leading to a decrease in control performance or even instability when navigation information deteriorates." Existing technologies have the following shortcomings in navigation-based trajectory tracking control: Firstly, traditional trajectory tracking control typically uses a fixed-parameter PID controller, which cannot adapt to real-time changes in navigation estimation accuracy. When navigation signals are obstructed, interfered with, or degraded by sensors, the fixed gain may amplify erroneous feedback, causing control oscillations or instability. Secondly, when navigation information is unreliable, control commands may fluctuate drastically based on distorted state errors, seriously affecting ride comfort and driving safety. Thirdly, existing control methods lack coordinated adaptive adjustment of lateral control (steering) and longitudinal control (throttle, braking), making it difficult to maintain the accuracy and stability of overall trajectory tracking in complex environments. Fourthly, control commands are directly issued after generation, lacking constraints on their physical limits and safety verification, which may cause the actuator to exceed mechanical or electrical limits, leading to equipment damage or danger. Solving the above problems will enable precise, stable, and safe control of the vehicle's motion even when navigation signal quality fluctuates, ensuring reliable tracking of the preset trajectory under any operating condition and providing crucial technical support for autonomous and intelligent driving. To achieve this, this disclosure proposes the following steps: First, acquiring the preset target vehicle motion trajectory. This provides a clear reference benchmark for tracking control, making the control target clear and quantifiable, thus solving the problem of blind control without trajectory guidance. Second, based on the aforementioned navigation state estimate and the target motion trajectory, generating position tracking error, velocity tracking error, and attitude tracking error. This accurately quantifies the deviation between the actual motion and the desired motion, providing accurate and multi-dimensional input for the subsequent control law, solving the problem of lacking quantitative basis for control. Third, based on the aforementioned dynamic confidence factors, performing a first adaptive adjustment on the preset lateral control proportional coefficient, lateral control integral coefficient, and lateral control derivative coefficient to obtain the lateral adaptive proportional coefficient, lateral adaptive integral coefficient, and lateral adaptive derivative coefficient. This allows the lateral control parameters to respond in real-time to the reliability of navigation information: maintaining or enhancing control when confidence is high, and automatically reducing gain to avoid overreaction when confidence is low, thus solving the problem of fixed parameters being unable to adapt to changes in navigation quality. The fourth step involves generating the desired front wheel steering angle based on the lateral position error in the aforementioned position tracking error, as well as the aforementioned lateral adaptive proportional coefficient, lateral adaptive integral coefficient, and lateral adaptive derivative coefficient. Therefore, an accurate steering command is calculated using an adaptive PID control law, effectively correcting the lateral deviation and solving the problem of matching steering control with error.Fifth, based on the aforementioned dynamic confidence factors, a second adaptive adjustment process is performed on the preset longitudinal control proportional coefficient, longitudinal control integral coefficient, and longitudinal control derivative coefficient to obtain the longitudinal adaptive proportional coefficient, longitudinal adaptive integral coefficient, and longitudinal adaptive derivative coefficient. This ensures that the longitudinal control parameters also possess confidence-adaptive capabilities, guaranteeing the robustness of speed control and resolving the lack of coordinated adaptive response between longitudinal and lateral control. Sixth, based on the longitudinal speed error in the aforementioned speed tracking error, as well as the aforementioned longitudinal adaptive proportional coefficient, longitudinal adaptive integral coefficient, and longitudinal adaptive derivative coefficient, the desired acceleration is generated. Thus, a reasonable acceleration requirement is calculated through adaptive PID control, achieving precise adjustment of speed deviation and resolving the adaptive problem of speed tracking control. Seventh, the desired acceleration is mapped to throttle and brake, generating the desired throttle opening and desired brake pressure. This rationally distributes the unified acceleration command to the throttle and brake actuators, avoiding conflicts between them, and simultaneously achieving smooth acceleration adjustment through mapping, resolving the problem of poor coordination between acceleration and braking. Step 8: Perform physical limiting processing on the aforementioned desired front wheel steering angle, desired throttle opening, and desired braking pressure to generate the actual front wheel steering angle, actual throttle opening, and actual braking pressure. This limits all control commands within the physical limits of the actuators, effectively preventing equipment damage or control failure due to exceeding limits and solving the problem of safe verification of control commands. Step 9: Based on the aforementioned actual front wheel steering angle, actual throttle opening, and actual braking pressure, control the steering actuator, throttle actuator, and braking actuator on the vehicle respectively. This reliably transforms the optimized and verified commands into physical actions, achieving a complete closed loop from decision-making to execution and solving the problem of difficult implementation of intelligent control. In summary, steps 1 to 9 of this embodiment cooperate with each other, starting from the entire chain of "target acquisition, error generation, confidence-adaptive parameter adjustment, control quantity calculation, actuator mapping, physical limiting to precise execution," and by constructing an adaptive trajectory tracking control framework based on dynamic confidence factors, it achieves proactive adaptation and robust response of carrier motion control to navigation signal quality fluctuations. This effectively eliminates control instability, oscillations, and safety hazards caused by the degradation of navigation information, ensuring that the vehicle can accurately, smoothly, and safely track the preset trajectory under any operating conditions, and to a certain extent meets the core requirements of highly reliable motion control in fields such as autonomous driving and intelligent driving.

[0076] In addressing the motion control problem under navigation signal quality fluctuations in the aforementioned background technology using the adaptive trajectory tracking control method based on dynamic confidence factors, the following technical issues often arise in the actual environment of high-performance intelligent vehicles with independent wheel drive capabilities, such as distributed drive electric vehicles and four-wheel independent drive vehicles, when driving on low-friction surfaces (such as ice, snow, and slippery surfaces), performing emergency obstacle avoidance operations, or high-speed cornering: When the vehicle exhibits instability trends such as oversteering, understeering, or sideslip under these extreme conditions, traditional vehicle stability control systems (such as electronic stability control systems), although capable of generating corrective yaw moments by applying independent braking forces to each wheel, typically rely on fixed thresholds and fixed weight allocations, failing to detect changes in the confidence level of the current navigation state estimate. For example, when the vehicle enters a tunnel, urban canyon, or area with strong electromagnetic interference, causing a decrease in the confidence level of the estimated yaw rate or center of gravity sideslip angle, traditional systems may trigger unnecessary interventions based on unreliable state variables, thus interfering with normal vehicle operation; and when navigation information recovers, fixed parameters cannot respond quickly enough to fully utilize high-quality information for accurate correction. Furthermore, for vehicles with independent drive capabilities, traditional methods fail to fully utilize the independent adjustment capabilities of each wheel's drive torque for precise yaw moment control. Moreover, when navigation confidence is low, they cannot prioritize the use of more reliable actuators for allocation, leading to decreased control efficiency and even exacerbating instability due to improper allocation. Considering the following requirements for this application scenario: robustness to navigation signal quality fluctuations under extreme conditions, precise yaw moment control capabilities that fully utilize the advantages of independent drive, and the intelligent requirement to adaptively adjust allocation weights to avoid erroneous intervention when navigation information is unreliable, we have decided to adopt the following solution.

[0077] Optionally, the aforementioned implementing entity may also perform the following steps: The first step is to obtain the measured yaw rate and the estimated center-of-gravity sideslip angle from the navigation state estimates mentioned above. The measured yaw rate refers to the angular velocity of the vehicle's rotation around its vertical axis (Z-axis), usually measured directly by the gyroscope in the inertial measurement unit, in radians per second or degrees per second. It reflects the severity of the vehicle's turning. The estimated center-of-gravity sideslip angle is the angle between the vehicle's actual direction of travel (velocity vector direction) and the direction the vehicle's nose points (longitudinal axis direction). It is usually estimated by a state estimator (such as a Kalman filter) based on the vehicle motion model and sensor fusion results, in radians or degrees. It reflects whether the vehicle has sideslipped. These two parameters are the core state variables for determining vehicle stability. In practice, the execution entity directly extracts the measured yaw rate from the navigation state estimates output in step 105, and simultaneously extracts or calculates the estimated center-of-gravity sideslip angle from the state vector. For example, for a car that is turning, the yaw rate might be 0.3 radians per second (about 17 degrees per second), and the estimated sideslip angle might be 0.05 radians (about 2.9 degrees), indicating that the vehicle has a slight tendency to sideslip.

[0078] The second step involves generating a yaw rate error based on the measured yaw rate and the preset target yaw rate. The preset target yaw rate is the desired yaw rate calculated using an ideal vehicle model based on the current vehicle speed and front wheel angle; it represents the vehicle's expected turning rate under stable conditions. The yaw rate error is the difference between the actual yaw rate and the target yaw rate, reflecting the deviation in the vehicle's yaw response. In practice, the actuator first calculates the target yaw rate based on the current vehicle speed and the actual front wheel angle (which can be obtained from the actual front wheel angle generated in step 107 or directly read from feedback from the steering actuator), combined with the vehicle's two-degree-of-freedom reference model (such as a linear two-degree-of-freedom model). Then, the actual yaw rate is subtracted from the target yaw rate to obtain the yaw rate error. For example, if the target yaw rate is 0.25 radians per second and the actual yaw rate is 0.3 radians per second, the error is +0.05 radians per second, indicating excessive yaw (oversteering).

[0079] The third step involves generating the center of gravity sideslip angle error based on the estimated value and the preset target center of gravity sideslip angle. The preset target center of gravity sideslip angle is typically set to zero or an expected value calculated based on vehicle parameters under ideal conditions. Generally, in a stable state, it is desirable for the center of gravity sideslip angle to be as small as possible. The center of gravity sideslip angle error refers to the difference between the actual center of gravity sideslip angle and the target value, reflecting the degree of vehicle sideslip. In practice, the executing entity usually sets the target center of gravity sideslip angle to zero or a very small threshold (e.g., 0.02 radians), and then subtracts the target value from the estimated value to obtain the error. For example, if the actual center of gravity sideslip angle is 0.05 radians and the target is 0, the error is 0.05 radians, indicating the presence of sideslip.

[0080] The fourth step involves generating a stability control activation signal based on the aforementioned yaw rate error and sideslip angle error. This activation signal is a Boolean value or flag used to determine whether the vehicle is currently in an unstable state, thus deciding whether to trigger stability control intervention. Typically, when either the yaw rate error or the sideslip angle error exceeds a preset threshold, the vehicle is considered unstable and control activation is required. In practice, the actuator compares the absolute value of the yaw rate error with a preset yaw rate error threshold (e.g., 0.1 radians per second) and the absolute value of the sideslip angle error with a preset sideslip angle error threshold (e.g., 0.03 radians). If either error exceeds the corresponding threshold, an activation signal indicating the need for stability control is generated (e.g., set to high or true); otherwise, a signal indicating no control is required is generated. For example, if the yaw rate error of 0.05 radians per second does not exceed 0.1 radians, but the sideslip angle error of 0.05 radians exceeds 0.03 radians, the activation signal is true, triggering stability control.

[0081] The fifth step involves responding to the stability control activation signal indicating the need for stability control. Based on the yaw rate error and the center of gravity sideslip error, a desired additional yaw moment is generated. This desired additional yaw moment, measured in Newton-meters, is the additional torque around the vehicle's vertical axis generated by the longitudinal forces (braking or driving forces) of each wheel to correct vehicle instability. This torque can suppress oversteer or understeer, restoring vehicle stability. In practice, the actuator can employ proportional-integral-derivative (PI-DI) control or sliding mode control, using the yaw rate error and center of gravity sideslip error as inputs to calculate the required additional yaw moment. For example, a simple control law is: Desired Additional Yaw Moment = Kp1 × Yaw Rate Error + Kp2 × Center of Gravity Sideslip Error, where Kp1 and Kp2 are gain coefficients. A positive result indicates a clockwise (viewed from top to bottom) additional yaw moment is needed to correct oversteer; a negative result indicates a counter-clockwise torque is required.

[0082] The sixth step involves adaptively adjusting the preset control allocation weights for each wheel based on the aforementioned dynamic confidence factors, generating adaptive allocation weights for each wheel. The control allocation weights refer to the priority or proportion of each wheel in generating additional yaw moment during braking and driving force distribution. The preset weights are typically set based on vehicle static parameters (such as wheelbase and track width). The adaptive adjustment process dynamically adjusts these weights according to the current dynamic confidence factors, making the control allocation more conservative or prioritizing more reliable actuators when the navigation state estimation confidence is low, thus avoiding erroneous interventions due to inaccurate state estimation. In practice, the actuator reads the preset basic allocation weights for each wheel from memory (e.g., initial values ​​set based on tire vertical load or road adhesion coefficient). Then, a comprehensive confidence index is calculated based on the various dynamic confidence factors (which may combine the confidence of all observation sources or focus on observation sources related to stability, such as yaw rate measurements). Next, the weights of each wheel are scaled using an adaptive function (e.g., weight adjustment factor = 0.8 + 0.2 × overall confidence level) to obtain the adaptive weight allocation for each wheel. For example, if the overall confidence level is low, the weights of all wheels may be reduced to weaken the control effect and avoid over-intervention; alternatively, the weights of each wheel can be adjusted individually based on the confidence level of its related sensors. It should be noted that the drive actuator control for each wheel described above in this embodiment is applicable to vehicles with independent wheel drive capabilities, such as distributed drive electric vehicles. For traditional centralized drive vehicles, stability control is typically achieved only through the braking actuator, and drive torque cannot be independently allocated. Therefore, this step and subsequent steps involving independent drive torque allocation are mainly applicable to vehicles with independent drive capabilities.

[0083] Step 7: Based on the aforementioned expected additional yaw moment and the adaptive weighting of each wheel, control allocation processing is performed to generate the expected braking pressure and expected driving torque for each wheel. Control allocation processing refers to the reasonable distribution of the expected additional yaw moment to each wheel, transforming it into the expected braking pressure (for generating braking force) and expected driving torque (for generating driving force) for each wheel to achieve the required yaw moment. The allocation must consider the wheel's adhesion limits, actuator capabilities, and weight constraints. In practice, the aforementioned actuators can use optimization algorithms (such as weighted least squares) for allocation. First, based on the vehicle geometry and tire model, the relationship between the braking force / driving force of each wheel and the generated yaw moment is established (usually determined by the wheelbase and tire longitudinal force coefficient). Then, with the expected additional yaw moment as the objective and the adaptive weighting of each wheel as the optimization objective, the required longitudinal force for each wheel (positive values ​​represent driving force, negative values ​​represent braking force) is solved. Finally, the longitudinal force is converted into the corresponding braking pressure (through the braking system model) and driving torque (through the power system model). For example, if a positive additional yaw moment is desired, it may be necessary to increase the braking force of the left wheel or decrease the driving force of the left wheel, while simultaneously decreasing the braking force of the right wheel or increasing the driving force of the right wheel. The specific allocation is determined by the weights and optimization algorithm. The execution entity ultimately outputs the desired braking pressure (e.g., 2 MPa for the left front wheel) and the desired driving torque (e.g., 50 Nm for the right rear wheel) for each wheel.

[0084] Step 8 involves applying physical limiting processing to the desired braking pressure and desired driving torque for each wheel to generate the actual braking pressure and actual driving torque for each wheel. This physical limiting processing is similar to the limiting in step 107, restricting the calculated desired values ​​to the allowable physical range of each wheel's braking and driving actuators. For example, the braking pressure cannot exceed the maximum pressure of the hydraulic system (e.g., 12 MPa), and the driving torque cannot exceed the maximum torque of the motor (e.g., 300 Nm). In practice, the actuator compares the desired braking pressure of each wheel with the preset maximum braking pressure and takes the minimum value; simultaneously, negative values ​​(if they occur) are limited to 0 (because braking pressure cannot be negative). A similar limiting is applied to the desired driving torque to ensure it remains within the motor's allowable range. After limiting, the actual braking pressure and actual driving torque for each wheel are obtained. For example, if the desired braking pressure for the left front wheel is 15 MPa, while the system maximum is 12 MPa, the actual braking pressure is limited to 12 MPa.

[0085] The ninth step involves controlling the braking actuators of each wheel based on the actual braking pressures corresponding to each wheel. Each wheel's braking actuator is a device capable of independently controlling the braking pressure of each wheel, such as the hydraulic control unit in an electronic stability control system. This control unit adjusts the inlet and outlet valves of each wheel's brake cylinder to achieve independent pressure increase, maintenance, or reduction. For distributed drive electric vehicles, the driving actuators of each wheel refer to the independent motor and controller for each wheel. In practice, these actuators send the actual braking pressure command for each wheel to the braking control unit (such as the ESC controller) via the vehicle network. The braking control unit then drives the corresponding hydraulic valves to bring the brake cylinder of the corresponding wheel to the target pressure. For example, if the actual braking pressure command for the left front wheel is 12 MPa, the braking control unit will adjust the pressure in the left front wheel brake line to 12 MPa.

[0086] Step 10: Based on the actual driving torques corresponding to each wheel, control the drive actuators of each wheel individually. The drive actuators of each wheel refer to devices capable of independently controlling the driving torque of each wheel. For distributed drive electric vehicles, this means the hub motor or wheel-side motor and its controller for each wheel. For traditional centralized drive vehicles, it may not be possible to independently control the driving torque of each wheel; therefore, this step applies to vehicles with independent drive capabilities. In practice, the actuator sends the actual driving torque command for each wheel to the corresponding motor controller via the vehicle network. The motor controller adjusts the motor output torque to the target value. For example, if the actual driving torque command for the right rear wheel is 50 Nm, the motor controller controls the motor to output 50 Nm of torque. Through the above stability control steps, when the vehicle exhibits instability trends such as oversteer, understeer, or sideslip, the actuator can adaptively allocate the braking and driving forces of each wheel based on high-precision navigation state estimates and dynamic confidence factors, generating a corrective yaw moment to restore vehicle stability, thereby further improving driving safety.

[0087] Steps one through ten of this disclosure are an inventive point of this disclosure, which solves the technical problem that "on the basis of using an adaptive trajectory tracking control method based on dynamic confidence factors to solve the motion control problem under navigation signal quality fluctuations, the existing technology cannot take into account the stability control requirements of high-performance intelligent vehicles with independent wheel drive capabilities under extreme conditions." Existing technologies for vehicle stability control have the following shortcomings: Firstly, traditional stability control systems (such as electronic stability control systems) are typically based on fixed thresholds and fixed weight allocations, making them unable to detect changes in the confidence level of the current navigation state estimate. When the confidence level of the estimated yaw rate or center of gravity sideslip angle decreases due to the vehicle entering tunnels, urban canyons, or areas with strong electromagnetic interference, unnecessary interventions may be triggered based on unreliable state variables, which may interfere with the normal driving of the vehicle. Secondly, when navigation information is restored, fixed parameters cannot respond quickly enough to fully utilize high-quality information for accurate correction. Thirdly, for vehicles with independent driving capabilities, traditional methods fail to fully utilize the independent adjustment capability of the driving torque of each wheel for fine-grained yaw torque control, and cannot prioritize the use of more reliable actuators for allocation when navigation confidence is low, resulting in decreased control efficiency and even exacerbating instability due to improper allocation. Fourthly, traditional control lacks physical limiting constraints on the braking and driving actuators of each wheel, which may lead to damage to the actuators or control failure due to commands exceeding the execution capability. Solving the above problems would enable precise, robust, and safe control of vehicle stability even under extreme conditions, despite fluctuations in navigation signal quality. This would ensure effective suppression of oversteering, understeering, and sideslip under any circumstances, providing highly reliable active safety for intelligent driving vehicles. To achieve this, this disclosure proposes the following steps: First, obtaining the measured yaw rate and the estimated centroid sideslip angle from the aforementioned navigation state estimates. This directly utilizes the core stability state variables in the high-precision navigation fusion results, providing accurate and real-time input for stability judgment and solving the problems of traditional methods relying on dedicated sensors and information silos. Second, generating a yaw rate error based on the measured yaw rate and a preset target yaw rate. This quantifies the deviation between the vehicle's actual yaw response and the ideal response, providing a precise basis for identifying oversteering or understeering and solving the problem of difficulty in determining instability types. Third, generating a centroid sideslip angle error based on the estimated centroid sideslip angle and a preset target centroid sideslip angle. This quantifies the degree of vehicle sideslip, providing a key indicator for identifying sideslip instability and solving the problem that yaw rate alone cannot comprehensively assess stability. The fourth step involves generating a stability control activation signal based on the aforementioned yaw rate error and center of gravity sideslip angle error.Thus, by accurately identifying instability states through a dual-threshold judgment mechanism, control is activated only when truly needed, avoiding frequent or erroneous interventions caused by misjudgments of single indicators and solving the problem of high false triggering rates in traditional methods. Fifth, in response to the stability control activation signal indicating the need for stability control, a desired additional yaw moment is generated based on the aforementioned yaw rate error and centroid sideslip angle error. This calculates the magnitude and direction of the required corrective torque according to the severity of instability, providing a clear target for subsequent allocation and solving the problem of unclear control objectives. Sixth, based on the aforementioned dynamic confidence factors, the preset control allocation weights for each wheel are adaptively adjusted to generate adaptive allocation weights for each wheel. This allows the allocation weights to respond in real-time to the reliability of navigation information: when confidence is high, the allocation is optimized according to conventional weights; when confidence is low, the weights are actively reduced, weakening the control effect or prioritizing the use of more reliable actuators, avoiding erroneous interventions caused by unreliable navigation information and solving the problem that fixed weights cannot adapt to changes in navigation quality. This is particularly suitable for robust control of vehicles with independent driving capabilities in navigation signal fluctuation scenarios. Step 7: Based on the aforementioned expected additional yaw moment and the adaptive weighting of each wheel, control allocation processing is performed to generate the expected braking pressure and expected driving torque for each wheel. This rationally distributes the unified corrective torque to the braking and driving actuators of each wheel, achieving precise and coordinated correction of instability. Particularly for distributed drive vehicles, this fully utilizes the independent driving capabilities of each wheel for refined yaw moment control, solving the problem of difficult multi-actuator coordinated allocation. Step 8: The expected braking pressure and expected driving torque for each wheel are physically limited by the actuators, generating the actual braking pressure and actual driving torque for each wheel. This restricts all control commands within the physical limits of the actuators, effectively preventing equipment damage or control failure due to exceeding limits, and solving the problem of safe verification of control commands. Step 9: Based on the actual braking pressures for each wheel, the braking actuators of each wheel are controlled separately. This reliably translates the optimized and verified braking force commands into physical actions, achieving independent braking control for each wheel and solving the problem of braking coordination. The tenth step involves controlling the drive actuators of each wheel based on the actual drive torques corresponding to each wheel. This fully utilizes the independent drive advantage of a distributed drive vehicle, and through fine adjustment of the drive torque to assist in generating a corrective yaw moment, achieves coordinated control of braking and drive, solving the problem that traditional stability control cannot leverage the advantages of independent drive.In summary, steps one through ten of this embodiment cooperate with each other, starting from the entire chain of "state acquisition, error calculation, activation judgment, torque generation, weight adaptation, control allocation, physical limiting to precise execution," and constructing an adaptive stability control framework based on dynamic confidence factors. This enables vehicles with independent driving capabilities to actively adapt to and robustly respond to navigation signal quality fluctuations under extreme conditions. Therefore, it effectively eliminates the risks of misinterpretation, improper allocation, and safety risks caused by navigation information degradation, ensuring that the vehicle can accurately suppress instability under any condition, significantly improving the active safety performance of intelligent driving vehicles, and to a certain extent meeting the core requirements of advanced autonomous driving for highly reliable stability control.

[0088] The above embodiments of the present invention have the following beneficial effects: Through the multi-source data synchronization inertial navigation method of the present invention, high-precision time synchronization, adaptive fusion, and anomaly error suppression of multi-source navigation data can be achieved, significantly improving the accuracy, robustness, and real-time performance of the integrated navigation system in complex environments, and ultimately achieving precise and reliable control of the carrier's motion. Specifically, traditional multi-source integrated navigation methods (such as existing schemes relying on simple timestamp alignment, fixed-weight fusion, and threshold-based anomaly removal) may encounter problems such as decreased positioning accuracy due to time synchronization errors, divergence in state estimation caused by fusion weight mismatch, and filter jumps caused by abnormal observation contamination when facing sensor clock differences, time-varying signal quality, and sudden anomalies. If only conventional loosely coupled or tightly coupled fusion is relied upon, microsecond-level time differences may be converted into meter-level errors, low-quality data may have excessively high weights, and outlier interference may be difficult to recover quickly, ultimately leading to navigation failure and control mismanagement. Based on this, the multi-source data synchronization inertial navigation method of the present invention first timestamps the collected navigation sensor data to obtain asynchronous observation data with timestamps. Therefore, high-precision absolute timestamps are uniformly marked for sensor data from different clock sources and sampling frequencies, eliminating clock differences at the data source and providing a foundation for subsequent accurate synchronization, avoiding alignment errors caused by hardware clock asynchrony. Then, synchronization compensation processing is performed on the aforementioned asynchronous observation data to obtain synchronized observation data. Based on timestamps and high-frequency inertial data, the displacement of the carrier caused by time differences is accurately compensated by calculating the time difference and extrapolating the motion trajectory, mapping asynchronous data to a unified fusion time. This effectively solves the meter-level spatial positioning error caused by microsecond-level time differences in high-speed dynamic scenarios, achieving true high-precision time synchronization. Next, dynamic confidence evaluation processing is performed on the aforementioned synchronized observation data to obtain various dynamic confidence factors. Thus, for different navigation sources such as GNSS, visual odometry, and UWB, multi-dimensional indicators such as the number of visible satellites, the number of feature points, and signal strength are selected for real-time reliability evaluation, generating dynamic factors that quantify the real-time reliability of sensors. This provides an accurate quality basis for subsequent adaptive fusion, overcoming the shortcomings of fixed weights that cannot respond to environmental changes. Subsequently, adaptive compensation and adjustment processing is performed on the statistical characteristics of each dynamic confidence factor and each historical news sequence to obtain the target observation noise matrix. Thus, on the one hand, the baseline observation noise matrix is ​​scaled according to the dynamic confidence factor to achieve confidence-driven adaptive weight adjustment; on the other hand, anomalies are detected based on the statistical characteristics of the historical news sequences, and penalties are imposed on anomalous observation sources. The penalized noise matrix is ​​used as the target matrix. This dual mechanism ensures that the observation noise matrix can reflect the true quality of each data source in real time and accurately, and effectively suppresses the impact of sudden anomalies.Then, based on the noise matrices of each target observation, the synchronous observation data are fused and processed to obtain navigation state estimates and current information sequences. The adjusted noise matrices are then substituted into a Kalman filter to perform optimal fusion of the multi-source synchronous data, obtaining a high-precision state estimate. Simultaneously, the information sequences are output for feedback, achieving a dynamic closed loop in the fusion process. Next, based on the current information sequences, the statistical characteristics of the historical information sequences are updated. This integrates the current information with historical statistics, enabling historical statistical characteristics to dynamically track data changes and providing a real-time updated benchmark for anomaly detection in the next cycle, forming an adaptive continuous optimization mechanism. Finally, based on the navigation state estimates, the vehicle's motion is controlled. This transforms the high-precision, high-reliability navigation state into specific control commands, driving actuators such as steering, throttle, and braking, achieving a complete closed loop from perception fusion to physical control, ensuring accurate and stable movement of the vehicle in complex environments. Furthermore, because this method incorporates dynamic perception and closed-loop feedback mechanisms in core aspects such as data synchronization, confidence assessment, adaptive adjustment, and anomaly compensation, it can effectively cope with complex operating conditions such as sensor clock differences, signal quality fluctuations, and sudden anomalies. It possesses an inherent ability to suppress low-quality or abnormal data, thereby enhancing the system's robustness and generalization in real-world scenarios. Simultaneously, by organically combining time synchronization, adaptive fusion, and anomaly handling, the entire navigation system achieves an overall improvement in accuracy, reliability, and real-time performance, providing crucial technical support for applications with stringent navigation performance requirements, such as autonomous driving and mobile robots.

[0089] Further reference Figure 2 As an implementation of the methods shown in the above figures, this disclosure provides some embodiments of a multi-source data synchronization inertial navigation system. These device embodiments are similar to... Figure 1 Corresponding to the method embodiments shown, the device can be specifically applied to various electronic devices.

[0090] like Figure 2As shown, some embodiments of the multi-source data synchronous inertial navigation device 200 include: a marking unit 201, a synchronization compensation unit 202, a confidence dynamic evaluation unit 203, an adaptive compensation adjustment unit 204, a fusion calculation unit 205, an update unit 206, and a control unit 207. The system includes: a marking unit 201, configured to timestamp the collected navigation sensor data to obtain timestamped asynchronous observation data; a synchronization compensation unit 202, configured to perform synchronization compensation on the asynchronous observation data to obtain synchronized observation data; a confidence dynamic evaluation unit 203, configured to perform confidence dynamic evaluation on the synchronized observation data to obtain dynamic confidence factors; an adaptive compensation adjustment unit 204, configured to adaptively compensate and adjust the statistical characteristics of the dynamic confidence factors and historical information sequences to obtain target observation noise matrices; a fusion calculation unit 205, configured to perform fusion calculation on the synchronized observation data based on the target observation noise matrices to obtain navigation state estimates and current information sequences; an update unit 206, configured to update the statistical characteristics of the historical information sequences based on the current information sequences; and a control unit 207, configured to control the movement of the carrier based on the navigation state estimates.

[0091] It is understandable that the units described in the device 200 are related to the reference. Figure 1 The steps in the described method correspond to each other. Therefore, the operations, features, and beneficial effects described above for the method also apply to the device 200 and the units contained therein, and will not be repeated here.

[0092] The following is for reference. Figure 3 It shows a schematic diagram of the structure of an electronic device 300 suitable for implementing some embodiments of the present disclosure. Figure 3 The electronic device shown is merely an example and should not be construed as limiting the functionality and scope of the embodiments of this disclosure.

[0093] like Figure 3 As shown, the electronic device 300 may include a processing unit 301 (e.g., a central processing unit, a graphics processor, etc.), which can perform various appropriate actions and processes according to a program stored in a read-only memory (ROM) 302 or a program loaded from a storage device 308 into a random access memory (RAM) 303. The RAM 303 also stores various programs and data required for the operation of the electronic device 300. The processing unit 301, ROM 302, and RAM 303 are interconnected via a bus 304. An input / output (I / O) interface 305 is also connected to the bus 304.

[0094] Typically, the following devices can be connected to I / O interface 305: input devices 306 including, for example, touchscreens, touchpads, keyboards, mice, cameras, microphones, accelerometers, gyroscopes, etc.; output devices 307 including, for example, liquid crystal displays (LCDs), speakers, vibrators, etc.; and communication devices 309. Communication device 309 allows electronic device 300 to communicate wirelessly or wiredly with other devices to exchange data. Although Figure 3 An electronic device 300 with various devices is shown; however, it should be understood that it is not required to implement or possess all of the devices shown. More or fewer devices may be implemented or possessed alternatively. Figure 3 Each box shown can represent a device or multiple devices as needed.

[0095] In particular, according to some embodiments of this disclosure, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, some embodiments of this disclosure include a computer program product comprising a computer program carried on a computer-readable medium, the computer program containing program code for performing the methods shown in the flowcharts. In such embodiments, the computer program can be downloaded and installed from a network via a communication device 309, or installed from a storage device 308, or installed from a ROM 302. When the computer program is executed by the processing device 301, it performs the functions defined in the methods of some embodiments of this disclosure.

[0096] It should be noted that, in some embodiments of this disclosure, the computer-readable medium may be a computer-readable signal medium or a computer-readable storage medium, or any combination thereof. A computer-readable storage medium may be, for example,—but not limited to—an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, apparatus, or device, or any combination thereof. More specific examples of a computer-readable storage medium may include, but are not limited to: an electrical connection having one or more wires, a portable computer disk, a hard disk, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fiber, portable compact disk read-only memory (CD-ROM), optical storage device, magnetic storage device, or any suitable combination thereof. In some embodiments of this disclosure, a computer-readable storage medium may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, apparatus, or device. In some embodiments of this disclosure, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, carrying computer-readable program code. Such propagated data signals may take various forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination thereof. A computer-readable signal medium can be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in connection with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium can be transmitted using any suitable medium, including but not limited to: wires, optical fibers, RF (radio frequency), etc., or any suitable combination thereof.

[0097] In some implementations, clients and servers can communicate using any currently known or future-developed network protocol such as HTTP (Hypertext Transfer Protocol) and can interconnect with digital data communication (e.g., communication networks) of any form or medium. Examples of communication networks include local area networks (“LANs”), wide area networks (“WANs”), the Internet (e.g., the Internet of Things), and peer-to-peer networks (e.g., ad hoc peer-to-peer networks), as well as any currently known or future-developed networks.

[0098] The aforementioned computer-readable medium may be included in the aforementioned electronic device; or it may exist independently and not assembled into the electronic device. The aforementioned computer-readable medium carries one or more programs that, when executed by the electronic device, cause the electronic device to: timestamp the collected navigation sensor data to obtain timestamped asynchronous observation data; perform synchronization compensation processing on the asynchronous observation data to obtain synchronous observation data; dynamically evaluate the confidence level of the synchronous observation data to obtain dynamic confidence factors; adaptively compensate and adjust the statistical characteristics of the dynamic confidence factors and historical news sequences to obtain target observation noise matrices; based on the target observation noise matrices, perform fusion calculation processing on the synchronous observation data to obtain navigation state estimates and current news sequences; update the statistical characteristics of the historical news sequences based on the current news sequences; and control the movement of the carrier based on the navigation state estimates.

[0099] Computer program code for performing operations of some embodiments of this disclosure can be written in one or more programming languages ​​or a combination thereof, including object-oriented programming languages ​​such as Java, Smalltalk, and C++, and conventional procedural programming languages ​​such as the "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as a standalone software package, partially on the user's computer and partially on a remote computer, or entirely on a remote computer or server. In cases involving remote computers, the remote computer can be connected to the user's computer via any type of network—including a local area network (LAN) or a wide area network (WAN)—or can be connected to an external computer (e.g., via the Internet using an Internet service provider).

[0100] The flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer program products according to various embodiments of this disclosure. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code containing one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions indicated in the blocks may occur in a different order than those indicated in the drawings. For example, two consecutively indicated blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or operation, or using a combination of dedicated hardware and computer instructions.

[0101] The units described in some embodiments of this disclosure can be implemented in software or hardware. The described units can also be housed in a processor; for example, a processor may be described as including a marking unit, a synchronization compensation unit, a confidence dynamic evaluation unit, an adaptive compensation adjustment unit, a fusion calculation unit, an update unit, and a control unit. The names of these units do not necessarily limit the specific unit itself; for example, a marking unit may also be described as "a unit that performs timestamp marking processing on the collected navigation sensor data to obtain timestamped asynchronous observation data."

[0102] The functions described above in this document can be performed at least in part by one or more hardware logic components. For example, exemplary types of hardware logic components that can be used, without limitation, include: field-programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), system-on-a-chip (SoCs), complex programmable logic devices (CPLDs), and so on.

[0103] Some embodiments of this disclosure also provide a computer program product, including a computer program that, when executed by a processor, implements any of the above-described multi-source data synchronization inertial navigation methods.

[0104] The above description is merely a selection of preferred embodiments of this disclosure and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of the invention involved in the embodiments of this disclosure is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the above-described inventive concept. For example, technical solutions formed by substituting the above-described features with (but not limited to) technical features with similar functions disclosed in the embodiments of this disclosure.

Claims

1. A multi-source data synchronous inertial navigation method, comprising: The collected navigation sensor data is timestamped to obtain asynchronous observation data with timestamps. Synchronization compensation processing is performed on each asynchronous observation data to obtain each synchronous observation data; The confidence level of each synchronous observation data is dynamically evaluated to obtain each dynamic confidence factor; Adaptive compensation and adjustment are performed on the statistical characteristics of each dynamic confidence factor and each historical information sequence to obtain the observation noise matrix of each target; Based on the noise matrices of each target observation, the synchronous observation data are fused and processed to obtain the navigation state estimate and each current information sequence; Based on the current information sequences, the statistical characteristics of the historical information sequences are updated; The motion of the carrier is controlled based on the estimated navigation state.

2. The method according to claim 1, wherein, The process of timestamping the collected navigation sensor data to obtain timestamped asynchronous observation data includes: The data acquisition times corresponding to each navigation sensor are obtained through various observation sources, including a global navigation satellite system receiver, a visual odometry system, and an ultra-wideband positioning system. The absolute timestamps of each system corresponding to each acquisition time are added to the data of each navigation sensor to obtain asynchronous observation data with timestamps.

3. The method according to claim 1, wherein, The process of performing synchronization compensation on the asynchronous observation data to obtain synchronous observation data includes: Based on the timestamps corresponding to the asynchronous observation data and the preset fusion calculation time, various time differences are generated; Based on the aforementioned time differences, obtain the corresponding driving input data for each asynchronous observation data; Based on the aforementioned driving input data, generate each high-frequency motion trajectory corresponding to each asynchronous observation data; Based on the aforementioned high-frequency motion trajectories, the asynchronous observation data are compensated and corrected to obtain the synchronous observation data.

4. The method according to claim 2, wherein, The process of dynamically evaluating the confidence level of each of the synchronous observation data to obtain various dynamic confidence factors includes: For the synchronous observation data corresponding to the Global Navigation Satellite System receiver, perform the following steps: Obtain the measured values ​​of the number of visible satellites, the precision factor, and the average signal-to-noise ratio; Based on the measured value of the number of visible satellites, a sub-confidence score of the number of visible satellites is generated; Based on the measured values ​​of the precision factor, generate the precision factor sub-confidence level; Based on the measured average signal-to-noise ratio value, an average signal-to-noise ratio sub-confidence score is generated; The weighted summation and fusion of the visible satellite quantity sub-confidence, the accuracy factor sub-confidence, and the average signal-to-noise ratio sub-confidence are performed to obtain the dynamic confidence factor corresponding to the global navigation satellite system receiver. For the synchronous observation data corresponding to the visual odometry, perform the following steps: Obtain the measured values ​​of the number of successfully tracked image feature points and the average reprojection error; Based on the measured value of the number of successfully tracked image feature points, a quantity sub-confidence score is generated; Based on the measured value of the average reprojection error, an error sub-confidence level is generated; The quantity sub-confidence and the error sub-confidence are subjected to a first multiplication and fusion process to obtain the dynamic confidence factor corresponding to the visual odometry. For the synchronous observation data corresponding to the ultra-wideband positioning system, perform the following steps: Obtain the measured value of the received signal strength indication and the measured value of the rate of change between two adjacent ranging results; Based on the measured value of the received signal strength indication value, a strength sub-confidence is generated; Based on the measured values ​​of the rate of change of the two adjacent ranging results, a sub-confidence level of the rate of change is generated; The intensity sub-confidence and the rate of change sub-confidence are multiplied and fused in a second process to obtain the dynamic confidence factor corresponding to the ultra-wideband positioning system.

5. The method according to claim 2, wherein, The adaptive compensation adjustment of the statistical characteristics of each dynamic confidence factor and each historical information sequence to obtain the observation noise matrix of each target includes: Based on the aforementioned dynamic confidence factors, the preset benchmark observation noise matrices are scaled to obtain the preliminary adjusted observation noise matrices. For each observation source, perform the following steps: Based on the statistical characteristics of the historical information sequences corresponding to the observation sources, the sample mean and sample covariance matrix corresponding to the observation sources are generated. Based on the sample mean and the sample covariance matrix, anomaly detection results corresponding to the observation source are generated; In response to the anomaly detection result indicating the presence of anomalies, the preliminarily adjusted observation noise matrix corresponding to the observation source is penalized, and the penalized observation noise matrix is ​​determined as the target observation noise matrix. In response to the anomaly detection result indicating that no anomaly exists, the preliminary adjusted observation noise matrix corresponding to the observation source is determined as the target observation noise matrix.

6. The method according to claim 1, wherein, The process of fusing and solving the synchronous observation data based on the noise matrices of each target observation to obtain navigation state estimates and current information sequences includes: Obtain the navigation state estimate corresponding to the previous fusion solution cycle; The navigation state estimate corresponding to the previous fusion solution cycle is subjected to state prediction processing to obtain the predicted state and the state prediction covariance matrix. Based on the target observation noise matrix and the state prediction covariance matrix, generate each Kalman gain; Based on the predicted state and the preset observation matrices, each predicted observation value is generated. Based on the Kalman gains and the synchronous observation data, the predicted state is updated by observation to obtain the estimated navigation state value corresponding to the current fusion solution cycle. Based on the synchronous observation data and the observation prediction values, each current information sequence is generated.

7. A multi-source data synchronous inertial navigation device, comprising: The marking unit is configured to timestamp the collected navigation sensor data to obtain asynchronous observation data with timestamps. The synchronization compensation unit is configured to perform synchronization compensation processing on the asynchronous observation data to obtain the synchronous observation data. The confidence dynamic evaluation unit is configured to perform a confidence dynamic evaluation on each of the synchronous observation data to obtain each dynamic confidence factor; An adaptive compensation adjustment unit is configured to adaptively compensate and adjust the statistical characteristics of each dynamic confidence factor and each historical information sequence to obtain each target observation noise matrix. The fusion calculation unit is configured to perform fusion calculation on the synchronous observation data based on the noise matrix of each target observation to obtain the navigation state estimate and each current information sequence. The update unit is configured to update the statistical characteristics of each historical information sequence based on each current information sequence; The control unit is configured to control the movement of the vehicle based on the navigation state estimate.

8. An electronic device, comprising: One or more processors; A storage device on which one or more programs are stored; When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any one of claims 1 to 6.

9. A computer-readable medium having a computer program stored thereon, wherein, When the program is executed by the processor, it implements the method as described in any one of claims 1 to 6.

Images