Attitude monitoring method and system in navigation of aircraft

Abstract

The invention provides an attitude monitoring method and system in navigation of an aircraft, and the method comprises the steps: determining a process noise covariance matrix through a high-frequency energy component of an angular velocity signal measured by a current gyroscope in each filtering period; predicting a prior state vector and a prior covariance matrix at the current moment according to the state vector at the previous moment, the angular velocity measured by the current gyroscope and the process noise covariance matrix; generating an asymmetric Sigma point set and determining a measurement noise covariance matrix; the asymmetric Sigma point set is substituted into a measurement model, and a predicted measurement value and a predicted measurement covariance are obtained through unscented transformation; calculating Kalman gain based on the predicted measurement covariance and the measurement noise covariance matrix, and performing weighted gating test on innovation formed by a real measurement value and a predicted measurement value; and updating a state vector and a covariance matrix on the basis of the adjusted Kalman gain and information so as to obtain attitude information of the aircraft at the current moment.

Description

Technical Field

[0001] The present application relates to the field of aircraft navigation, and in particular to a method and system for attitude monitoring during aircraft navigation. Background Art

[0002] Vehicle attitude monitoring is a key technology in the field of navigation, guidance, and control, crucial for ensuring safe navigation and successful mission execution for a wide range of vehicles, including ships, aircraft, submarines, and spacecraft. A vehicle's attitude information, typically referring to its roll, pitch, and yaw angles relative to a specific reference coordinate system, is fundamental to precise control and stable navigation. Currently, the mainstream technology for attitude monitoring relies on multi-sensor information fusion based on inertial measurement units and magnetometers. The inertial measurement unit (IMU) consists of a three-axis gyroscope and a three-axis accelerometer. The gyroscope measures angular velocity, which is integrated to obtain attitude angles. However, its measurements have inherent bias and noise, which can cause integration errors to accumulate over time, a phenomenon known as drift. The accelerometer measures the gravity acceleration vector in static or uniform motion conditions and is used to correct for pitch and roll errors generated by the gyroscope. However, when the vehicle is undergoing maneuvering acceleration, its output struggles to accurately separate the gravity and maneuvering acceleration components. The magnetometer measures the Earth's magnetic field vector to correct for yaw angles, but is highly susceptible to magnetic interference from the vehicle's own components and the external environment, leading to inaccurate heading information. The Kalman filter method is widely used in vehicle attitude monitoring. However, when the vehicle switches from stable navigation to violent maneuvers, or when the sensor's noise characteristics change due to factors such as temperature and vibration, attitude estimation can suffer from overshoot and delay. Furthermore, sensors may experience transient outliers or be subject to shock disturbances, resulting in biased, non-Gaussian distributions of the true state error. Existing methods for handling magnetic field interference often rely on offline calibration, which cannot adapt to magnetic field variations caused by motor startup, load changes, or proximity to large ferromagnetic objects. This makes it difficult to ensure the long-term reliability of heading angles. All of these factors affect the accuracy of attitude monitoring during navigation. Summary of the Invention

[0003] In order to improve the accuracy of attitude monitoring during aircraft navigation, and thus improve the accuracy of navigation, the present invention proposes a method for attitude monitoring during aircraft navigation, comprising: Initialize a state vector and a covariance matrix of the state vector, wherein the state vector includes at least a quaternion of the vehicle attitude, a real-time bias of a three-axis gyroscope, a real-time bias of a three-axis accelerometer, and a three-axis magnetic field disturbance vector in the vehicle body coordinate system; in each filtering cycle, determine a process noise covariance matrix using the high-frequency energy component of the angular velocity signal measured by the current gyroscope, and predict a priori state vector and a priori covariance matrix at the current moment based on the state vector at the previous moment, the angular velocity measured by the current gyroscope, and the process noise covariance matrix; generate an asymmetric Sigma point set based on the priori state vector and the prior covariance matrix, wherein the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error; The measurement noise covariance matrix is ​​determined using the statistical variance of the latest innovation sequence within a preset sliding time window, and the component of the measurement noise covariance matrix corresponding to the magnetometer measurement value is increased according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm; The asymmetric Sigma point set is substituted into the measurement model, and the predicted measurement value and the predicted measurement covariance are obtained through untraceable transformation; the Kalman gain is calculated based on the predicted measurement covariance and the measurement noise covariance matrix, and a weighted gated check is performed on the new information consisting of the true measurement value and the predicted measurement value; based on the adjusted Kalman gain and the new information, the state vector and the covariance matrix are updated to obtain the attitude information of the aircraft at the current moment.

[0004] Preferably, the process noise covariance matrix of determining the high-frequency energy component of the angular velocity signal measured by the current gyroscope includes: Get the gyroscope's three-axis angular velocity measurement value at the current sampling moment; The high-frequency component of the angular velocity signal is obtained by performing a vector difference between the current angular velocity measurement value and the angular velocity measurement value at the previous moment; Calculating the energy of the high-frequency component to obtain a high-frequency energy value; The high frequency energy value is multiplied by a preset scaling factor matrix and added to the basic process noise covariance matrix to obtain the process noise covariance matrix at the current moment.

