Unmanned aerial vehicle attitude and heading resolving method based on multi-sensor fusion

Abstract

The invention provides an unmanned aerial vehicle attitude and heading resolving method based on multi-sensor fusion, and belongs to the technical field of unmanned aerial vehicle navigation. The method comprises the following steps: initializing a navigation state which covers state information required by a heading attitude resolving process; constructing a sensor measurement model; state prediction is completed based on real-time updating of IMU data, and an initial prediction value is provided for observation updating; correcting prior state estimation by using measurement data acquired by multiple sensors; a prediction-observation-correction loop iteration mechanism is adopted, real-time fusion of multi-sensor data is achieved, and a heading attitude result is output in real time. Complementary characteristics of the sensors are brought into full play, the precision, anti-interference capability and scene adaptability of attitude and heading calculation are improved, attitude angle, say velocity and height information can be output in real time, and the real-time performance and reliability of navigation and control of the unmanned aerial vehicle are met.

Description

Technical Field

[0001] This application relates to the field of unmanned aerial vehicle (UAV) navigation technology, and more specifically, to a method for calculating the attitude of a UAV based on multi-sensor fusion. Background Technology

[0002] Real-time navigation and control of UAVs rely on precise attitude, speed, and altitude information, which is acquired through sensor data processing. Single sensors suffer from limitations in measurement accuracy and susceptibility to noise interference. For example, the gyroscope in an IMU (Inertial Measurement Unit) exhibits zero bias, leading to accumulated attitude calculation errors over long-term use; accelerometers are susceptible to interference from the vehicle's motion acceleration, making it difficult to maintain the accuracy of pure inertial navigation over extended periods; magnetometers are easily affected by external electromagnetic interference, causing deviations in heading measurements; and barometers are susceptible to fluctuations in ambient temperature and air pressure, resulting in insufficient stability in altitude measurement.

[0003] Currently, some attitude and bearing calculation methods use only a single sensor or a simple fusion of two sensors, failing to fully utilize the complementary characteristics of multi-source sensors. This results in calculation accuracy and anti-interference capabilities that are insufficient to meet the navigation and control requirements of UAVs in complex scenarios. Regarding magnetometer observation updates, traditional complementary filtering methods do not consider the difference between actual and theoretical geomagnetic fields, easily introducing large errors when the magnetometer is subjected to significant interference. Furthermore, traditional fusion algorithms struggle to adapt to changes in sensor characteristics under different UAV motion states, further limiting the adaptability and accuracy of attitude and bearing calculations. Summary of the Invention

[0004] In view of this, the purpose of this application is to provide a UAV attitude calculation method based on multi-sensor fusion, so as to solve the problems of current methods being unable to adapt to different motion and interference scenarios, lacking adaptive correction strategies, having weak anti-interference capabilities, and poor adaptability, and to achieve accurate real-time estimation of UAV attitude information.

[0005] This application provides a method for UAV attitude calculation based on multi-sensor fusion, including... Initialize the navigation state, which includes the state information required for the attitude and bearing calculation process; Construction of sensor measurement model; Real-time updates based on IMU data complete state prediction and provide initial predicted values ​​for observation updates; The prior state estimate is corrected using measurement data collected by multiple sensors; A cyclical iterative mechanism of "prediction-observation-correction" is adopted to achieve real-time fusion of multi-sensor data and output the attitude results in real time.

[0006] Preferably, the aforementioned UAV attitude calculation method based on multi-sensor fusion initializes the navigation state, encompassing the state information required for the attitude calculation process, including: The state vector of the navigation state includes attitude quaternions, gyroscope zero bias, z-axis accelerometer zero bias, azimuth velocity, and altitude. The quaternions are calculated based on the initial measurement data of the accelerometer and magnetometer combined with magnetic declination compensation. The gyroscope zero bias and z-axis accelerometer zero bias are initialized to zero, and the altitude is initialized to barometric altitude.

[0007] Preferably, in the aforementioned UAV attitude calculation method based on multi-sensor fusion, the quaternion initialization process includes: Collect the output of the triaxial accelerometer over a period of time and average it to calculate the roll and pitch angles. The quaternion of the leveling magnetometer output is obtained based on the roll angle and pitch angle, and the leveling process is performed on the output of the triaxial magnetometer. The heading angle is obtained by combining the local magnetic declination compensation calculated by the world geomagnetic field model, and finally the quaternion initialization is completed based on the three-axis attitude angle.

[0008] Preferably, in the aforementioned UAV attitude calculation method based on multi-sensor fusion, the construction of the sensor measurement model includes: The sensors include an IMU, a magnetometer, and a barometer. Measurement models for the gyroscope, accelerometer, magnetometer, and barometer are established respectively to clarify the relationship between the actual physical quantity and the measured value, zero bias, and noise. Real-time updates based on IMU data enable state prediction. State recursion is achieved through the state propagation equation, and a state transition matrix is ​​constructed to complete prior state estimation and prior covariance recursion.

[0009] Preferably, the aforementioned UAV attitude calculation method based on multi-sensor fusion establishes measurement models for gyroscopes, accelerometers, magnetometers, and barometers, respectively; Establish an IMU measurement model; Establish a gyroscope measurement model that satisfies the following: ,in, This represents the true angular velocity of the carrier. To achieve zero bias in the gyroscope, The Gaussian white noise of the gyroscope. It is a projection of the Earth's rotational angular rate and can be ignored; Establish an accelerometer measurement model that satisfies the following: ,in, To ensure the true comparison of the carriers, To achieve zero bias in the accelerometer, Gaussian white noise for accelerometers; Establish a magnetometer measurement model that satisfies the following: ,in, This is the geomagnetic reference vector in the navigation coordinate system. The rotation matrix from the machine coordinate system to the navigation coordinate system. It is Gaussian white noise; Establish a barometer measurement model that satisfies the following: ,in, To measure the static pressure value, Noise for altitude measurement.

[0010] Preferably, the aforementioned UAV attitude calculation method based on multi-sensor fusion, which completes state prediction based on real-time updates of IMU data, includes: The quaternion state propagation equation is updated through quaternion multiplication; Both the gyroscope zero bias and the z-axis accelerometer zero bias are modeled as slowly varying stochastic processes; The equation for the propagation of celestial velocity satisfies: ,in It is the acceleration due to gravity. This is velocity noise; The altitude propagation equation uses the trapezoidal integral method: ,in, Noise for altitude measurement.

[0011] Preferably, the aforementioned UAV attitude calculation method based on multi-sensor fusion uses measurement data collected by multiple sensors to correct the prior state estimate, including accelerometer observation update, magnetometer observation update and barometer observation update. The accelerometer uses dynamic adaptive noise covariance to adjust the observation weight, the magnetometer adaptively selects the correction method based on the difference between the measured geomagnetism and the world geomagnetic field model, and the barometer performs error calibration for altitude status. The accelerometer uses dynamic adaptive noise covariance adjustment of observation weights, including: The dynamic adaptive noise covariance of the accelerometer satisfies: ,in It is a third-order identity matrix. These are adaptive coefficients; When the drone is stationary ( and )hour, ; When in motion: .

[0012] Preferably, in the aforementioned UAV attitude calculation method based on multi-sensor fusion, the magnetometer adaptively selects a correction method based on the difference between measured geomagnetism and the global geomagnetic field model, including: Calculate the Euclidean distance between the actual measured geomagnetic data and the reference vector of the World Geomagnetic Magnetic Model. ,in ; Set the difference threshold ,when The three-axis geomagnetic observation model was used for correction. when At that time, based on the current horizontal attitude leveling magnetometer output, the heading angle is calculated and the target quaternion is constructed. The quaternion is then updated smoothly through spherical linear interpolation.

[0013] Preferably, the aforementioned UAV attitude calculation method based on multi-sensor fusion corrects the prior state estimate, and the general process for observation updates includes: Calculate the Kalman filter gain ,in, For the observation matrix, The observation noise matrix; Execution status correction ,in, The sensor measurement value. For observation vectors; Update covariance And the quaternions are normalized.

[0014] Preferably, the aforementioned UAV attitude calculation method based on multi-sensor fusion employs a cyclic iterative mechanism of "prediction-observation-correction" to achieve real-time fusion of multi-sensor data and output attitude results in real time, including: The system employs a cyclical iterative mechanism of "prediction-observation-correction," using the IMU's high-frequency sampling frequency to perform state prediction, and multiple sensors to perform observation updates alternately or in parallel at a set frequency, outputting the UAV's attitude angle, azimuth speed, and altitude information in real time. The sampling frequency of the IMU is higher than the observation update frequency of the accelerometer, magnetometer and barometer. The observation updates of each sensor are performed alternately or in parallel, and the output frequency of the attitude results is consistent with the sampling frequency of the IMU.

[0015] As can be seen from the above, the UAV attitude calculation method based on multi-sensor fusion provided in this application can dynamically adapt to noise based on accelerometer measurements by observing noise through accelerometers, adapting to different motion states and avoiding the influence of external acceleration on attitude during motion; adaptive optimization of geomagnetic observation by judging the difference between measured geomagnetic field and the global geomagnetic field model, and adaptively selecting three-axis observation or interpolation correction to ensure stable heading output; by establishing measurement and observation models, the fusion of data from three types of sensors—IMU, magnetometer, and barometer—is realized, making full use of the complementary characteristics of the sensors, which can adapt to the application scenario of UAV without satellite guidance signals and meet the usage requirements. Attached Figure Description

[0016] The above and other objects, features and advantages of this application will become clearer from the following description of embodiments with reference to the accompanying drawings, in which: Figure 1 The flowchart of a UAV attitude calculation method based on multi-sensor fusion according to an embodiment of this application is illustrated. Figure 1 ; Figure 2 The flowchart of a UAV attitude calculation method based on multi-sensor fusion according to an embodiment of this application is illustrated. Figure 2 ; Figure 3 The flowchart of a UAV attitude calculation method based on multi-sensor fusion according to an embodiment of this application is illustrated. Figure 3 ; Figure 4 The flowchart of a UAV attitude calculation method based on multi-sensor fusion according to an embodiment of this application is illustrated. Figure 4 ; Figure 5 The flowchart of a UAV attitude calculation method based on multi-sensor fusion according to an embodiment of this application is illustrated. Figure 4 . Detailed Implementation

[0017] The embodiments of this application will now be described with reference to the accompanying drawings. However, it should be understood that these descriptions are exemplary only and are not intended to limit the scope of this application. In the following detailed description, numerous specific details are set forth to provide a thorough understanding of the embodiments of this application for ease of explanation. However, it will be apparent that one or more embodiments may be implemented without these specific details. Furthermore, descriptions of well-known structures and technologies are omitted in the following description to avoid unnecessarily obscuring the concepts of this application.