[0005] Preferably, the predicting of the prior state vector and the prior covariance matrix at the current moment based on the state vector at the previous moment, the angular velocity measured by the current gyroscope, and the process noise covariance matrix includes: Extract the attitude quaternion and gyroscope bias from the posterior state vector at the previous moment; Compensating the gyroscope angular velocity measurement value at the current moment using the gyroscope bias to obtain a compensated angular velocity; Based on the compensated angular velocity, the predicted attitude quaternion at the current moment is obtained by integrating the quaternion kinematic differential equation; The gyroscope bias, accelerometer bias, and magnetic field disturbance vector at the previous moment are used as the predicted value at the current moment, and are combined with the predicted attitude quaternion to obtain the prior state vector at the current moment; The Sigma point set at the previous moment is calculated through the quaternion kinematic differential equation to transfer the state covariance, and is added to the process noise covariance matrix to obtain the prior covariance matrix at the current moment.

[0006] Preferably, generating an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, wherein the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error, includes: In the sliding time window, the vector difference between the historical predicted state vector and the posterior state vector is calculated to obtain the prediction error sequence; Calculating the third-order central moment of the forecast error sequence to obtain a skewness coefficient vector; Decompose the prior covariance matrix at the current moment to obtain the lower triangular matrix; Obtaining weights of the first group of Sigma points and weights of the second group of Sigma points, wherein a difference between the weights of the first group of Sigma points and the weights of the second group of Sigma points is proportional to the skewness coefficient vector; A Sigma point set asymmetrically distributed in a state space is generated using the priori state vector, the lower triangular matrix, and the first and second groups of weights.

[0007] Preferably, the determining of the measurement noise covariance matrix by using the statistical variance of the latest innovation sequence within a preset sliding time window includes: Obtain and calculate the difference between the actual measurement value at the current moment and the predicted measurement value to obtain the new information vector at the current moment; The new information vector at the current moment is stored in a sliding time window with a preset length of M to obtain the new information sequence; Calculating the covariance of the innovation sequence to obtain an innovation covariance matrix; The covariance of the state prediction in the measurement space obtained by unscented transformation is subtracted from the innovation covariance matrix to obtain the measurement noise covariance matrix at the current moment.

[0008] Preferably, the step of increasing the component corresponding to the magnetometer measurement value in the measurement noise covariance matrix according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm includes: Calculate the L2 norm of the magnetometer three-axis measurement value at the current moment, and calculate the L2 norm of the reference geomagnetic field vector at the current position obtained from the local reference geomagnetic model, and calculate the absolute value of the difference between the L2 norm of the magnetometer measurement value and the L2 norm of the reference geomagnetic field vector to obtain the norm deviation; The norm deviation is input into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1; and the diagonal elements corresponding to the three-axis measurement values ​​of the magnetometer in the measurement noise covariance matrix determined by the latest innovation sequence are multiplied by the noise scaling factor.

[0009] Preferably, performing weighted gated testing on the innovation consisting of the actual measurement value and the predicted measurement value comprises: Calculate the inverse matrix of the covariance matrix of the innovation vector, and use the innovation vector and the inverse matrix of the covariance matrix of the innovation vector to calculate the square value of the distance of the innovation; substitute the square value of the distance into the Gaussian decay function to obtain a continuous weighting factor in the range of [0,1]; The continuous weighting factor is multiplied by the unadjusted Kalman gain to obtain an adjusted Kalman gain.

[0010] The present invention also provides a system for monitoring the attitude of an aircraft during navigation, comprising: A Sigma point set generation unit is used to initialize a state vector and a covariance matrix of the state vector, wherein the state vector includes at least a quaternion of the aircraft attitude, a real-time bias of a three-axis gyroscope, a real-time bias of a three-axis accelerometer, and a three-axis magnetic field disturbance vector in the aircraft body coordinate system; in each filtering cycle, the process noise covariance matrix is ​​determined using the high-frequency energy component of the angular velocity signal measured by the current gyroscope, and the prior state vector and prior covariance matrix at the current moment are predicted based on the state vector at the previous moment and the angular velocity measured by the current gyroscope and the process noise covariance matrix; an asymmetric Sigma point set is generated based on the prior state vector and the prior covariance matrix, wherein the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error; a measurement noise covariance matrix generation unit, configured to determine the measurement noise covariance matrix using the statistical variance of the latest innovation sequence within a preset sliding time window, and to increase the component of the measurement noise covariance matrix corresponding to the magnetometer measurement value according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm; The monitoring unit is used to substitute the asymmetric Sigma point set into the measurement model, obtain the predicted measurement value and the predicted measurement covariance through unscented transformation; calculate the Kalman gain based on the predicted measurement covariance and the measurement noise covariance matrix, and perform a weighted gated check on the new information consisting of the true measurement value and the predicted measurement value; and update the state vector and the covariance matrix based on the adjusted Kalman gain and the new information to obtain the attitude information of the aircraft at the current moment.

[0011] Preferably, the process noise covariance matrix of determining the high-frequency energy component of the angular velocity signal measured by the current gyroscope includes: Get the gyroscope's three-axis angular velocity measurement value at the current sampling moment; The high-frequency component of the angular velocity signal is obtained by performing a vector difference between the current angular velocity measurement value and the angular velocity measurement value at the previous moment; Calculating the energy of the high-frequency component to obtain a high-frequency energy value; The high frequency energy value is multiplied by a preset scaling factor matrix and added to the basic process noise covariance matrix to obtain the process noise covariance matrix at the current moment.

[0012] Preferably, the predicting of the prior state vector and the prior covariance matrix at the current moment based on the state vector at the previous moment, the angular velocity measured by the current gyroscope, and the process noise covariance matrix includes: Extract the attitude quaternion and gyroscope bias from the posterior state vector at the previous moment; Compensating the gyroscope angular velocity measurement value at the current moment using the gyroscope bias to obtain a compensated angular velocity; Based on the compensated angular velocity, the predicted attitude quaternion at the current moment is obtained by integrating the quaternion kinematic differential equation; The gyroscope bias, accelerometer bias, and magnetic field disturbance vector at the previous moment are used as the predicted value at the current moment, and are combined with the predicted attitude quaternion to obtain the prior state vector at the current moment; The Sigma point set at the previous moment is calculated through the quaternion kinematic differential equation to transfer the state covariance, and is added to the process noise covariance matrix to obtain the prior covariance matrix at the current moment.

[0013] Preferably, generating an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, wherein the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error, includes: In the sliding time window, the vector difference between the historical predicted state vector and the posterior state vector is calculated to obtain the prediction error sequence; Calculating the third-order central moment of the forecast error sequence to obtain a skewness coefficient vector; Decompose the prior covariance matrix at the current moment to obtain the lower triangular matrix; Obtaining weights of the first group of Sigma points and weights of the second group of Sigma points, wherein a difference between the weights of the first group of Sigma points and the weights of the second group of Sigma points is proportional to the skewness coefficient vector; A Sigma point set asymmetrically distributed in a state space is generated using the priori state vector, the lower triangular matrix, and the first and second groups of weights.

[0014] Preferably, the determining of the measurement noise covariance matrix by using the statistical variance of the latest innovation sequence within a preset sliding time window includes: Obtain and calculate the difference between the actual measurement value at the current moment and the predicted measurement value to obtain the new information vector at the current moment; The new information vector at the current moment is stored in a sliding time window with a preset length of M to obtain the new information sequence; Calculating the covariance of the innovation sequence to obtain an innovation covariance matrix; The covariance of the state prediction in the measurement space obtained by unscented transformation is subtracted from the innovation covariance matrix to obtain the measurement noise covariance matrix at the current moment.

[0015] Preferably, the step of increasing the component corresponding to the magnetometer measurement value in the measurement noise covariance matrix according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm includes: Calculate the L2 norm of the magnetometer three-axis measurement value at the current moment, and calculate the L2 norm of the reference geomagnetic field vector at the current position obtained from the local reference geomagnetic model, and calculate the absolute value of the difference between the L2 norm of the magnetometer measurement value and the L2 norm of the reference geomagnetic field vector to obtain the norm deviation; The norm deviation is input into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1; and the diagonal elements corresponding to the three-axis measurement values ​​of the magnetometer in the measurement noise covariance matrix determined by the latest innovation sequence are multiplied by the noise scaling factor.

[0016] Preferably, performing weighted gated testing on the innovation consisting of the actual measurement value and the predicted measurement value comprises: Calculate the inverse matrix of the covariance matrix of the innovation vector, and use the innovation vector and the inverse matrix of the covariance matrix of the innovation vector to calculate the square value of the distance of the innovation; substitute the square value of the distance into the Gaussian decay function to obtain a continuous weighting factor in the range of [0,1]; The continuous weighting factor is multiplied by the unadjusted Kalman gain to obtain an adjusted Kalman gain.

[0017] Compared with the existing technology, the present invention additionally introduces the real-time bias of the three-axis accelerometer and the three-axis magnetic field disturbance vector into the state vector, which more accurately separates the gravity acceleration vector and the linear acceleration of the aircraft itself, thereby improving the reliability of the heading angle; and the asymmetric Sigma point generated based on the third-order moment of the historical error enables the filter to better fit the non-Gaussian error distribution, thereby suppressing the estimation bias caused by the asymmetric disturbance of the system; by adjusting the measurement noise covariance matrix and the process noise covariance matrix, the optimal filtering performance can be maintained when the aircraft switches between dynamic and static and the sensor noise changes; and by performing weighted soft gating test on the new information, the impact of data mutation on the system is avoided. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 This is a flow chart of Example 1; Figure 2 Schematic diagram generated for the process noise covariance matrix; Figure 3 Schematic diagram comparing symmetric Sigma point set and asymmetric Sigma point set; Figure 4 Schematic diagram for increasing the components of the measurement noise covariance matrix corresponding to the magnetometer measurements; Figure 5 Schematic diagram of the gain adjustment factor. DETAILED DESCRIPTION

[0019] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the embodiments described are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without making creative efforts are within the scope of protection of this application.

[0020] Specific embodiments, such as Figure 1 As shown, the present application provides a method for monitoring the attitude of an aircraft during navigation, comprising: S1, initialize the state vector and the covariance matrix of the state vector, wherein the state vector includes at least the quaternion of the aircraft attitude, the real-time bias of the three-axis gyroscope, the real-time bias of the three-axis accelerometer, and the three-axis magnetic field disturbance vector in the aircraft body coordinate system; in each filtering cycle, determine the process noise covariance matrix using the high-frequency energy component of the angular velocity signal measured by the current gyroscope, and predict the prior state vector and prior covariance matrix at the current moment based on the state vector at the previous moment and the angular velocity measured by the current gyroscope and the process noise covariance matrix; generate an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, wherein the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error; The state vector is used to describe the state of the aircraft. The state vector includes at least attitude quaternion, gyroscope bias, accelerometer bias and magnetic field disturbance vector. The attitude quaternion represents the orientation of the aircraft in three-dimensional space. The use of quaternion can avoid the universal joint deadlock problem of Euler angle. The gyroscope bias is due to the zero-point drift of the gyroscope, and the three values ​​represent the current drift size. The accelerometer bias is three values ​​used to calibrate the zero-point bias of the accelerometer in real time. The magnetic field disturbance vector is three values ​​representing the interference field of the internal magnetic field generated by the aircraft's own motor and metal hull. The present invention introduces the accelerometer bias into the state vector, which can more accurately separate the gravity acceleration vector and the aircraft's own linear acceleration. The magnetic field disturbance vector is introduced, and the magnetic field disturbance is used as a state quantity to be estimated. The filter can compensate for local interference in real time and improve the stability and reliability of the heading angle.