[0018] The terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of this application. The terms “comprising,” “including,” etc., as used herein indicate the presence of features, steps, operations, and / or components, but do not exclude the presence or addition of one or more other features, steps, operations, or components.

[0019] All terms used herein (including technical and scientific terms) have the meanings commonly understood by those skilled in the art, unless otherwise defined. It should be noted that the terms used herein are to be interpreted in a manner consistent with the context of this specification, and not in an idealized or overly rigid way.

[0020] When using expressions such as "at least one of A, B and C", they should generally be interpreted in accordance with the meaning that is commonly understood by those skilled in the art (e.g., "a system having at least one of A, B and C" should include, but is not limited to, a system having A alone, a system having B alone, a system having C alone, a system having A and B, a system having A and C, a system having B and C, and / or a system having A, B and C, etc.).

[0021] In the embodiments of this application, the collection, updating, analysis, processing, use, transmission, provision, disclosure, and storage of data (e.g., including but not limited to user personal information) comply with relevant laws and regulations, are used for legitimate purposes, and do not violate public order and good morals. In particular, necessary measures have been taken to prevent unauthorized access to user personal information data and to safeguard user personal information security, network security, and national security.

[0022] Currently, in the field of real-time navigation and control for unmanned aerial vehicles (UAVs), the acquisition of attitude information relies on the sensor data processing. Among these factors, the gyroscope in the inertial measurement unit (IMU) exhibits zero-bias phenomenon, causing attitude calculation errors to accumulate over time; accelerometers are susceptible to interference from the vehicle's motion acceleration, affecting the accuracy of velocity calculations; magnetometers are easily affected by external electromagnetic interference, causing deviations in heading measurements; and barometers are easily affected by fluctuations in ambient temperature and air pressure, resulting in insufficient stability in altitude measurement. Existing attitude calculation methods only use a single sensor or a simple fusion of two sensors, failing to fully utilize the complementary characteristics of multi-source sensors and neglecting the difference between actual and theoretical geomagnetism. This introduces significant errors when the magnetometer is interfered with, and it is difficult to adapt to changes in sensor characteristics under different UAV motion states. Consequently, the calculation accuracy is reduced, the anti-interference capability is weakened, and the system's adaptability is limited.

[0023] For example, when performing logistics delivery tasks in densely populated urban areas, drones fly between tall buildings. In this scenario, GPS signals are weakened due to building obstruction, and there are also electromagnetic interference sources such as mobile communication base stations. Magnetometers are significantly affected by electromagnetic interference, outputting abnormal magnetic field measurements; barometers exhibit unstable altitude readings due to rapid changes in ambient temperature and local air pressure fluctuations; and the inertial measurement unit experiences increasing attitude error due to zero bias during continuous flight. Furthermore, accelerometers are affected by the acceleration of the drone's maneuvering flight, resulting in decreased velocity calculation accuracy. Consequently, the attitude calculation results show significant deviations, making it difficult for the drone to maintain the predetermined flight path, and causing errors in the execution of control commands.

[0024] If these problems are not addressed, attitude calculation errors will accumulate over time, becoming particularly pronounced in complex electromagnetic and weather environments. This will cause the navigation system to output incorrect attitude, velocity, and altitude information, leading the flight control system to generate inappropriate control commands. Ultimately, the UAV may deviate from its intended trajectory or even experience a loss of control, severely impacting the safety and reliability of mission execution and failing to meet the stringent requirements for navigation and control in complex scenarios.

[0025] like Figure 1-5 As shown, embodiments of this application provide a method for UAV attitude calculation based on multi-sensor fusion. The method includes: initializing the navigation state, covering the state information required for the attitude calculation process; constructing a sensor measurement model; completing state prediction based on real-time updates of IMU data, providing initial prediction values ​​for observation updates; correcting the prior state estimate using measurement data collected by multiple sensors; and adopting a cyclic iterative mechanism of "prediction-observation-correction" to achieve real-time fusion of multi-sensor data and outputting the attitude result in real time.

[0026] Navigation status refers to the kinematic and dynamic information of a UAV at a specific moment, typically including its attitude (such as roll angle, pitch angle, and yaw angle), speed (such as azimuth speed, northward speed, and eastward speed), and position (such as altitude, longitude, and latitude). This information is the basis for the UAV to perform autonomous flight, mission planning, and control.

[0027] Attitude and bearing calculation is a technical process that processes measurement data from various sensors to estimate the navigational state (particularly attitude, speed, and altitude) of a drone in real time. This process aims to provide accurate and stable attitude and bearing information to support the safe and efficient operation of drones.

[0028] A sensor measurement model is a mathematical expression that describes the relationship between the sensor's output measurement value and the actual physical quantity. This model typically considers the sensor's inherent characteristics, bias, noise, and the influence of environmental factors on the measurement results, and serves as the foundation for data fusion and state estimation.

[0029] IMU data refers to the raw measurement data output by the inertial measurement unit (IMU), mainly including angular velocity measured by a three-axis gyroscope and specific force measured by a three-axis accelerometer. IMU data is characterized by high sampling frequency and high short-time accuracy, but long-term integration will produce drift error.

[0030] State prediction refers to the process of calculating the prior state estimate of the UAV at the current moment based on the estimated state of the UAV at the previous moment and the known kinematic model, combined with high-frequency sensor data such as IMU. This process provides initial and continuous state estimates for subsequent observation updates.

[0031] Observation updates refer to the process of correcting prior state estimates obtained through state prediction using measurement data from other sensors (such as magnetometers, barometers, GPS, etc.). Observation updates can introduce external reference information, effectively suppress the accumulation of errors in the prediction process, and improve the accuracy of state estimation.

[0032] Prior state estimation refers to the estimated value of the UAV navigation state obtained from the state prediction stage before observation updates are performed. This estimate is derived from the system dynamic model and historical data and has not yet been corrected by external observation data at the current moment.

[0033] Multi-sensor fusion is a technique that integrates measurement data from different types of sensors. By leveraging the complementary characteristics and redundant information of different sensors, multi-sensor fusion aims to obtain a more comprehensive, accurate, and robust estimation of the navigation state of a UAV than that of a single sensor.

[0034] The "prediction-observation-correction" iterative mechanism is a filtering framework widely used in dynamic system state estimation, such as the Kalman filter. In this mechanism, the system state is first predicted, then the predicted state is corrected using new sensor observation data, and this process is continuously iterated to achieve a continuous, real-time, and optimal estimate of the system state.

[0035] This application proposes a method for UAV attitude calculation based on multi-sensor fusion. This method integrates data from multiple sensors and adopts a cyclic iterative mechanism to address the problems of low accuracy, poor anti-interference ability, and insufficient adaptability in UAV attitude calculation caused by insufficient accuracy of a single sensor, susceptibility to interference, and insufficient fusion.

[0036] Specifically, this method first requires initializing the navigation state to provide the starting state information needed for attitude and bearing calculation. For example, the initial attitude, velocity, and altitude of the UAV can be set manually, or this can be accomplished through simple sensor measurements (such as roughly estimating the initial attitude using accelerometers and obtaining the initial position and velocity using GPS). However, these simple initialization methods may have limitations such as low accuracy, susceptibility to environmental interference, or the need for manual intervention.

[0037] Next, a sensor measurement model needs to be constructed. This model describes the relationship between the measurements of each sensor (e.g., IMU, magnetometer, barometer, etc.) and the actual physical quantities of the UAV. For example, a simple linear model can be built, directly mapping the sensor outputs to physical quantities without considering complex nonlinear characteristics, bias, or noise. However, such a simplified model may not accurately reflect the actual operating conditions of the sensors, thus affecting the accuracy of subsequent data fusion.

[0038] Based on this, real-time state predictions are made using IMU data, providing initial predicted values ​​for observation updates. For example, the Euler integral method can be used to directly integrate the angular velocity data from the gyroscope to obtain attitude changes, and to integrate the specific force data from the accelerometer to obtain velocity and position changes. Although this method is computationally simple, due to the inherent bias and noise of the IMU, long-term integration can lead to the accumulation of errors in attitude, velocity, and position estimations, resulting in significant drift.

[0039] Initializing the navigation state includes: The state vector is a 10-dimensional vector containing attitude, sensor bias, and body motion state information:

[0040] Quaternion ( , , , ): Describes the rotational relationship between the navigation coordinate system and the body coordinate system, satisfying unit constraints. ; Gyroscope zero bias ( , , ): The constant zero bias of the three axes measured by the gyroscope; z-axis accelerometer zero bias ( ): The constant zero bias measured by the z-axis accelerometer; celestial velocity ( ): The velocity in the opposite direction to the z-axis of the navigation coordinate system; high( : The altitude of the drone carrier relative to the takeoff point.

[0041] Quaternion initialization methods: If the triaxial accelerometer output is collected over a period of time, the averaged triaxial accelerometer output will be:

[0042] Roll angle and pitch angle The calculation formula is:

[0043]

[0044] Based on the obtained horizontal attitude, the quaternion used to level the magnetometer output is:

[0045] Averaged output of the triaxial magnetometer:

[0046] The output of the balanced magnetometer for:

[0047] Magnetic declination of the local geographical location was calculated based on the world geomagnetic field model. And compensation is performed. The formula for calculating the heading angle is as follows:

[0048] After obtaining the three-axis attitude angles, initialize the quaternions:

[0049] The gyroscope zero bias and the z-axis accelerometer zero bias are initialized to zero, and the altitude is initialized to barometric altitude.

[0050] When performing a high-precision mapping task, a drone needs to continuously provide accurate attitude and bearing information. At the start of the mission, the drone first initializes its navigation state. For example, the system can use the drone's stationary state at startup to roughly estimate the initial roll and pitch angles by briefly collecting accelerometer data, and set the initial heading angle and initial velocity to zero, for example, while the initial altitude is input as a preset value through the ground control station. This initialization method provides a preliminary starting point for subsequent attitude and bearing calculations.

[0051] Subsequently, the system constructed measurement models for each sensor. For example, mathematical models were established for the gyroscope and accelerometer in the IMU, as well as the magnetometer and barometer. These models defined the relationships between sensor measurements and the UAV's true angular velocity, specific force, geomagnetic field vector, and atmospheric pressure, and preliminarily considered sensor bias and measurement noise. The establishment of these models forms the basis for subsequent data processing and fusion.

[0052] After the UAV takes off and begins flight, the system performs real-time state predictions based on IMU data. The IMU continuously outputs angular velocity and acceleration data at a high frequency (e.g., 200 Hz). This data is used to continuously predict the UAV's attitude, azimuth velocity, and altitude using kinematic equations. For example, gyroscope data is used to integrate and update the attitude quaternion, and accelerometer data is used to integrate and update the azimuth velocity and altitude. Due to the high sampling frequency of the IMU, the prediction process can provide continuous and dynamically responsive attitude estimation; however, its inherent bias and noise cause the prediction results to accumulate errors over time.

[0053] To correct for accumulated errors in IMU predictions, the system utilizes measurement data collected by multiple sensors to refine prior state estimates. For example, when the UAV is in flight, accelerometer data is used to provide a reference for the direction of gravity, correcting roll and pitch drift in attitude estimation. Magnetometer data is used to provide a reference for the direction of the geomagnetic field, correcting heading drift in attitude estimation. Barometer data provides atmospheric pressure information, correcting altitude estimates using a barometric altitude model. These external observation data effectively constrain IMU drift and improve the absolute accuracy of attitude estimation.

[0054] The entire attitude calculation process employs a cyclical iterative mechanism of "prediction-observation-correction". Specifically, IMU data drives the state prediction stage at a high frequency (e.g., 200Hz), providing continuous prior state estimates. Other sensors, such as magnetometers and barometers, update their observations at relatively lower frequencies (e.g., 10Hz or 50Hz). Within each observation update cycle, the system first performs an IMU prediction, then corrects the prediction result using the latest magnetometer or barometer measurement data. The corrected state then serves as the starting state for the next prediction cycle. Through this cyclical iteration, the system achieves real-time fusion of multi-sensor data and outputs the UAV's attitude angles, azimuth velocity, and altitude information in real time at a frequency consistent with the IMU sampling frequency, ensuring the continuity, accuracy, and real-time nature of the attitude information.

[0055] The technical solution presented in this application demonstrates a significant technological contribution to the field of UAV attitude calculation. Traditional attitude calculation methods often rely on a single sensor or a simple combination of sensors, resulting in insufficient accuracy and anti-interference capability of attitude information in complex flight environments. For example, pure inertial navigation systems suffer from severe drift due to IMU bias and noise, while systems relying solely on magnetometers or barometers are susceptible to external environmental interference.

[0056] This application lays a solid theoretical foundation for subsequent data fusion by constructing an accurate sensor measurement model. More importantly, this application uses real-time updates based on IMU data for state prediction, combines measurement data collected from multiple sensors to correct prior state estimates, and achieves complementary advantages of different sensor data through a cyclical iterative mechanism of "prediction-observation-correction".

[0057] The proposed solution more effectively utilizes the high-frequency dynamic information of the IMU and the absolute reference information of other sensors, thereby significantly improving the accuracy and robustness of attitude calculation while ensuring real-time performance. For example, the high-frequency prediction of the IMU can capture the rapid dynamic changes of the UAV, while the observation updates from the magnetometer and barometer can effectively suppress long-term IMU drift. This tightly integrated and iterative mechanism enables the system to adapt to the navigation needs of the UAV in different motion states and complex environments, providing continuous, stable, and high-precision attitude results, thus providing a reliable guarantee for the precise control and mission execution of the UAV.

[0058] The initialization of the navigation state includes the state information required for the attitude calculation process, including: the state vector of the navigation state contains attitude quaternions, gyroscope zero bias, z-axis accelerometer zero bias, azimuth velocity and altitude. The quaternions are calculated based on the initial measurement data of the accelerometer and magnetometer combined with magnetic declination compensation. The gyroscope zero bias and z-axis accelerometer zero bias are initialized to zero, and the altitude is initialized to barometric altitude.

[0059] The state vector of the navigation state is a set of key variables used in attitude and bearing calculation algorithms to describe the current motion state of the UAV and sensor errors. It typically includes information such as attitude, position, velocity, and sensor bias. The accuracy of this vector directly affects the accuracy and stability of subsequent calculation results. In addition to the attitude quaternions, gyroscope zero bias, z-axis accelerometer zero bias, yaw velocity, and altitude quantities listed in this application, the state vector can also include other state quantities such as horizontal position, horizontal velocity, magnetometer zero bias, and barometer zero bias, depending on specific application requirements.

[0060] Attitude quaternions are a commonly used mathematical tool for representing the rotational attitude of an object in three-dimensional space. Compared to Euler angles, they avoid gimbal lock and offer advantages such as high computational efficiency and compact storage. Their function is to provide precise orientation information of the UAV relative to the navigation coordinate system (usually the N / A coordinate system). Initialization of attitude quaternions can be achieved using various methods. For example, in addition to utilizing initial measurement data from accelerometers and magnetometers, initialization can be performed using a preset initial attitude or with the assistance of an external vision system.

[0061] Gyroscope bias refers to the output deviation that a gyroscope still exhibits even in an ideal stationary state. It is one of the main error sources affecting the long-term accuracy of inertial navigation systems. Its function is to compensate for the inherent systematic errors of the gyroscope to obtain a more accurate angular rate measurement. Gyroscope bias can typically be initialized to zero, or estimated by averaging gyroscope data collected over a period of time while the UAV is stationary.

[0062] Z-axis accelerometer zero bias refers to the inherent output deviation of the accelerometer in the z-axis direction (usually the vertical direction of the UAV). Its function is to compensate for systematic errors in the accelerometer, especially in vertical measurements, which is crucial for accurate calculation of altitude and yaw velocity. The z-axis accelerometer zero bias is typically initialized to zero, or determined through calibration under specific attitudes (e.g., horizontal stillness).

[0063] Vertical velocity refers to the speed of a UAV in the vertical direction. It is a key parameter describing the vertical motion dynamics of a UAV and is of great significance for accurate altitude estimation and vertical motion control. Vertical velocity is typically initialized to zero, or, if an external reference (such as GPS vertical velocity) is available, it can be initialized using that reference value.

[0064] Altitude refers to the vertical distance of a UAV relative to a reference plane (such as sea level or takeoff point). It is one of the fundamental parameters in UAV navigation and control. Altitude can be initialized based on various sensor data; for example, in addition to barometric altitude, it can be initialized using GPS altitude, lidar altimeter, or ultrasonic altimeter.

[0065] Quaternions are calculated based on initial measurements from accelerometers and magnetometers, combined with magnetic declination compensation. Their function is to use the Earth's gravity vector and geomagnetic vector as external references to quickly and accurately determine the initial attitude of the UAV during startup. The accelerometer senses the direction of gravity, providing initial information for roll and pitch angles; the magnetometer senses the direction of the Earth's magnetic field, providing initial information for the heading angle. Combined with magnetic declination compensation, the deviation between the local geomagnetic field and the geographic North Pole can be eliminated, ensuring the accuracy of the heading angle. This initialization method provides a reliable starting point for subsequent attitude calculations, avoiding the accumulation of errors caused by inaccurate initial attitude.

[0066] The gyroscope and z-axis accelerometer biases are initialized to zero. This is to ensure that, during the initial system startup, the gyroscopes and z-axis accelerometers are in an ideal state without significant systematic deviations. This simplification facilitates a rapid start to the attitude calculation process and reduces the computational burden of introducing complex bias estimations in the initial stage. In subsequent fusion processes, these biases are estimated and corrected online using algorithms such as Kalman filtering.

[0067] Altitude is initialized to barometric altitude, which serves as a reference for the drone's initial altitude using barometer measurements. A barometer is a commonly used altitude sensor; its measurements are related to atmospheric pressure and can be converted to altitude using a standard atmospheric pressure model. This initialization method provides a relatively accurate initial value for subsequent altitude calculations, especially when no other high-precision altitude references are available.

[0068] The solution presented in this application provides a robust starting point for the aforementioned UAV attitude calculation method based on multi-sensor fusion by comprehensively and accurately initializing the navigation state. Upon UAV startup, a state vector is first constructed, comprising attitude quaternions, gyroscope zero bias, z-axis accelerometer zero bias, azimuth velocity, and altitude. These state variables cover the core information for attitude calculation. Specifically, the attitude quaternions utilize the gravity direction sensed by the accelerometer and the geomagnetic direction sensed by the magnetometer, combined with local magnetic declination for compensation, thereby accurately determining the UAV's initial spatial attitude. Simultaneously, the gyroscope zero bias and z-axis accelerometer zero bias are initialized to zero, simplifying the complexity of the initial state and reserving space for subsequent online estimation. The altitude is directly initialized using barometer measurements, providing a reliable vertical position reference. This refined initialization process enables subsequent sensor measurement model construction, state prediction based on IMU data, and multi-sensor observation updates to be carried out in a high-confidence initial state, thereby ensuring the stability and accuracy of the entire "prediction-observation-correction" iterative mechanism and significantly improving the overall performance of UAV attitude calculation.

[0069] During the initialization phase, the navigation state parameters need to be configured to ensure the accuracy of the solution starting point: The state vector is a 10-dimensional vector containing attitude, sensor bias, and body motion state information:

[0070] The meanings of each component are as follows: Quaternion ( , , , ): Describes the rotational relationship between the navigation coordinate system and the body coordinate system, satisfying unit constraints. ; Gyroscope zero bias ( , , ): The constant zero bias of the three axes measured by the gyroscope is initialized to 0; z-axis accelerometer zero bias ( ): The constant zero bias measured by the z-axis accelerometer is initialized to 0; celestial velocity ( ): The velocity in the opposite direction of the z-axis of the navigation coordinate system, initialized to 0; high( ): The altitude of the UAV carrier relative to the takeoff point is initialized to the actual altitude measured by the barometer.

[0071] Quaternion initialization process: Collect the triaxial accelerometer outputs over 2 seconds and calculate the average value of the triaxial accelerometer outputs:

[0072] Anomaly detection: If With gravitational acceleration The absolute value of the difference is greater than If the data is discarded, it is re-collected and averaged. If the requirements are still not met, it is determined to be an initialization error.

[0073] Take the averaged accelerometer data that meets the requirements, and calculate the roll angle. and pitch angle The calculation formula is:

[0074]

[0075] Based on the obtained horizontal attitude, the quaternion used to level the magnetometer output is:

[0076] Averaged output of the triaxial magnetometer:

[0077] The output of the balanced magnetometer for:

[0078] Magnetic declination of the local geographical location was calculated based on the world geomagnetic field model. And compensation is performed. The formula for calculating the heading angle is as follows:

[0079] After obtaining the three-axis attitude angles, initialize the quaternions:

[0080] By precisely initializing the attitude quaternions, gyroscope bias, z-axis accelerometer bias, and azimuth velocity and altitude, a high-confidence starting point is provided for the subsequent multi-sensor fusion process. This not only ensures the accuracy of the starting point for the attitude calculation process and significantly reduces the accumulation of early errors, but also improves the stability and reliability of the entire attitude calculation system. This allows the UAV to enter a high-precision navigation state more quickly after startup, thus laying a solid foundation for the UAV's precise control and safe flight.

[0081] like Figure 3As shown, the quaternion initialization process includes: collecting the output of the three-axis accelerometer over a period of time and averaging it, and calculating the roll and pitch angles; obtaining the quaternion of the leveling magnetometer output based on the roll and pitch angles, and performing leveling processing on the three-axis magnetometer output; obtaining the heading angle by combining the local magnetic declination compensation calculated by the world geomagnetic field model, and finally completing the quaternion initialization based on the three-axis attitude angles.

[0082] The initialization process begins by preprocessing the accelerometer measurement data to eliminate or reduce the impact of instantaneous noise and vibration on attitude calculation, thereby obtaining more stable and reliable initial values ​​for roll and pitch angles. Specifically, a moving average filter can be used to smooth the continuously acquired triaxial accelerometer data. For example, the X, Y, and Z axis outputs of the accelerometer can be arithmetically averaged within a preset time window to reduce the influence of random noise. Alternatively, an exponentially weighted average method can be used, assigning different weights to historical data for averaging, to balance data real-time performance and smoothness. After obtaining the averaged accelerometer outputs, the roll and pitch angles of the UAV are calculated using the arctangent function based on the projection relationship of the gravity vector in the body coordinate system.

[0083] Next, this process aims to correct errors in the magnetometer measurement data introduced by changes in the UAV's attitude (roll and pitch), ensuring that the magnetometer data accurately reflects the horizontal component of the Earth's magnetic field, laying the foundation for subsequent accurate calculation of the heading angle. Leveling can be achieved by rotating the original triaxial magnetometer output from the body coordinate system to the horizontal coordinate system (or navigation coordinate system). Specifically, using the roll and pitch angles calculated in the previous step, a rotation matrix from the body coordinate system to the horizontal coordinate system can be constructed. Then, the original magnetometer measurement values ​​are multiplied by this rotation matrix to obtain the magnetometer output in the horizontal coordinate system, thus completing the leveling process. Alternatively, attitude quaternions can be used to transform the magnetometer measurement values ​​from the body coordinate system to the navigation coordinate system, and then the horizontal component can be extracted. Alternatively, the vertical component of the magnetometer can be directly corrected using attitude information to project it onto the horizontal plane.

[0084] Finally, this process aims to address the magnetic declination issue between the geomagnetic field and geographic true north, ensuring that the calculated heading angle is relative to geographic true north, thereby improving the accuracy of the initial attitude. Magnetic declination compensation can be achieved by querying a world geomagnetic model (e.g., the World Geomagnetic Model WMM or the International Geomagnetic Reference Field IGRF model), calculating the local magnetic declination value based on the UAV's current geographic latitude and longitude information. Alternatively, a magnetic declination data table for a specific region can be pre-stored. During initialization, the current position is obtained based on positioning information (such as GPS), and the corresponding magnetic declination is then looked up from the data table for compensation. After obtaining the magnetically declination-compensated heading angle, it is combined with the previously calculated roll and pitch angles to form a complete three-axis attitude angle. Finally, these three-axis attitude angles are converted into quaternion form as the UAV's initial attitude.

[0085] This application's solution effectively filters out instantaneous noise and vibration by collecting and averaging the output of a three-axis accelerometer over a period of time, thereby stably calculating the roll and pitch angles of the UAV. These stable roll and pitch angles provide a reliable attitude reference for subsequent magnetometer leveling. Next, based on the obtained roll and pitch angles, the three-axis magnetometer output is leveled, transforming the magnetometer measurements from the body coordinate system to the horizontal coordinate system. This eliminates the influence of UAV attitude changes on the magnetometer readings, ensuring that the magnetometer data accurately reflects the component of the Earth's magnetic field on the horizontal plane. Furthermore, the leveled magnetometer data is compensated using the local magnetic declination calculated by the World Geomagnetic Magnetic Model, correcting the magnetic north direction to geographic true north, thus obtaining an accurate heading angle. Finally, the compensated heading angle is combined with the previously calculated roll and pitch angles to complete the quaternion initialization. Through this series of steps, the solution proposed in this application can provide a more accurate and robust initial attitude quaternion, providing a high-quality starting point for the subsequent iterative mechanism of real-time updates based on IMU data to complete state prediction and to correct prior state estimates using measurement data collected by multiple sensors, thus significantly improving the initial accuracy and stability of the entire attitude calculation system.

[0086] This application significantly improves the stability of roll and pitch angle calculations by averaging the accelerometer output, providing a solid foundation for subsequent attitude calculations. Leveling the magnetometer output based on stable roll and pitch angles effectively eliminates the influence of UAV attitude changes on magnetometer readings, resulting in more accurate heading angle calculations. Magnetic declination compensation using a global geomagnetic field model ensures the heading angle is relative to geographic true north, thus avoiding heading deviations caused by geographical location differences. These improvements work together to make the initial attitude quaternion more accurate and reliable, providing a high-quality starting state for the entire UAV attitude calculation system. This enhances the accuracy and robustness of subsequent real-time fusion calculations, especially in complex electromagnetic environments or applications requiring high-precision heading information, where its advantages are even more pronounced.

[0087] like Figure 2 As shown, the construction of the sensor measurement model includes: the sensors include an IMU, a magnetometer, and a barometer. Measurement models for the gyroscope, accelerometer, magnetometer, and barometer are established respectively to clarify the relationship between the actual physical quantity and the measured value, zero bias, and noise. Real-time updates based on IMU data enable state prediction. State recursion is achieved through the state propagation equation, and a state transition matrix is ​​constructed to complete prior state estimation and prior covariance recursion.

[0088] The sensors include an IMU (Inertial Measurement Unit), a magnetometer, and a barometer. An IMU is a sensor that integrates a gyroscope and an accelerometer to measure the angular velocity and linear acceleration of the vehicle. The magnetometer senses the Earth's magnetic field, providing a heading reference. The barometer measures atmospheric pressure, thereby calculating altitude. These sensors are indispensable data sources for UAV attitude calculation; each provides unique measurement information, and through synergy, they achieve data complementarity.

[0089] For the aforementioned sensors, this application establishes measurement models for gyroscopes, accelerometers, magnetometers, and barometers, respectively. A measurement model is a mathematical expression describing the relationship between the sensor output and the actual physical quantity, aiming to quantify various errors in the sensor measurement process. For example, linear or nonlinear functions can be used to describe the mapping relationship between the sensor output and the actual physical quantity, introducing error terms. Another approach is to use statistical methods, such as least squares or maximum likelihood estimation, to identify and establish parameterized models that accurately reflect the sensor characteristics using calibration data.

[0090] When establishing the measurement model, this application explicitly defines the relationship between the actual physical quantities, the measured values, the zero bias, and the noise. This means that the mathematical model clearly defines the connection between the actual physical quantities (such as actual angular velocity, actual specific force, actual geomagnetic field strength, and actual height) and the measured values ​​directly output by the sensor, and explicitly incorporates the zero bias (the fixed deviation of the sensor under no input or specific input) and noise (random, unpredictable measurement disturbances) as part of the model. For example, an additive error model can be used to represent the measured values ​​as a linear superposition of the actual physical quantities, the zero bias, and random noise; or a more complex nonlinear model can be used to incorporate these error sources in a multiplicative or more complex functional form. In this way, the inherent error characteristics of the sensor can be incorporated into the state estimation framework, providing an accurate error propagation mechanism for subsequent state prediction and correction.

[0091] This application utilizes real-time updates of IMU data to perform state prediction. State prediction is the process of estimating the system state at the next moment by combining the current system state estimate and system dynamics model with sensor data. Because IMUs have a high sampling frequency and the ability to directly sense the motion of the carrier, they are often used for high-frequency state prediction. This can be achieved by inputting the IMU's angular velocity and acceleration data into the UAV's kinematic and dynamic equations to continuously update the UAV's attitude, velocity, position, and other state variables.

[0092] To achieve state prediction, this application implements state recursion through state propagation equations. State propagation equations are mathematical formulas describing the evolution of a system's state over time. For UAV attitude calculation, these equations typically include quaternion update equations for attitude, velocity update equations, and position update equations. For example, attitude updates can be performed by integrating quaternion differential equations based on angular velocities measured by gyroscopes; velocity updates can be performed by integrating specific forces measured by accelerometers, taking into account gravitational acceleration and the effects of Earth's rotation. These equations enable the system to derive the predicted state for the current moment from the state of the previous moment.

[0093] Preferably, in the aforementioned attitude calculation method based on multi-sensor fusion, the construction of the sensor measurement model includes: IMU measurement model: Gyroscope measurement model: The gyroscope outputs the angular rate in the body coordinate system, including the projection of the Earth's rotation angular rate in the carrier coordinate system, gyroscope bias and noise, and the true angular rate versus the measured angular rate. The relationship is:

[0094] in, This represents the true angular velocity of the carrier. To achieve zero bias in the gyroscope, The Gaussian white noise of the gyroscope. This is a projection of the Earth's rotation angular rate (this is usually negligible for MEMS IMUs). Accelerometer measurement model: The accelerometer outputs the force ratio in the body coordinate system, including accelerometer bias and noise, the true force ratio and the measured force ratio. The relationship is:

[0095] in, To ensure the true comparison of the carriers, Zero bias for accelerometer (only modeling zero bias in the z-axis direction). Gaussian white noise for accelerometers.

[0096] Magnetometer measurement model: The magnetometer outputs the geomagnetic component in the body coordinate system, including noise; the geomagnetic reference vector in the navigation coordinate system is compared with the magnetometer measurement. The relationship is:

[0097] in, This is the geomagnetic reference vector in the navigation coordinate system (calculated based on the world geomagnetic field model). This is the rotation matrix from the body coordinate system to the navigation coordinate system. It is Gaussian white noise.

[0098] in, This is the geomagnetic reference vector in the navigation coordinate system (calculated based on the world geomagnetic field model). The rotation matrix from the body coordinate system to the navigation coordinate system is:

[0099] Barometer measurement model: static pressure value (Unit: Pa) can be converted to altitude using the following formula:

[0100] in, Noise for altitude measurement.

[0101] This application constructs a state transition matrix to complete prior state estimation and prior covariance recursion. The state transition matrix is ​​a matrix describing the linear transformation of the system state from one time step to the next, and it plays a crucial role in state estimation algorithms such as Kalman filtering. By locally linearizing the nonlinear system at the current state estimation point, the state transition matrix can be obtained, which can then be used to calculate the prior state estimate (i.e., the predicted state) and the prior covariance (the uncertainty of the predicted state). The prior covariance recursion updates the covariance matrix of the state estimation error based on the state transition matrix and the process noise covariance matrix. This reflects the accumulation of uncertainty during the prediction process and provides quantified uncertainty information for subsequent observation updates.

[0102] This application proposes a comprehensive sensor measurement model that, combined with IMU data, enables real-time state prediction, thereby providing accurate prior state estimates for UAV attitude calculation. First, the core sensors required for UAV navigation—IMU, magnetometer, and barometer—are incorporated into a unified modeling framework. For each sensor, a separate measurement model is established. These models not only describe the fundamental relationship between sensor output and actual physical quantities but, more importantly, explicitly quantify the impact of unavoidable bias and random noise during measurement. This explicit modeling approach allows the system to accurately understand the error characteristics of sensor data, laying a solid foundation for subsequent state estimation. Based on this, utilizing the high-frequency sampling characteristics of the IMU, the system can perform real-time and continuous state prediction based on IMU data. This process is achieved through state propagation equations, which recursively derive the predicted state for the next moment from the current attitude, velocity, and other state quantities, combined with the IMU's angular velocity and acceleration measurements. To quantify the uncertainties in the prediction process, this application further constructs a state transition matrix, which is used not only to calculate the prior state estimate but also to derive the prior covariance. Prior covariance reflects the error range of the predicted state and is a key parameter for measuring prediction reliability in fusion algorithms such as Kalman filtering. In this way, this application provides high-precision initial predictions with uncertainty quantification for the observation update step within a "prediction-observation-correction" iterative mechanism, significantly improving the accuracy and robustness of the entire attitude calculation system and effectively solving the problem of inaccurate predictions due to unclear models in traditional methods.

[0103] like Figure 4 As shown, the measurement models for the gyroscope, accelerometer, magnetometer, and barometer are established respectively, including: establishing an IMU measurement model; and establishing a gyroscope measurement model, which satisfies the following: ,in, This represents the true angular velocity of the carrier. To achieve zero bias in the gyroscope, The Gaussian white noise of the gyroscope. The model represents a negligible projection of the Earth's rotation angular rate. This model allows for the separation of the carrier's true angular rate from the raw measurements of the gyroscope, providing accurate input for attitude updates and effectively suppressing the long-term drift effects of bias and noise on attitude calculations. In practical applications, bias can be estimated through static calibration of the gyroscope, and dynamic changes in bias can be estimated and compensated using filtering algorithms (such as Kalman filtering). Alternatively, adaptive filtering techniques can be employed to adjust the bias estimate in real-time based on the gyroscope's operating status and environmental changes, thereby improving the model's accuracy.

[0104] Establish an accelerometer measurement model that satisfies the following: ,in, To ensure the true comparison of the carriers, To achieve zero bias in the accelerometer, The accelerometer uses Gaussian white noise; this model allows for the extraction of the true specific force acting on the carrier from the raw accelerometer measurements. This is crucial for distinguishing between motion acceleration and gravitational components, thus providing accurate vertical information for attitude and velocity observation updates. The accelerometer's zero bias can be determined through calibration in a static state or through online estimation and compensation using a state estimator in a dynamic environment. To improve the robustness of the zero bias estimation, a multi-position calibration method can be employed, collecting data under different attitudes for zero bias estimation, or a temperature compensation model can be combined to address the issue of zero bias varying with temperature.

[0105] Establish a magnetometer measurement model that satisfies the following: ,in, This is the geomagnetic reference vector in the navigation coordinate system. The rotation matrix from the machine coordinate system to the navigation coordinate system. The noise is Gaussian white noise. By transforming the magnetometer's measurements in the mechanical system to the navigation coordinate system and removing noise, accurate heading information is provided to correct heading drift in attitude calculations. The rotation matrix can be calculated using the current attitude estimate (e.g., quaternions). The magnetometer's noise can be characterized statistically by performing static tests on the sensor. To improve the reliability of the magnetometer data, soft and hard magnetic calibration methods can be used to eliminate internal and external magnetic field interference, ensuring a closer approximation to the real Earth's magnetic field.

[0106] Establish a barometer measurement model that satisfies the following: ,in, To measure the static pressure value, This model addresses altitude measurement noise. It converts raw barometer measurements into altitude information, providing observational data for the UAV's vertical position and correcting altitude estimation errors. The constants in the model are derived from the International Standard Atmospheric Pressure Model (ISPA) and can be used directly. The altitude measurement noise, n_(h_p), can be determined through statistical analysis of barometer measurements under stable conditions. To improve the accuracy of barometer altitude measurements, temperature compensation can be applied to the barometer output using a temperature sensor, as barometer readings are affected by ambient temperature.

[0107] Establishing an IMU measurement model is a mathematical model describing the relationship between the output data of the inertial measurement unit (IMU) and the actual motion state of the carrier. As the foundation of the inertial navigation system, it provides a unified mathematical framework for the measurement data from gyroscopes and accelerometers. The purpose of establishing an IMU measurement model is to provide consistency and interpretability for subsequent inertial sensor data processing, ensuring an accurate understanding of the physical meaning of the sensor data during state prediction and observation updates. In practical applications, various methods can be used to establish an IMU measurement model. For example, one method is to use a linear error model, representing the IMU measurement value as a superposition of the true value, zero bias, scaling factor error, inter-axis error, and random noise. Another method is to use a simplified model, considering only zero bias and Gaussian white noise. This approach is suitable for embedded systems with strict limitations on computational resources, while still effectively capturing the main error sources.

[0108] This application, by explicitly defining the measurement model for each sensor, including the quantitative relationship between real physical quantities and measured values, zero bias, and noise, accurately characterizes the deviation between sensor output and actual state. This precise modeling significantly reduces additional errors introduced during state correction, thereby greatly improving the accuracy and anti-interference capability of attitude calculation. Specifically, explicit handling of gyroscope zero bias effectively suppresses long-term attitude drift; compensation for z-axis accelerometer zero bias improves state estimation in the vertical direction; the magnetometer measurement model provides a more reliable heading reference through coordinate system transformation and noise reduction; and the barometer measurement model stabilizes altitude measurement, effectively responding to environmental fluctuations. These improvements collectively ensure that the "prediction-observation-correction" cycle operates with higher fidelity and robustness.

[0109] Real-time updates based on IMU data complete status predictions include: The quaternion state propagation equation is updated through quaternion multiplication; Both the gyroscope zero bias and the z-axis accelerometer zero bias are modeled as slowly varying stochastic processes; The equation for the propagation of celestial velocity satisfies: ,in It is the acceleration due to gravity. This is a velocity noise factor used to accurately predict the vertical velocity variation of a UAV. It comprehensively considers the specific force measured by the IMU, the estimated accelerometer bias, the vertical component after transformation to the navigation coordinate system via a rotation matrix, and subtracts the effect of gravitational acceleration g. It also introduces velocity noise to reflect measurement uncertainties. This makes the vertical velocity prediction more realistic.

[0110] The altitude propagation equation uses the trapezoidal integral method: ,in, This addresses noise in height measurement. It's used to infer height changes from velocity information. Compared to simple rectangular integration, the trapezoidal integration method calculates displacement by averaging the velocity at the current and next moments, significantly improving integration accuracy, effectively reducing discretization errors, and thus enhancing the reliability of height prediction.

[0111] Quaternion state propagation equation: When sampling time When the interval is relatively small (meeting the frequency requirements for real-time calculation by UAVs), the quaternion increment can be approximated as:

[0112] The quaternion is updated at time k+1 via quaternion multiplication:

[0113] in, Substituting the quaternion multiplication matrix into the gyroscope's true angular velocity (obtained by subtracting zero bias from the gyroscope's measured value), a specific quaternion prediction propagation matrix can be obtained, enabling real-time attitude recursion.