[0021] Monitor the smoothness of the gyroscope angular velocity signal, for example, by separating the high-frequency jitter components in the signal through a high-pass filter. When the aircraft is sailing at a constant speed in calm waters, there are few high-frequency components, low energy, and the process noise covariance matrix is ​​very small, indicating that the prediction based on the current motion is very confident. When the aircraft suddenly starts the thrusters or encounters turbulence, the angular velocity signal shakes violently and the high-frequency energy soars, then the process noise covariance matrix is ​​increased, which means that the trust in the motion model is reduced, and more reliance is placed on subsequent actual sensor measurements for correction. Based on the best attitude estimate at the previous moment, combined with the bias-compensated gyroscope readings, the attitude at the next moment is calculated through the quaternion kinematic equation. For other error terms in the state vector, such as various biases and disturbances, due to slow changes, the predicted value is equal to the best estimate at the previous moment. The state uncertainty at the previous moment, that is, the covariance matrix, is also passed through the kinematic model and superimposed with the process noise covariance matrix calculated in the previous step. In one embodiment, the process noise covariance matrix is ​​generated as follows Figure 2 shown.

[0022] Representative sample points, or Sigma points, are selected from the probability cloud representing the current predicted attitude and uncertainty for subsequent verification. The present invention obtains the recent prediction error history. If the filter consistently overpredicts roll angles, more or more heavily weighted sample points are generated in directions with lower roll angles when generating Sigma points. Asymmetry is determined by the third-order moment of the historical prediction error, or skewness, which corrects for non-Gaussian prediction biases.

[0023] S2, using the statistical variance of the latest innovation sequence within a preset sliding time window to determine the measurement noise covariance matrix, and increasing the component of the measurement noise covariance matrix corresponding to the magnetometer measurement value according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm; The predicted measurement value is compared with the actual measurement value, and the difference between the two is calculated to obtain the innovation. Within a sliding time window, exemplarily a window of 20 sampling periods, the dispersion of the innovation sequence within the window is statistically analyzed. If the innovation is consistently small and stable, indicating that the measurement value is reliable, the measurement noise covariance matrix is ​​adjusted down. Conversely, if the innovation varies significantly, indicating that the measurement value is very noisy, the measurement noise covariance matrix is ​​adjusted up.

[0024] The total strength of the Earth's magnetic field is essentially constant and can be found in the built-in global geomagnetic model. The vector length of the current magnetometer's three-axis reading, known as the magnetic field norm, is calculated and compared with the reference model value. When the vehicle approaches an underwater pipeline or the steel structure of a sunken ship, its own magnetic field is severely disturbed, causing the measured norm to deviate sharply from the reference value. If a deviation exceeding a preset threshold is detected, the values ​​in the measurement noise covariance matrix belonging to the magnetometer channel are multiplied by a penalty factor, such as 100. This signals to the filter that the magnetometer reading is unreliable and its heading information is ignored in this update.

[0025] S3, substitute the asymmetric Sigma point set into the measurement model, and obtain the predicted measurement value and the predicted measurement covariance through untraceable transformation; calculate the Kalman gain based on the predicted measurement covariance and the measurement noise covariance matrix, and perform weighted gated test on the new information consisting of the true measurement value and the predicted measurement value; based on the adjusted Kalman gain and the new information, update the state vector and the covariance matrix to obtain the attitude information of the aircraft at the current moment.

[0026] Each asymmetric Sigma point generated in S1 is converted through the measurement model, the values ​​of the accelerometer and magnetometer under the assumed state are calculated, and the mean and covariance of the measured values ​​are statistically predicted. In one embodiment, the measurement model includes at least two sub-models, which are distributed as an accelerometer measurement model and a magnetometer measurement model. Preferably, the accelerometer measurement model is , where C(q) is the rotation matrix from the geographic coordinate system to the vehicle coordinate system. Specifically, the current attitude quaternion is extracted from the state vector, and then C(q) is obtained based on the attitude quaternion. is the gravity vector in geographic coordinates, is the current accelerometer bias extracted from the state vector. Preferably, the magnetometer measurement model is , is the reference geomagnetic vector, To extract the current magnetic field disturbance vector from the state vector.

[0027] The Kalman gain is calculated based on the predicted measurement covariance and the measurement noise covariance matrix determined in S2. Before updating the state, the new information vector is calculated, that is, the difference between the true measurement value and the predicted measurement value mean, and the distance between this difference and the theoretical covariance. Preferably, the Mahalanobis distance is used. If the distance is very low, it means that the measurement value is within expectations, and a weighting factor close to 1 is used. If the distance is large, for example, a sensor suddenly outputs a limit value, a weighting factor close to 0 is calculated based on the distance, and the Kalman gain is scaled by the weighting factor. This can prevent outliers from contaminating the system while retaining some useful information in the critical value. Using the Kalman gain adjusted by the weighting factor, the predicted state vector is corrected to obtain the most accurate posterior state vector at the current moment. The attitude quaternion is extracted from the vector to obtain the current stable and reliable attitude information of the aircraft. Those skilled in the art will know that the various errors in the state vector will also be updated.

[0028] In an optional embodiment, the process noise covariance matrix is ​​determined by using the high-frequency energy component of the angular velocity signal currently measured by the gyroscope, including: Get the gyroscope's three-axis angular velocity measurement value at the current sampling moment; The high-frequency component of the angular velocity signal is obtained by performing a vector difference between the current angular velocity measurement value and the angular velocity measurement value at the previous moment; Calculating the energy of the high-frequency component to obtain a high-frequency energy value; The high frequency energy value is multiplied by a preset scaling factor matrix and added to the basic process noise covariance matrix to obtain the process noise covariance matrix at the current moment.

[0029] Specifically, when a vehicle slowly translates or hovers in water, its motion is stable, and the gyroscope's three-axis angular velocity readings vary minimally. For example, if the previous reading was 0.01 degrees per second of X-axis rotation, and the current reading is 0.011 degrees per second, the difference between the two will produce a high-frequency component that is almost zero, and its energy value will also approach zero, indicating that the vehicle is in a predictable, stable state. Conversely, when the vehicle's robotic arm suddenly extends or its thrusters suddenly exert force to avoid an obstacle, a rapid rotation occurs, and the gyroscope reading may instantly jump from 0.1 degrees per second to 5 degrees per second. The difference between the two readings at this moment is significant, and the calculated high-frequency energy value will be very large. The calculated high-frequency energy value, which measures the intensity of the vehicle's current motion, is multiplied by a pre-set scaling factor matrix, which converts energy units into noise variance units that match the state. The energy value is then added to a base process noise matrix representing the system's lowest uncertainty to produce the final process noise covariance matrix. When the vehicle is hovering steadily, the high-frequency energy is minimal, so the process noise matrix remains low and the prediction model is highly reliable. However, when the vehicle is in violent motion, the high-frequency energy significantly increases the process noise matrix, reducing the reliability of the prediction model. Therefore, more actual measurement data from the accelerometer and magnetometer must be used for correction to ensure that the attitude estimate can keep up with the violent motion.

[0030] In an optional embodiment, predicting the prior state vector and the prior covariance matrix at the current moment based on the state vector at the previous moment, the angular velocity measured by the current gyroscope, and the process noise covariance matrix includes: Extract the attitude quaternion and gyroscope bias from the posterior state vector at the previous moment; Compensating the gyroscope angular velocity measurement value at the current moment using the gyroscope bias to obtain a compensated angular velocity; Based on the compensated angular velocity, the predicted attitude quaternion at the current moment is obtained by integrating the quaternion kinematic differential equation; The gyroscope bias, accelerometer bias, and magnetic field disturbance vector at the previous moment are used as the predicted value at the current moment, and are combined with the predicted attitude quaternion to obtain the prior state vector at the current moment; The Sigma point set at the previous moment is calculated through the quaternion kinematic differential equation to transfer the state covariance, and is added to the process noise covariance matrix to obtain the prior covariance matrix at the current moment.

[0031] Specifically, if the spacecraft completed a filter update at the previous moment k-1, it obtained its precise attitude at that time, and at the same time obtained the zero-point bias of the gyroscope of about 0.1 degrees per second around the Z axis. At the current moment k, the spacecraft's gyroscope transmits back an original Z-axis angular velocity reading of 9.9 degrees per second. From the complete state file of the previous moment, since the state vector contains the attitude quaternion and the gyroscope bias, the attitude quaternion and the 0.1-degree Z-axis bias are extracted according to the position in the state vector. The 9.9-degree original reading is subtracted from the 0.1-degree bias to obtain an angular velocity of 9.8 degrees per second. Using the quaternion kinematic equation, the 9.8-degree angular velocity is integrated for a very short time to calculate the predicted attitude of the spacecraft at the current moment k. Among them, the quaternion kinematic equation is the rate of change of the attitude quaternion Angular velocity with respect to the vehicle body The relationship equation is: , integrated and normalized to obtain the predicted attitude quaternion.

[0032] For other slowly varying error terms in the state, such as accelerometer bias and internal magnetic field interference, which do not change suddenly over a short period of time, the optimal estimate at the previous moment is used as the predicted value at the current moment. Combining this with the predicted attitude quaternion yields the complete prior state vector. To determine the uncertainty of the new state, the sample set of Sigma points representing the uncertainty distribution at the previous moment is substituted into the same quaternion kinematic equations for calculation. Since the initial state itself is uncertain, the discreteness of the sample points increases after the motion transfer, thus calculating the post-transfer state covariance. The calculated process noise covariance matrix is ​​added to the state covariance to obtain the resulting prior covariance matrix, which captures the total uncertainty of the predicted state at the current moment.

[0033] In an optional embodiment, generating an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, wherein the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error, includes: In the sliding time window, the vector difference between the historical predicted state vector and the posterior state vector is calculated to obtain the prediction error sequence; Calculating the third-order central moment of the forecast error sequence to obtain a skewness coefficient vector; Decompose the prior covariance matrix at the current moment to obtain the lower triangular matrix; Obtaining weights of the first group of Sigma points and weights of the second group of Sigma points, wherein a difference between the weights of the first group of Sigma points and the weights of the second group of Sigma points is proportional to the skewness coefficient vector; A Sigma point set asymmetrically distributed in a state space is generated using the priori state vector, the lower triangular matrix, and the first and second groups of weights.