[0114] When sampling time At that time, the quaternion increment can be approximated as:

[0115] The quaternion is updated at time k+1 via quaternion multiplication:

[0116] in, This is a quaternion multiplication matrix, specifically in the form of:

[0117] Substituting the actual angular velocity of the gyroscope (obtained by subtracting the zero bias from the gyroscope measurement) into the actual quaternion prediction propagation matrix, we can obtain the specific quaternion prediction propagation matrix and realize the real-time recursion of attitude.

[0118] Zero bias propagation equation: Both the gyroscope zero bias and the z-axis accelerometer zero bias are modeled as slowly varying stochastic processes, meaning the zero bias changes slowly over time. Their propagation equations are as follows: The gyroscope zero-bias model is as follows:

[0119] in, This refers to the gyroscope's zero-bias random walk noise.

[0120] The z-axis accelerometer zero bias model is as follows:

[0121] in, This represents the random walk noise of the z-axis accelerometer zero bias.

[0122] Equation for propagation of celestial velocity: The axial velocity is obtained by integrating the force measured by the accelerometer into the navigation system. The conversion process is performed using a rotation matrix. To achieve this, the acceleration due to gravity must be counteracted. The effect is expressed mathematically as follows:

[0123] Altitude propagation equation: The altitude is obtained by integrating the upward velocity using the trapezoidal integration method:

[0124] in, Noise for altitude measurement.

[0125] State transition matrix and prior update: State transition matrix The prior covariance is derived from the Jacobian matrix of the state vectors based on the propagation equations of each state variable. This matrix is ​​used for the Kalman filter prior covariance recursion. Its block form and the calculation formula for each block state are as follows: State transition matrix The definition is as follows:

[0126] The specific form of each block matrix is ​​as follows: (Partial derivative of a quaternion with respect to a quaternion):

[0127] (Partial derivative of quaternion with respect to zero bias of gyroscope):

[0128] (Partial derivative of celestial velocity with respect to quaternions):

[0129] (Partial derivative of the upward velocity with respect to the zero bias of the accelerometer):

[0130] (Partial derivative of altitude with respect to celestial velocity):

[0131] It is a 3-order identity matrix. It is an m-row, n-column zero matrix.

[0132] Based on the state transition matrix, the prior state estimation and prior covariance recursion are as follows: Prior state estimation: ,in This is a state propagation model; Prior covariance recursion: ,in Let be the process noise covariance matrix. Its values ​​are: .

[0133] Calibrated based on sensor characteristics and actual test data, it is used to characterize random errors in the state propagation process.

[0134] In the real-time update of state prediction based on IMU data, accurate and robust prediction of the UAV's attitude state was achieved. Specifically, the application of quaternion state propagation equations ensured the continuity and stability of attitude updates, avoiding the singularity problem that may be caused by traditional Euler angles, and providing an accurate attitude reference for subsequent velocity and altitude calculations. Simultaneously, modeling the gyroscope bias and z-axis accelerometer bias as slowly varying stochastic processes enabled the navigation system to dynamically estimate and compensate for these sensor biases, effectively suppressing the accumulation of long-term errors and significantly improving the long-term accuracy of predictions. Building upon this, the vertical velocity propagation equation explicitly considers IMU measurement specific force, bias compensation, and gravitational acceleration, and introduces velocity noise to construct a more accurate vertical velocity prediction model, effectively reducing the impact of noise on velocity prediction. Furthermore, the altitude propagation equation employs the trapezoidal integral method, reducing discretization errors through more accurate numerical integration of velocity, thereby improving the reliability of altitude prediction. These technical features work together to enhance the state prediction process, providing more accurate and stable prior state estimates for subsequent observation updates, thereby improving the accuracy and stability of the entire multi-sensor fusion attitude calculation method.