[0034] Traditional unscented Kalman filtering assumes a symmetrical distribution of prediction errors, but this is not always the case in reality, for example, when a vehicle is affected by crosswinds or unilateral currents. A sliding time window is set, for example, the prediction error sequence of the most recent 50 filter cycles. Within this window, the difference between the predicted roll angle and the final confirmed roll angle is calculated. If the mean of the difference sequence is close to zero, but the majority of the data falls on the same side of zero, the distribution exhibits a skewed distribution. By calculating the third-order central moment of the error sequence, or skewness, a skewness coefficient vector is obtained, which quantifies the asymmetry. For example, the skewness coefficient vector indicates that the error distribution in the roll angle dimension is left-skewed. The prior covariance matrix of the current prediction uncertainty is decomposed to obtain a lower triangular matrix that contains the shape and orientation of the uncertainty ellipsoid. Because historical data indicates a left-skewed tendency in roll angle predictions, when generating sigma points, the weights of sample points to the right of the uncertainty ellipsoid are increased, while those to the left are decreased. The weight difference is proportional to the skewness coefficient. The predicted state vector, lower triangular matrix and asymmetric weights are used to generate a set of Sigma point sets that are asymmetrically distributed in the state space. Compared with the symmetric Sigma point set, the asymmetric Sigma point set more realistically reflects the current state. Figure 3 A comparison diagram of symmetric Sigma point set and asymmetric Sigma point set is shown.

[0035] In an optional embodiment, determining the measurement noise covariance matrix using the statistical variance of the latest innovation sequence within a preset sliding time window includes: Obtain and calculate the difference between the actual measurement value at the current moment and the predicted measurement value to obtain the new information vector at the current moment; The new information vector at the current moment is stored in a sliding time window with a preset length of M to obtain the new information sequence; Calculating the covariance of the innovation sequence to obtain an innovation covariance matrix; The covariance of the state prediction in the measurement space obtained by unscented transformation is subtracted from the innovation covariance matrix to obtain the measurement noise covariance matrix at the current moment.

[0036] Specifically, while performing its mission, the vehicle continuously converts its predicted attitude into corresponding theoretical sensor readings and compares them with the actual measurements from the accelerometer and magnetometer. The difference between the two is the innovation vector. For example, if the vehicle is predicted to be level, the corresponding Z-axis accelerometer reading should be 9.8, but the actual measurement is 9.9. Therefore, the innovation in Z-axis acceleration is -0.1. This innovation vector, along with all innovation vectors calculated over the past 30 sampling periods, forms an innovation sequence. The covariance matrix of this sequence of 30 innovation vectors is calculated. The covariance matrix reflects the overall dispersion of the differences between the predictions and measurements. This total difference consists of two components: one due to inaccurate state predictions of the system itself, and the other due to inherent noise in the sensor measurements. Using an unscented transform, the former, the covariance contribution due to uncertainty in the state prediction, is calculated. This covariance contribution due to state prediction is subtracted from the total covariance matrix of the innovation sequence, leaving the remainder theoretically due to noise and error introduced by the sensor measurement process itself. This difference is used as the measurement noise covariance matrix at the current moment. For example, when a vehicle enters turbid waters, the impact of the current on the hull causes the actual accelerometer reading to fluctuate more. This increases the overall variance of the innovation sequence, and the resulting measurement noise covariance matrix also increases accordingly, reducing confidence in the current accelerometer reading.

[0037] In an optional embodiment, increasing the component corresponding to the magnetometer measurement value in the measurement noise covariance matrix according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm includes: Calculate the L2 norm of the magnetometer three-axis measurement value at the current moment, and calculate the L2 norm of the reference geomagnetic field vector at the current position obtained from the local reference geomagnetic model, and calculate the absolute value of the difference between the L2 norm of the magnetometer measurement value and the L2 norm of the reference geomagnetic field vector to obtain the norm deviation; The norm deviation is input into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1; and the diagonal elements corresponding to the three-axis measurement values ​​of the magnetometer in the measurement noise covariance matrix determined by the latest innovation sequence are multiplied by the noise scaling factor.

[0038] The total intensity of the Earth's magnetic field in a local area is relatively stable, and the giant value can be obtained by querying the global geomagnetic model. For example, in a certain sea area, the total intensity of the reference geomagnetic model is 50μT. Under normal circumstances, when the spacecraft arrives here, the magnetometer calculates the vector length of the three-axis measurement value, that is, the L2 norm, which should also be around 50μT. However, when the spacecraft approaches an iron-rich seabed hill, or approaches a steel shipwreck, the local magnetic field will be distorted, and the total intensity measured by the magnetometer may be 200μT. The difference between the measured norm and the model reference norm is calculated in real time to be 150μT, indicating that the magnetometer reading at this time has been completely interfered with by the external environment. Substitute the deviation value into an exponential function, for example, the function is that for every 10μT increase in the deviation, the noise scaling factor doubles, such as Figure 4 As shown in the example above, the calculated noise scaling factor is 1024. The noise scaling factor is used to adjust the diagonal elements in the measurement noise covariance matrix corresponding to the three axes of the magnetometer. This means that the magnetometer's measurement noise is amplified by a factor of 1024. The weight contributed by the magnetometer is then reduced to almost zero during the subsequent calculation of the Kalman gain. Even if the magnetometer returns erroneous heading information, it is ignored. Attitude estimation relies entirely on the gyroscope's integration and the accelerometer's gravity vector correction, ensuring a stable heading angle and reducing or even preventing strong external magnetic interference.

[0039] In an optional embodiment, performing weighted gated testing on the innovation consisting of the actual measurement value and the predicted measurement value includes: Calculate the inverse matrix of the covariance matrix of the innovation vector, and use the innovation vector and the inverse matrix of the covariance matrix of the innovation vector to calculate the square value of the distance of the innovation; substitute the square value of the distance into the Gaussian decay function to obtain a continuous weighting factor in the range of [0,1]; The continuous weighting factor is multiplied by the unadjusted Kalman gain to obtain an adjusted Kalman gain.

[0040] When the aircraft attitude system is operating stably, each measurement of the new information, that is, the difference between the predicted and the actual measurement value, corresponds to a theoretical covariance matrix, which defines a multi-dimensional reasonable error range. When all the sensors of the aircraft are working properly, the new information vector falls within the reasonable error range. The square value of the Mahalanobis distance is calculated using the inverse matrix of the new information vector and its covariance matrix. When operating stably, the value will be very small, for example, 0.5. Substitute the square value of the distance, that is, 0.5, into a Gaussian attenuation function. The Gaussian attenuation function makes the output close to 1 when the input value is very small, such as Figure 5As shown. If the input is 0.5, the function may output a continuous weighting factor of 0.98. The unadjusted Kalman gain represents the trust weight of the measurement value under normal circumstances. The weighting factor is multiplied by the entire Kalman gain matrix to obtain a Kalman gain that is almost unchanged, and it is used to update the attitude normally. However, if one of the propellers of the vehicle is entangled in water plants, causing the hull to shake violently, the accelerometer will produce a huge abnormal reading, and the calculated Mahalanobis distance squared value is 50. The Gaussian decay function will output an extremely small value such as 0.01. The weighting factor of 0.01 will cause the Kalman gain to decay to close to zero, thereby almost ignoring the erroneous measurements caused by abnormal jitter during the update, ensuring the stability of the attitude estimation.

[0041] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Although the present invention has been described in detail with reference to the above embodiments, it should be understood by those skilled in the art that the technical solutions described in the above embodiments can still be modified, or some of the technical features thereof can be replaced by equivalents. However, these modifications or replacements do not deviate from the spirit and scope of the technical solutions of the embodiments of the present invention. In addition, the various different implementations of the embodiments of the present invention can also be arbitrarily combined, as long as they do not violate the ideas of the embodiments of the present invention, and they should also be regarded as the contents disclosed in the embodiments of the present invention.

Claims

1. A method for monitoring the attitude of an aircraft during navigation, characterized in that: include: Initialize a state vector and a covariance matrix of the state vector, wherein the state vector includes at least a quaternion of the aircraft attitude, a real-time bias of a three-axis gyroscope, a real-time bias of a three-axis accelerometer, and a three-axis magnetic field disturbance vector in the aircraft body coordinate system; In each filtering cycle, the high-frequency energy component of the angular velocity signal measured by the current gyroscope is used to determine the process noise covariance matrix, and the prior state vector and prior covariance matrix of the current moment are predicted based on the state vector at the previous moment, the angular velocity measured by the current gyroscope, and the process noise covariance matrix; Generate an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, where the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error; The measurement noise covariance matrix is ​​determined using the statistical variance of the latest innovation sequence within a preset sliding time window, and the component of the measurement noise covariance matrix corresponding to the magnetometer measurement value is increased according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm; Substituting the asymmetric Sigma point set into the measurement model, obtaining predicted measurement values ​​and predicted measurement covariances through unscented transformation; calculating the Kalman gain based on the predicted measurement covariances and the measurement noise covariance matrix, and performing a weighted gated test on the innovation consisting of the true measurement values ​​and the predicted measurement values; Based on the adjusted Kalman gain and new information, the state vector and covariance matrix are updated to obtain the attitude information of the aircraft at the current moment.

2. The method according to claim 1, characterized in that The method of determining a noise covariance matrix of a process using a high-frequency energy component of an angular velocity signal measured by a current gyroscope includes: Get the gyroscope's three-axis angular velocity measurement value at the current sampling moment; The high-frequency component of the angular velocity signal is obtained by performing a vector difference between the current angular velocity measurement value and the angular velocity measurement value at the previous moment; Calculating the energy of the high-frequency component to obtain a high-frequency energy value; The high frequency energy value is multiplied by a preset scaling factor matrix and added to the basic process noise covariance matrix to obtain the process noise covariance matrix at the current moment.

3. The method according to claim 1, characterized in that The method of predicting the prior state vector and the prior covariance matrix at the current moment based on the state vector at the previous moment, the angular velocity measured by the current gyroscope, and the process noise covariance matrix includes: Extract the attitude quaternion and gyroscope bias from the posterior state vector at the previous moment; Compensating the gyroscope angular velocity measurement value at the current moment using the gyroscope bias to obtain a compensated angular velocity; Based on the compensated angular velocity, the predicted attitude quaternion at the current moment is obtained by integrating the quaternion kinematic differential equation; The gyroscope bias, accelerometer bias, and magnetic field disturbance vector at the previous moment are used as the predicted value at the current moment, and are combined with the predicted attitude quaternion to obtain the prior state vector at the current moment; The Sigma point set at the previous moment is calculated through the quaternion kinematic differential equation to transfer the state covariance, and is added to the process noise covariance matrix to obtain the prior covariance matrix at the current moment.