[0135] The prior state estimate is corrected using measurement data collected by multiple sensors, including accelerometer observation updates, magnetometer observation updates, and barometer observation updates. The accelerometer uses dynamic adaptive noise covariance to adjust the observation weights, the magnetometer adaptively selects the correction method based on the difference between the measured geomagnetism and the world geomagnetic field model, and the barometer performs error calibration for altitude status. The accelerometer uses dynamic adaptive noise covariance adjustment of observation weights, including: The dynamic adaptive noise covariance of the accelerometer satisfies: ,in It is a third-order identity matrix. These are adaptive coefficients; When the drone is stationary ( and )hour, ; When in motion: .

[0136] The method for correcting the prior state estimate using measurement data collected by multiple sensors is as follows: Accelerometer observation update: Observation model: The normalized gravity vector in the navigation coordinate system is Projecting this onto the body coordinate system, the constructed observation vector is:

[0137] The measured quantity is the unitized result of the accelerometer measurement:

[0138] The observation vector is only related to the quaternion, and the partial derivatives of the other states are 0. The observation matrix is:

[0139] Observation noise matrix: A dynamic adaptive noise covariance is adopted, and the noise weights are adjusted according to the carrier motion state to improve the adaptability of observation updates.

[0140] in, For adaptive coefficients: In a static state ( and ): ; In motion: The more intense the movement, the higher the noise weight.

[0141] Magnetometer observation update: Based on the discrepancy between measured geomagnetism and geomagnetism calculated using the global geomagnetic field model, an adaptive correction method is selected to balance accuracy and stability: Difference calculation: Transform the original magnetometer measurements to the navigation coordinate system using a rotation matrix:

[0142] Difference index calculation: Calculate the Euclidean distance between the measured geomagnetic vector and the reference vector under the world geomagnetic field model. This distance directly reflects the degree of interference in geomagnetic measurements; Threshold setting: Set the difference threshold The subsequent correction model is selected based on the threshold.

[0143] Observation model without significant magnetic interference : When the difference is small, it indicates that the magnetometer measurement is reliable and there is no significant geomagnetic interference. The complete geomagnetic information is preserved through triaxial geomagnetic observation, which improves the correction accuracy.

[0144] Observation model: The magnetic force vector in the navigation coordinate system is Projecting this onto the body coordinate system, the constructed observation vector is:

[0145] The measured quantity is the original measurement value of the magnetometer:

[0146] The observation vector is only related to the quaternion, and the partial derivatives of the other states are 0. The observation matrix is:

[0147]

[0148] Observation noise matrix: Based on the noise characteristics calibrated from the static test of the magnetometer, a fixed matrix is ​​set. .

[0149] Observation model of strong magnetic interference : When the difference is large, it indicates that the magnetometer measurement is subject to strong electromagnetic interference, and triaxial observation is prone to introducing errors. By correcting the difference, a smooth transition based on historical attitude can be achieved, thus ensuring attitude stability. Target quaternion construction: Based on the current horizontal attitude, level the magnetometer and level the magnetometer output to... Then the heading angle is:

[0150] Based on the horizontal attitude and the calculated heading angle, construct the target quaternion. ; Dot product judgment and shortest path guarantee: Calculate the quaternion of the current state With the target quaternion dot product When the dot product is less than 0, the target quaternion needs to be... Invert the value to avoid the orientation rotating around the long path during interpolation; Spherical linear interpolation: Set the interpolation ratio Interpolate as follows:

[0151]

[0152] This allows for a smooth transition between quaternions.

[0153] By normalizing the interpolated quaternions and updating the quaternion components in the state vector, the heading correction can be completed.

[0154] Barometer observation update: Observation model: The observation vector is the height in the state variables:

[0155] The measured quantity is the height obtained from the hydrostatic calculation:

[0156] The observation vector is only related to the height of the state variables; the partial derivatives of the other states are 0. The observation matrix is:

[0157] Observation noise matrix: calibrated based on barometer data to characterize measurement errors caused by pressure fluctuations.

[0158] This application introduces a dynamic adaptive noise covariance adjustment method to adjust observation weights. This method intelligently adjusts the accelerometer's observation weight in the multi-sensor fusion algorithm based on whether the UAV is stationary or in motion. When the UAV is stationary, the accelerometer's measurement data is assigned a higher weight, fully utilizing its high precision in a stable state, thereby improving the initial accuracy and stability of attitude calculation in a stationary state. When the UAV is in motion, the adaptive coefficient dynamically increases according to the acceleration deviation, causing the accelerometer's observation noise covariance to increase accordingly, thus reducing the accelerometer's observation weight. This effectively suppresses the interference of motion acceleration on accelerometer measurements, avoiding excessive correction errors introduced by unreliable accelerometer data during violent movements, and significantly enhancing the robustness and adaptability of attitude calculation in complex dynamic environments. This dynamic adaptive mechanism, combined with the overall "prediction-observation-correction" iterative mechanism, enables the UAV attitude calculation system to fuse multi-sensor data more flexibly and accurately, providing high-precision and high-reliability attitude information under various flight conditions, providing a solid foundation for the precise navigation and control of UAVs.

[0159] The magnetometer's adaptive selection and correction methods based on the difference between measured geomagnetism and the global geomagnetic field model include: Calculate the Euclidean distance between the actual measured geomagnetic data and the reference vector of the World Geomagnetic Magnetic Model. ,in ; Set the difference threshold ,when The three-axis geomagnetic observation model was used for correction. when At that time, based on the current horizontal attitude leveling magnetometer output, the heading angle is calculated and the target quaternion is constructed. The quaternion is then updated smoothly through spherical linear interpolation.

[0160] The prior state estimate is corrected using measurement data collected by multiple sensors; Accelerometer observations are updated at a frequency of 10 Hz. Observation model: The normalized gravity vector in the navigation coordinate system is Projecting this onto the body coordinate system, the constructed observation vector is:

[0161] The measured quantity is the unitized result of the accelerometer measurement:

[0162] The observation vector is only related to the quaternion, and the partial derivatives of the other states are 0. The observation matrix is:

[0163] Observation noise matrix: A dynamic adaptive noise covariance is adopted, and the noise weights are adjusted according to the carrier motion state to improve the adaptability of observation updates.

[0164] in, For adaptive coefficients: In a static state ( and ): ; In motion: The more intense the movement, the higher the noise weight.

[0165] Magnetometer observations are updated at a frequency of 10 Hz. Based on the discrepancy between measured geomagnetism and geomagnetism calculated using the global geomagnetic field model, an adaptive correction method is selected to balance accuracy and stability: Difference calculation: Transform the original magnetometer measurements to the navigation coordinate system using a rotation matrix:

[0166] Difference index calculation: Calculate the Euclidean distance between the measured geomagnetic vector and the reference vector under the world geomagnetic field model. This distance directly reflects the degree of interference in geomagnetic measurements; Threshold setting: Set the difference threshold The subsequent correction model is selected based on the threshold.

[0167] Observation model without significant magnetic interference : When the difference is small, it indicates that the magnetometer measurement is reliable and there is no significant geomagnetic interference. The complete geomagnetic information is preserved through triaxial geomagnetic observation, which improves the correction accuracy.

[0168] Observation model: The magnetic force vector in the navigation coordinate system is Projecting this onto the body coordinate system, the constructed observation vector is:

[0169] The measured quantity is the original measurement value of the magnetometer:

[0170] The observation vector is only related to the quaternion, and the partial derivatives of the other states are 0. The observation matrix is:

[0171]

[0172] Observation noise matrix: Based on the noise characteristics calibrated from the static test of the magnetometer, a fixed matrix can be set: .

[0173] Observation model of strong magnetic interference : When the difference is large, it indicates that the magnetometer measurement is subject to strong electromagnetic interference, and triaxial observation is prone to introducing errors. By correcting the difference, a smooth transition based on historical attitude can be achieved, thus ensuring attitude stability. Target quaternion construction: Based on the current horizontal attitude, level the magnetometer and level the magnetometer output to... Then the heading angle is:

[0174] Based on the horizontal attitude and the calculated heading angle, construct the target quaternion. ; Dot product judgment and shortest path guarantee: Calculate the quaternion of the current state With the target quaternion dot product When the dot product is less than 0, the target quaternion needs to be... Invert the value to avoid the orientation rotating around the long path during interpolation; Spherical linear interpolation: Set the interpolation ratio Interpolate as follows:

[0175]

[0176] This allows for a smooth transition between quaternions.

[0177] By normalizing the interpolated quaternions and updating the quaternion components in the state vector, the heading correction can be completed.

[0178] Barometer observations are updated at a frequency of 10 Hz. Observation model: The observation vector is the height in the state variables:

[0179] The measured quantity is the height obtained from the hydrostatic calculation:

[0180] The observation vector is only related to the height of the state variables; the partial derivatives of the other states are 0. The observation matrix is:

[0181] Observation noise matrix: Based on barometer data calibration, this matrix characterizes the measurement error caused by pressure fluctuations and can be set as a fixed matrix. .

[0182] It can be set to a fixed constant value or dynamically adjusted according to environmental changes, flight phases, and other factors. When the value is less than the constant value, a three-axis geomagnetic observation model is used for correction. This technical feature describes the correction strategy adopted when the deviation between the magnetometer measurement value and the theoretical model is small, i.e., the magnetic field environment is considered relatively stable and the interference is weak. The three-axis geomagnetic observation model utilizes the three axial component information of the magnetometer in the navigation coordinate system, combined with the World Geomagnetic Magnetic Model reference vector, to provide comprehensive magnetic field observation updates for attitude calculation. This correction method can make full use of all the information from the magnetometer to achieve high-precision heading and attitude correction. It can be implemented by using the magnetometer's measurement value in the navigation coordinate system as the observation vector, comparing it with the World Geomagnetic Magnetic Model reference vector, and updating the attitude state using state estimation algorithms such as Kalman filtering. When the value is greater than or equal to a constant value, the heading angle is calculated and a target quaternion is constructed based on the current horizontal attitude leveled magnetometer output. Spherical linear interpolation is used to achieve a smooth transition and update of the quaternion. This technical feature describes the correction strategy adopted when the magnetometer measurement deviates significantly from the theoretical model, indicating significant interference in the magnetic field environment. In this case, to avoid errors introduced by the disturbed vertical magnetic field component, the system levels the magnetometer output based on the currently known horizontal attitude (usually calculated by the accelerometer or IMU), using only the horizontal magnetic field component to calculate the heading angle. The leveled magnetometer output can then be used to calculate a quaternion containing only heading information, serving as the target quaternion. To avoid attitude discontinuity caused by direct jumps, methods such as spherical linear interpolation (Slerp) are used to smoothly transition the current attitude quaternion to the target quaternion, thus providing relatively stable heading information even in environments with strong interference and ensuring the continuity of attitude updates.