4. The method according to claim 1, wherein The generating of an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, wherein the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error, includes: In the sliding time window, the vector difference between the historical predicted state vector and the posterior state vector is calculated to obtain the prediction error sequence; Calculating the third-order central moment of the forecast error sequence to obtain a skewness coefficient vector; Decompose the prior covariance matrix at the current moment to obtain the lower triangular matrix; Obtaining weights of the first group of Sigma points and weights of the second group of Sigma points, wherein a difference between the weights of the first group of Sigma points and the weights of the second group of Sigma points is proportional to the skewness coefficient vector; A Sigma point set asymmetrically distributed in a state space is generated using the priori state vector, the lower triangular matrix, and the first and second groups of weights.

5. The method according to claim 1, wherein The method of determining the measurement noise covariance matrix by using the statistical variance of the latest innovation sequence within a preset sliding time window includes: Obtain and calculate the difference between the actual measurement value at the current moment and the predicted measurement value to obtain the new information vector at the current moment; The new information vector at the current moment is stored in a sliding time window with a preset length of M to obtain the new information sequence; Calculating the covariance of the innovation sequence to obtain an innovation covariance matrix; The covariance of the state prediction in the measurement space obtained by unscented transformation is subtracted from the innovation covariance matrix to obtain the measurement noise covariance matrix at the current moment.

6. The method according to claim 1, characterized in that The step of increasing the component corresponding to the magnetometer measurement value in the measurement noise covariance matrix according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm includes: Calculate the L2 norm of the magnetometer three-axis measurement value at the current moment, and calculate the L2 norm of the reference geomagnetic field vector at the current position obtained from the local reference geomagnetic model, and calculate the absolute value of the difference between the L2 norm of the magnetometer measurement value and the L2 norm of the reference geomagnetic field vector to obtain the norm deviation; The norm deviation is input into a preset nonlinear exponential function to obtain a noise scaling factor greater than or equal to 1; and the diagonal elements corresponding to the three-axis measurement values ​​of the magnetometer in the measurement noise covariance matrix determined by the latest innovation sequence are multiplied by the noise scaling factor.

7. The method according to claim 1, characterized in that The weighted gated test on the innovation consisting of the real measurement value and the predicted measurement value includes: Calculate the inverse matrix of the covariance matrix of the innovation vector, and use the innovation vector and the inverse matrix of the covariance matrix of the innovation vector to calculate the square value of the distance of the innovation; substitute the square value of the distance into the Gaussian decay function to obtain a continuous weighting factor in the range of [0,1]; The continuous weighting factor is multiplied by the unadjusted Kalman gain to obtain an adjusted Kalman gain.

8. A navigation attitude monitoring system for an aircraft, characterized in that: include: A Sigma point set generation unit, configured to initialize a state vector and a covariance matrix of the state vector, wherein the state vector includes at least the quaternion of the vehicle attitude, the real-time bias of the three-axis gyroscope, the real-time bias of the three-axis accelerometer, and the three-axis magnetic field disturbance vector in the vehicle body coordinate system; In each filtering cycle, the high-frequency energy component of the angular velocity signal measured by the current gyroscope is used to determine the process noise covariance matrix, and the prior state vector and prior covariance matrix of the current moment are predicted based on the state vector at the previous moment, the angular velocity measured by the current gyroscope, and the process noise covariance matrix; Generate an asymmetric Sigma point set based on the prior state vector and the prior covariance matrix, where the asymmetry of the asymmetric Sigma point set is determined by the third-order moment of the historical prediction error; a measurement noise covariance matrix generation unit, configured to determine the measurement noise covariance matrix using the statistical variance of the latest innovation sequence within a preset sliding time window, and to increase the component of the measurement noise covariance matrix corresponding to the magnetometer measurement value according to the deviation between the current magnetometer measurement norm and the local reference geomagnetic model norm; A monitoring unit is configured to substitute the asymmetric Sigma point set into a measurement model, obtain predicted measurement values ​​and predicted measurement covariances through an unscented transformation, calculate a Kalman gain based on the predicted measurement covariances and the measurement noise covariance matrix, and perform a weighted gated check on innovations consisting of true measurement values ​​and predicted measurement values; Based on the adjusted Kalman gain and new information, the state vector and covariance matrix are updated to obtain the attitude information of the aircraft at the current moment.

9. The system according to claim 8, characterized in that The method of determining a noise covariance matrix of a process using a high-frequency energy component of an angular velocity signal measured by a current gyroscope includes: Get the gyroscope's three-axis angular velocity measurement value at the current sampling moment; The high-frequency component of the angular velocity signal is obtained by performing a vector difference between the current angular velocity measurement value and the angular velocity measurement value at the previous moment; Calculating the energy of the high-frequency component to obtain a high-frequency energy value; The high frequency energy value is multiplied by a preset scaling factor matrix and added to the basic process noise covariance matrix to obtain the process noise covariance matrix at the current moment.

10. The system according to claim 8, wherein: The method of predicting the prior state vector and the prior covariance matrix at the current moment based on the state vector at the previous moment, the angular velocity measured by the current gyroscope, and the process noise covariance matrix includes: Extract the attitude quaternion and gyroscope bias from the posterior state vector at the previous moment; Compensating the gyroscope angular velocity measurement value at the current moment using the gyroscope bias to obtain a compensated angular velocity; Based on the compensated angular velocity, the predicted attitude quaternion at the current moment is obtained by integrating the quaternion kinematic differential equation; The gyroscope bias, accelerometer bias, and magnetic field disturbance vector at the previous moment are used as the predicted value at the current moment, and are combined with the predicted attitude quaternion to obtain the prior state vector at the current moment; The Sigma point set at the previous moment is calculated through the quaternion kinematic differential equation to transfer the state covariance, and is added to the process noise covariance matrix to obtain the prior covariance matrix at the current moment.

Images