[0183] The general process for revising prior state estimates and updating observations includes: Calculate the Kalman filter gain ,in, For the observation matrix, The observation noise matrix; Execution status correction ,in, The sensor measurement value. For observation vectors; Update covariance And the quaternions are normalized.

[0184] The general process for revising prior state estimates and updating observations is as follows: Calculate the Kalman filter gain:

[0185] in, For the observation matrix of the corresponding sensor, The observation noise matrix; Status correction:

[0186] in, For the corresponding sensor measurement value, For the corresponding observation vector; Covariance update:

[0187] Quaternion normalization:

[0188] This application can employ the gain calculation formulas of Extended Kalman Filter (EKF) or Unscented Kalman Filter (UKF), adjusted according to the nonlinear characteristics of the system state and the observation model. Alternatively, in specific application scenarios, a simplified Kalman filter gain calculation can be used, such as directly applying the standard Kalman filter formula under a linearized model, or approximating it using a preset fixed gain matrix. The observation matrix H describes how the state vector is mapped to the observation vector. This matrix can be obtained by linearizing the observation model; for example, in EKF, H is the Jacobian matrix of the observation function with respect to the state vector. For linear observation models, H can be a constant matrix, directly determined by the system design. The observation noise matrix R represents the covariance matrix of the uncertainty or noise level in the observation data. This matrix can be obtained through sensor calibration experiments, such as performing multiple measurements on the sensor under different operating conditions and statistically analyzing the measurement error characteristics. Alternatively, it can be set based on empirical values ​​or data provided by the manufacturer and can be adaptively adjusted during system operation, for example, by estimating it through residual sequence analysis.

[0189] State corrections are performed more precisely through multiple iterations or sampling. Sensor measurements are raw or preprocessed data directly output from actual physical sensors (such as accelerometers, magnetometers, and barometers). This can be achieved by directly reading the digital output from the sensor's ADC (analog-to-digital converter) and performing unit conversion and calibration. Alternatively, the raw sensor data can be preprocessed, such as filtering and denoising, to improve the quality and stability of the measurements. The observation vector is the sensor measurement predicted by the observation model based on the current state estimate. This can be calculated by substituting the prior state estimate into the sensor's measurement model (such as an accelerometer measurement model, magnetometer measurement model, or barometer measurement model). For complex nonlinear observation models, the observation vector can also be approximated using numerical methods or lookup tables.

[0190] like Figure 5 As shown, a cyclical iterative mechanism of "prediction-observation-correction" is adopted to achieve real-time fusion of multi-sensor data and output the attitude results in real time, including: The system employs a cyclical iterative mechanism of "prediction-observation-correction," using the IMU's high-frequency sampling frequency to perform state prediction, and multiple sensors to perform observation updates alternately or in parallel at a set frequency, outputting the UAV's attitude angle, azimuth speed, and altitude information in real time. The sampling frequency of the IMU is higher than the observation update frequency of the accelerometer, magnetometer and barometer. The observation updates of each sensor are performed alternately or in parallel, and the output frequency of the attitude results is consistent with the sampling frequency of the IMU.

[0191] A cyclical iterative mechanism of "prediction-observation-correction" is adopted to achieve real-time fusion of multi-sensor data and output the attitude results in real time.

[0192] Calculate the Kalman filter gain:

[0193] in, For the observation matrix of the corresponding sensor, The observation noise matrix; Status correction:

[0194] in, For the corresponding sensor measurement value, For the corresponding observation vector; Covariance update:

[0195] Quaternion normalization:

[0196] A cyclical iterative mechanism of "prediction-observation-correction" is adopted to achieve real-time fusion of multi-sensor data and output the attitude results in real time, including: Using IMU data as the core, a state prediction model is used to achieve high-frequency recursion of attitude and bearing state, ensuring the real-time performance of the solution. Accelerometer observation updates are mainly used to correct horizontal attitude errors. They have a higher weight when stationary or moving slowly, and the weight is automatically reduced when moving violently. The observation update correction frequency can also be customized. The magnetometer observation update is based on the adaptive selection of correction methods according to the geomagnetic difference, and the heading angle error is adaptively corrected according to the interference intensity; The barometer observation update is mainly used to correct altitude errors. Combined with the integral results of the celestial velocity, it enables accurate estimation of celestial velocity and altitude. A cyclical iterative mechanism of "prediction-observation-correction" is adopted to achieve real-time fusion of multi-sensor data and output the attitude results in real time, including: State prediction is performed using IMU data at a frequency of 200 Hz, while accelerometers, magnetometers, and barometers alternately perform observation updates at a frequency of 10 Hz. Real-time output of UAV attitude and attitude results: attitude angle, azimuth velocity and altitude, with the output frequency consistent with the IMU sampling frequency; This embodiment ensures that the attitude calculation method can be directly deployed by clarifying hardware parameters and supplementing details in actual use, while taking into account both calculation accuracy and real-time performance. It is applicable to navigation and control scenarios of various UAVs such as multi-rotor and fixed-wing UAVs.

[0197] The observation updates from each sensor are executed alternately or in parallel based on the actual sensor frequency to ensure the consistency of the fusion results. This method achieves iterative state prediction and observation updates, outputting the UAV's attitude, azimuth velocity, and altitude information in real time. The output frequency matches the IMU sampling frequency, meeting the accuracy and real-time requirements of UAV navigation and control.

[0198] This application proposes a multi-sensor fusion-based method for UAV attitude calculation. This method employs a cyclical iterative mechanism of "prediction-observation-correction," using an IMU (Integrated Sensor Unit) with a high-frequency sampling rate for state prediction. Multiple sensors perform observation updates alternately or in parallel at set frequencies, outputting the UAV's attitude angle, azimuth velocity, and altitude information in real time. Specifically, the IMU's sampling frequency is higher than the observation update frequencies of the accelerometer, magnetometer, and barometer. The observation updates from each sensor are performed alternately or in parallel, and the attitude result output frequency is consistent with the IMU sampling frequency.

[0199] Specifically, the "prediction-observation-correction" iterative mechanism is the core idea of ​​Kalman filtering or its variants (such as extended Kalman filtering and unscented Kalman filtering). It gradually reduces state estimation errors by periodically performing state prediction (based on the system model) and observation updates (based on sensor measurements). The prediction phase uses the system's dynamic model to estimate the state at the next moment, while the observation update phase uses actual measurement data to correct the prediction results, thus obtaining a more accurate state estimate. This iterative characteristic allows it to continuously integrate new data and track the system state in real time. Performing state prediction at a high-frequency IMU sampling frequency means that the IMU (Inertial Measurement Unit) typically contains gyroscopes and accelerometers, providing information on the angular velocity and linear acceleration of the vehicle. Due to its fast response speed and high sampling frequency (e.g., hundreds of hertz or even higher), it is very suitable for short-term state estimation. Performing state prediction at a high-frequency IMU sampling frequency means that each time IMU data arrives, the system predicts the attitude, velocity, and other states of the UAV based on the current motion state and IMU measurements using a kinematic model. This ensures the continuity of state estimation and timely response to rapid dynamic changes in the carrier. Multi-sensor alternating or parallel observation updates at a set frequency refer to the fact that, in addition to the IMU, UAVs are typically equipped with other sensors such as magnetometers and barometers. The sampling frequency of these sensors is usually lower than that of the IMU. To fully utilize this complementary information, the system updates observations at a preset frequency based on the characteristics and data update frequency of each sensor. Alternating execution means that the system uses different sensor data in turn for observation updates at different times. For example, within an IMU prediction cycle, the magnetometer observation update might be performed first, followed by the barometer observation update in the next cycle. This method simplifies the processing logic and avoids the computational burden of processing multiple observation data simultaneously. Parallel execution means that the system processes data from multiple sensors simultaneously for observation updates at the same time point or within a similar time window. For example, when data from the magnetometer and barometer are available simultaneously, the system can calculate their respective observation residuals and Kalman gains in parallel and correct the state. This method maximizes the utilization of sensor information and improves fusion efficiency. Regardless of the method used, the core is to effectively integrate sensor data from different frequencies while ensuring real-time performance. Real-time output of UAV attitude angles, vertical velocity, and altitude information refers to the fact that attitude angles (such as roll angle, pitch angle, and yaw angle), vertical velocity, and altitude are key information for UAV navigation and control. Real-time output of this information means the system can provide the latest navigation status at a sufficiently high frequency to meet the timeliness requirements of the UAV flight control system. This is crucial for stable flight, accurate path tracking, and obstacle avoidance functions of the UAV.The sampling frequency of the IMU is higher than that of the observation update frequencies of the accelerometer, magnetometer, and barometer, which is a common configuration in multi-sensor fusion systems. The high-frequency data from the IMU is used to capture the rapid dynamics of the vehicle, while other sensors (such as accelerometers, magnetometers, and barometers), although at lower frequencies, provide absolute position or orientation references to correct for errors caused by long-term IMU drift. The high-frequency predictions from the IMU can compensate for the time intervals between low-frequency observation updates, ensuring the continuity and smoothness of state estimation. The alternating or parallel execution of sensor observation updates further refines the multi-sensor observation update strategy. For example, in the main loop, prediction is performed whenever IMU data arrives. Then, based on a preset counter or timestamp, it is determined whether a magnetometer update is needed. If so, the magnetometer observation update is performed. At the next time point that meets the conditions, the barometer observation update is then performed. This approach effectively manages computational resources and avoids processing too much data at the same time. Alternatively, the system can assign independent tasks or threads to each low-frequency sensor. When data from a particular sensor becomes ready, its corresponding observation update task is triggered and executed simultaneously with update tasks from other sensors. This approach maximizes the use of multi-core processor resources, shortens overall processing time, and improves the real-time performance of data fusion. The choice between these two execution methods depends on the specific hardware platform, computing resources, and real-time requirements. The attitude result output frequency is consistent with the IMU sampling frequency, ensuring that the frequency of the final output attitude results (attitude angle, azimuth velocity, altitude) matches the IMU's sampling frequency. This means the system can provide continuous and smooth navigation information at the highest possible frequency. Even if other sensors have lower observation update frequencies, the system can still output a state estimate, updated with the latest predictions and (possibly) observation corrections, immediately after each IMU data update, thanks to the IMU's high-frequency predictions. This guarantees the timeliness and continuity of the output data, which is crucial for flight control systems requiring high refresh rates.

[0200] This application constructs an efficient and real-time multi-sensor fusion framework based on the fundamental "prediction-observation-correction" iterative mechanism by refining the processing frequency and update strategy of different sensors. This method uses high-frequency sampling data from the IMU as the driving force to continuously predict the state. The IMU's rapid response characteristics enable it to capture the rapid dynamic changes of the UAV within a short period, thus providing a timely and relatively accurate prior state estimate with each IMU data update. This high-frequency prediction process provides continuous and high-quality initial values ​​for subsequent observation updates, effectively compensating for the information lag caused by the lower sampling frequencies of other sensors. Based on this, for other sensors with relatively low sampling frequencies, such as the accelerometer, magnetometer, and barometer, this application employs an alternating or parallel approach to perform observation updates. This means that the system does not passively wait for all sensor data to arrive simultaneously, but flexibly performs observation corrections based on the availability and preset frequency of each sensor. For example, when magnetometer data is available, the system uses it for attitude (especially heading) correction; when barometer data is available, it is used for altitude calibration. This flexible update strategy avoids the problems of low fusion efficiency and output delay caused by waiting for low-frequency sensor data. By combining high-frequency prediction from the IMU with flexible observation updates from multiple sensors, this application ensures that the system can perform state estimation based on the latest IMU data at any time and make timely corrections when other sensor data becomes available. Finally, the output frequency of the attitude result is set to be consistent with the sampling frequency of the IMU. This means that even when low-frequency sensor data is not updated, the system can provide a continuous and smooth attitude estimate through high-frequency prediction from the IMU, thereby ensuring the real-time performance, continuity, and high refresh rate of the output data, meeting the stringent requirements of UAV navigation and control for data timeliness. This mechanism effectively solves the problems of fusion efficiency, real-time performance, and accuracy caused by the mismatch of sampling frequencies of different sensors, enabling the entire attitude calculation system to operate stably and efficiently.

[0201] The UAV's IMU has a sampling frequency of 200Hz, while the accelerometer, magnetometer, and barometer all have a sampling frequency of 50Hz. In the flight controller's main loop, this method can be implemented as follows: The system's main loop runs at 200Hz, reading the latest IMU data in each iteration. In each loop, a state prediction step based on IMU data is first executed. Using measurements from the gyroscope and accelerometer, combined with current attitude, velocity, and altitude estimates, prior estimates of the attitude quaternion, azimuth velocity, and altitude for the next moment are calculated. Subsequently, the system checks whether the data from other sensors has been updated. For example, a counter can be set to trigger an update of the accelerometer, magnetometer, and barometer observations every four main loop iterations (i.e., after 20ms, corresponding to a 50Hz cycle). Parallel processing can be used when triggering observation updates: for example, different tasks or threads in the flight controller can simultaneously process the latest measurements from the accelerometer, magnetometer, and barometer. Each task independently calculates its corresponding observation residual and Kalman gain and corrects the master state estimate. For example, one task uses accelerometer data to correct attitude and z-axis accelerometer bias, another uses magnetometer data to correct heading angle, and yet another uses barometer data to correct altitude. At the end of each main cycle, regardless of whether an observation update has been performed, the system outputs the latest attitude angle, azimuth velocity, and altitude information to the flight control module at a frequency of 200Hz. Even during the three IMU cycles without observation updates, the output attitude results are based on the latest IMU predictions, thus ensuring output continuity and a high refresh rate.

[0202] This application uses the high-frequency sampling frequency of the IMU to perform state prediction, ensuring that the system can respond promptly to the rapid dynamic changes of the UAV and providing continuous and high-precision short-term predictions for state estimation. Simultaneously, multiple sensors perform observation updates alternately or in parallel at a set frequency, enabling the system to flexibly and efficiently utilize complementary information from low-frequency sensors for correction, avoiding unnecessary waiting and wasted computational resources. The attitude result output frequency is consistent with the IMU sampling frequency, ensuring the real-time nature, continuity, and high refresh rate of the output data, providing stable, reliable, and timely navigation information for the UAV flight control system. This optimized iterative mechanism significantly improves the overall performance of attitude calculation, enabling the UAV to achieve accurate and stable navigation and control even in complex and changing environments. The embodiments of this application have been described above. However, these embodiments are merely illustrative and not intended to limit the scope of this application. Although various embodiments have been described above, this does not mean that the measures in the various embodiments cannot be used advantageously in combination. Various substitutions and modifications can be made by those skilled in the art without departing from the scope of this application, and these substitutions and modifications should all fall within the scope of this application.

Claims

1. A method for calculating the attitude of a UAV based on multi-sensor fusion, characterized in that, include: Initialize the navigation state, which includes the state information required for the attitude and bearing calculation process; Construction of sensor measurement model; Real-time updates based on IMU data complete state prediction and provide initial predicted values ​​for observation updates; The prior state estimate is corrected using measurement data collected by multiple sensors; A cyclical iterative mechanism of "prediction-observation-correction" is adopted to achieve real-time fusion of multi-sensor data and output the attitude results in real time.

2. The UAV attitude calculation method based on multi-sensor fusion according to claim 1, characterized in that, The initial navigation state, encompassing the state information required for the attitude calculation process, includes: The state vector of the navigation state includes attitude quaternions, gyroscope zero bias, z-axis accelerometer zero bias, celestial velocity, and altitude. The quaternions are calculated based on the initial measurement data of the accelerometer and magnetometer combined with magnetic declination compensation. The gyroscope zero bias and z-axis accelerometer zero bias are initialized to zero, and the altitude is initialized to barometric altitude.

3. The UAV attitude calculation method based on multi-sensor fusion according to claim 2, characterized in that, The initialization process for quaternions includes: Collect the output of the triaxial accelerometer over a period of time and average it to calculate the roll and pitch angles. Based on the roll angle and pitch angle, the quaternion of the leveling magnetometer output is obtained, and the output of the triaxial magnetometer is leveled. The heading angle is obtained by combining the local magnetic declination compensation calculated by the world geomagnetic field model, and finally the quaternion initialization is completed based on the three-axis attitude angle.

4. The UAV attitude calculation method based on multi-sensor fusion according to claim 1, characterized in that, The construction of the sensor measurement model includes: The sensors include an IMU, a magnetometer, and a barometer. Measurement models for the gyroscope, accelerometer, magnetometer, and barometer are established respectively to clarify the relationship between the actual physical quantity and the measured value, zero bias, and noise. Real-time updates based on IMU data enable state prediction. State recursion is achieved through the state propagation equation, and a state transition matrix is ​​constructed to complete prior state estimation and prior covariance recursion.

5. The UAV attitude calculation method based on multi-sensor fusion according to claim 4, characterized in that, Measurement models for the gyroscope, accelerometer, magnetometer, and barometer were established respectively. Establish an IMU measurement model; Establish a gyroscope measurement model, wherein the gyroscope measurement model satisfies: ,in, This represents the true angular velocity of the carrier. To achieve zero bias in the gyroscope, The Gaussian white noise of the gyroscope. It is a projection of the Earth's rotational angular rate and can be ignored; An accelerometer measurement model is established, which satisfies the following: ,in, To ensure the true comparison of the carriers, To achieve zero bias in the accelerometer, Gaussian white noise for accelerometers; Establish a magnetometer measurement model, which satisfies the following: ,in, This is the geomagnetic reference vector in the navigation coordinate system. The rotation matrix from the machine coordinate system to the navigation coordinate system. It is Gaussian white noise; Establish a barometer measurement model, wherein the barometer measurement model satisfies: ,in, To measure the static pressure value, Noise for altitude measurement.

6. The UAV attitude calculation method based on multi-sensor fusion according to claim 4, characterized in that, The real-time update completion status prediction based on IMU data includes: The quaternion state propagation equation is updated through quaternion multiplication; Both the gyroscope zero bias and the z-axis accelerometer zero bias are modeled as slowly varying stochastic processes; The equation for the propagation of celestial velocity satisfies: ,in It is the acceleration due to gravity. This is velocity noise; The altitude propagation equation uses the trapezoidal integral method: ,in, Noise for altitude measurement.

7. The UAV attitude calculation method based on multi-sensor fusion according to claim 1, characterized in that, The prior state estimate is corrected using measurement data collected by multiple sensors, including accelerometer observation updates, magnetometer observation updates, and barometer observation updates. The accelerometer uses dynamic adaptive noise covariance to adjust the observation weights, the magnetometer adaptively selects the correction method based on the difference between the measured geomagnetism and the world geomagnetic field model, and the barometer performs error calibration for altitude status. The accelerometer employs dynamic adaptive noise covariance adjustment of observation weights, including: The dynamic adaptive noise covariance of the accelerometer satisfies: ,in It is a third-order identity matrix. These are adaptive coefficients; When the drone is stationary ( and )hour, ; When in motion: .

8. The UAV attitude calculation method based on multi-sensor fusion according to claim 7, characterized in that, The magnetometer adaptively selects correction methods based on the difference between measured geomagnetism and the global geomagnetic field model, including: Calculate the Euclidean distance between the actual measured geomagnetic data and the reference vector of the World Geomagnetic Magnetic Model. ,in ; Set the difference threshold ,when The three-axis geomagnetic observation model was used for correction. when At that time, based on the current horizontal attitude leveling magnetometer output, the heading angle is calculated and the target quaternion is constructed. The quaternion is then updated smoothly through spherical linear interpolation.

9. The UAV attitude calculation method based on multi-sensor fusion according to claim 1, characterized in that, The general process for correcting prior state estimates and updating observations includes: Calculate the Kalman filter gain ,in, For the observation matrix, The observation noise matrix; Execution status correction ,in, The sensor measurement value. For observation vectors; Update covariance And the quaternions are normalized.

10. The UAV attitude calculation method based on multi-sensor fusion according to claim 1, characterized in that, The aforementioned iterative "prediction-observation-correction" mechanism enables real-time fusion of multi-sensor data and real-time output of attitude results, including: The system employs a cyclical iterative mechanism of "prediction-observation-correction," using the IMU's high-frequency sampling frequency to perform state prediction. Multiple sensors perform observation updates alternately or in parallel at a set frequency, outputting the UAV's attitude angle, azimuth speed, and altitude information in real time. The sampling frequency of the IMU is higher than the observation update frequency of the accelerometer, magnetometer and barometer. The observation updates of each sensor are performed alternately or in parallel, and the output frequency of the attitude results is consistent with the sampling frequency of the IMU.

Images