Pose solution method based on fusion of complementary filtering and unscented kalman

Abstract

The application discloses a pose solution method based on complementary filtering and unscented Kalman fusion, and particularly relates to the technical field of waterways, and the specific solution steps are as follows: S1: for a nonlinear system, the system state vector is iteratively updated by using unscented Kalman filtering; the application fuses Mohony complementary filtering and unscented Kalman filtering (UKF) algorithm, replaces the pose angle directly calculated from acceleration and magnetic field intensity information with the pose angle obtained by solving based on the complementary filtering algorithm from the perspective of making the measurement information more accurate, improves the overall pose solution accuracy; meanwhile, the framework of the fusion algorithm is based on the unscented Kalman filtering algorithm, the probability distribution of the nonlinear function is approximated by unscented transformation, the high-order truncation error caused by Taylor expansion of the extended Kalman filtering algorithm is avoided, and the pose solution accuracy for the nonlinear system is improved.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, and more specifically, to an attitude calculation method based on complementary filtering and unscented Kalman fusion. Background Technology

[0002] Currently, combined navigation systems based on inertial navigation and satellite navigation are widely used in aerospace, ship navigation, unmanned aerial vehicles, and other fields. The position and velocity updates of inertial navigation systems are affected by attitude angles; therefore, solving for high-precision attitude angles is one of the current research hotspots.

[0003] Attitude calculation utilizes information such as angular velocity, acceleration, and magnetic field strength output from sensors to calculate the attitude and heading information of the carrier relative to a reference frame. However, sensor outputs are susceptible to noise and external interference. For example, the attitude information calculated by directly integrating the angular velocity output from the gyroscope exhibits divergence over time, and the accelerometer is highly sensitive to the carrier's acceleration and is subject to high-frequency noise interference. Therefore, attitude calculation requires fusing information acquired from sensors such as gyroscopes, accelerometers, and magnetometers to improve the accuracy of attitude calculation.

[0004] Currently, mainstream attitude calculation algorithms include gradient descent, complementary filtering, and Kalman filtering. Gradient descent is susceptible to external interference, and the accuracy of gyroscope calculations depends on the output of accelerometers and magnetometers; moreover, it suffers from significant attitude calculation errors under high dynamic conditions. Complementary filtering cannot accurately estimate the cutoff frequency between the high-pass and low-pass filters, and cannot adjust weights in real time to achieve optimal attitude calculation. For nonlinear systems in engineering applications, the extended Kalman filter (EPF) introduces high-order truncation errors during attitude calculation, resulting in reduced accuracy. Therefore, a high-precision attitude calculation algorithm suitable for nonlinear systems is needed. Summary of the Invention

[0005] In order to overcome the above-mentioned defects of the prior art, the embodiments of the present invention provide an attitude calculation method based on the fusion of complementary filtering and unscented Kalman filter. The technical problem to be solved by the present invention is that the existing attitude calculation algorithms have the disadvantages of low attitude calculation accuracy and limited application scenarios.

[0006] To achieve the above objectives, the present invention provides the following technical solution: an attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering, which integrates the Mohony complementary filtering algorithm and the unscented Kalman filtering algorithm to establish nonlinear system equations, with the specific calculation steps as follows:

[0007] S1: For nonlinear systems, an unscented Kalman spectroscopy is used to iteratively update the system state vector; at the same time, a measurement equation is established to correct the state prediction value. The accuracy of the measurement information will directly affect the accuracy of the UKF algorithm in solving the attitude angle.

[0008] S2: The traditional unscented Kalman filter algorithm directly uses the attitude angle calculated from acceleration and magnetic field strength as the measurement information in the state update process; the attitude calculation algorithm based on the fusion of the Mohony complementary filter algorithm and the unscented Kalman filter (UKF) algorithm effectively improves the attitude calculation accuracy and achieves the optimal attitude estimation.

[0009] S3: The nonlinear system equations are established based on the attitude calculation algorithm of complementary filtering and unscented Kalman fusion as shown below:

[0010] S3.1, State Equations:

[0011] State prediction is based on the optimal estimate of the current state variables from the previous time step's state variables. The offline, nonlinear state equation for the unscented Kalman filter algorithm is established as follows:

[0012] x k =A k-1 x k-1 +w k-1

[0013] Where, x k Let x be the prior estimate at time k. k-1 Let w be the posterior estimate at time k-1. k For the process noise of covariance Q, A k The state transition matrix is ​​as follows:

[0014]

[0015] Among them, (w x ,w y ,w z This refers to the angular velocity information obtained from the gyroscope output.

[0016] S3.2 Measurement Equation:

[0017] Measurement estimation is a crucial step in the Kalman filter algorithm for correcting the state. The measurement equation is established as follows:

[0018]

[0019] Wherein, H(x) k ) is the measurement output function, v k Let (q0 q1 q2 q3) be the measurement noise with covariance R. TThe attitude information predicted by the state equation is represented in the form of quaternions;

[0020] S4. State Update: For the state equations and measurement equations of the above attitude calculation nonlinear system, the core formula of the unscented Kalman filter algorithm is used to predict and update the state variables. The attitude angles calculated by the Mohony complementary filter algorithm are used as measurement information to replace the attitude angles calculated directly using accelerometer and magnetometer information. This key step makes the measurement information used for state correction more accurate and improves the overall attitude calculation accuracy.

[0021] In a preferred embodiment, step S1 in the unscented Kalman algorithm establishes a state equation by correcting the angular velocity with a gyroscope, and then substitutes it into a quaternion differential equation to solve for the attitude quaternion at the next moment.

[0022] In a preferred embodiment, in step S2, the measurement information is first processed using the Mohony complementary filtering algorithm, which combines the advantages of the high-frequency characteristics of the gyroscope with the low-frequency characteristics of the accelerometer and magnetometer. The gyroscope angular velocity is corrected by a PI feedback controller, and the attitude differential equation is solved to obtain the attitude angle, providing more accurate measurement information for the UKF algorithm. Secondly, the attitude estimation angle obtained from the gyroscope angular velocity information is used as the state prediction information. Finally, the attitude angle is updated according to the Kalman equation.

[0023] In a preferred embodiment, the Mohony complementary filtering algorithm uses acceleration and magnetometer information to calculate estimated attitude angles: roll angle Φ1, pitch angle θ1, and yaw angle ψ1. Then, the attitude angles: roll angle Φ2, pitch angle θ2, and yaw angle ψ2 are calculated by fusing information from the accelerometer, magnetometer, and gyroscope. The deviation between the two sets of estimated attitude angles is used as the attitude angle error e and input to the PI feedback controller. Its output is used to correct the gyroscope output angular velocity, making the gyroscope output more accurate, thereby achieving stable attitude angle information output.

[0024] In a preferred embodiment, in the complementary filtering algorithm, the setting of the PI control parameters will affect the accuracy of attitude calculation and the convergence speed of the filtering algorithm. Specifically, the larger the proportional parameter KP and integral parameter KI are set, the faster the output correction angular velocity will be.

[0025] In a preferred embodiment, the core of the unscented Kalman filter algorithm lies in using an unscented transformation to approximate the probability distribution of the state variables of a nonlinear system. The unscented transformation involves selecting some sampling points from the original state distribution such that the mean X and covariance P of these sampling points are equal to the mean X and covariance P of the original state vector distribution. These point sets are then mapped to a nonlinear function to obtain the corresponding function value set. The mean X and covariance P after the nonlinear function transformation are calculated using the transformed point set. This transformation ensures that the unscented Kalman filter algorithm has at least second-order accuracy, and can achieve third-order accuracy for state variables that satisfy a Gaussian distribution. The specific implementation process of the unscented transformation includes the following two steps:

[0026] (a) Construct 2n+1 sigma points:

[0027]

[0028] in: is the element in the i-th column of the square root matrix, P is a positive definite matrix; n represents the dimension of the state vector, and i represents the i-th sigma point;

[0029] (b) Calculate the sigma point weights:

[0030]

[0031] Where: λ=α 2 (n+k)-n is the scaling factor to reduce the total prediction error; α is set to a small positive value to control the distribution of sigma points; k is a parameter chosen to ensure positive semidefiniteness, usually set to 0; for Gaussian distribution, β is a non-negative weighting coefficient, usually set to 2; the subscripts m and c represent the mean and covariance, respectively.

[0032] In a preferred embodiment, the unscented Kalman filter algorithm uses the unscented transform to propagate the mean and covariance. Its filtering process is the same as that of the linear Kalman filter, using process models and measurement models to predict and correct the mean and covariance of the state variables, thereby achieving the optimal estimation of the state variables. The specific algorithm flow is as follows:

[0033] (a) Set the initial mean X0 and covariance P0;

[0034] (b) Calculate 2n+1 sigma points using unscented transformation;

[0035] (c) Calculate the state prediction value based on the sigma points obtained in step b);

[0036] (d) Perform UT transformation on the obtained state prediction values ​​and calculate the predicted values ​​of the observed variables;

[0037] (e) Calculate the covariance matrix of the system state variables and observed variables;

[0038] (f) Calculate the unscented Kalman filter gain matrix;

[0039] (g) Update the state and covariance according to the formulas in the algorithm flowchart.

[0040] The technical effects and advantages of this invention are as follows:

[0041] This invention integrates Mohony complementary filtering with the unscented Kalman filtering (UKF) algorithm. From the perspective of making measurement information more accurate, it replaces the attitude angles directly calculated from acceleration and magnetic field strength information with attitude angles obtained based on the complementary filtering algorithm, thus improving the overall attitude calculation accuracy. Simultaneously, the framework of this fusion algorithm is based on the unscented Kalman filtering algorithm. It approximates the probability distribution of nonlinear functions through unscented transformation, avoiding the high-order truncation error caused by Taylor expansion in the extended Kalman filtering algorithm. This improves the attitude calculation accuracy for nonlinear systems and provides an algorithmic reference for researching high-precision attitude measurement based on multi-degree-of-freedom inertial sensors. Attached Figure Description

[0042] Figure 1 This is a schematic diagram of the complementary filtering algorithm of the present invention.

[0043] Figure 2 This is a schematic diagram of the unscented transformation of the present invention.

[0044] Figure 3 This is a flowchart of the unscented Kalman filter algorithm of the present invention.

[0045] Figure 4 This is a flowchart of the attitude calculation process of the fusion algorithm of this invention. Detailed Implementation

[0046] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0047] This invention provides an attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering. The Mohony complementary filtering algorithm and the unscented Kalman filtering algorithm are fused to establish nonlinear system equations. The attitude calculation process of the fused algorithm is as follows: Figure 4 As shown, the specific solution steps are as follows:

[0048] S1: For nonlinear systems, the unscented Kalman algorithm is used to iteratively update the system state vector; at the same time, a measurement equation is established to correct the state prediction value. The accuracy of the measurement information will directly affect the accuracy of the UKF algorithm in solving the attitude angle. The state equation is established by correcting the angular velocity with a gyroscope, and the attitude quaternion can be solved by substituting it into the quaternion differential equation.

[0049] S2: Traditional unscented Kalman filtering algorithms directly use the attitude angles calculated from acceleration and magnetic field strength as measurement information in the state update process. The attitude calculation algorithm, which integrates the Mohony complementary filtering algorithm and the unscented Kalman filtering (UKF) algorithm, effectively improves the attitude calculation accuracy and achieves optimal attitude estimation. The measurement information is first obtained by combining the high-frequency characteristics of the gyroscope with the low-frequency characteristics of the accelerometer and magnetometer using the Mohony complementary filtering algorithm. A PI feedback controller corrects the gyroscope angular velocity, and the attitude differential equation is solved to obtain the attitude angles, providing more accurate measurement information for the UKF algorithm. Secondly, the attitude estimation angle obtained from the gyroscope angular velocity information is used as the state prediction information. Finally, the attitude angles are updated according to the Kalman equation.

[0050] Mohony complementary filtering algorithm:

[0051] Since each sensor has its own shortcomings in acquiring attitude information, gyroscopes have high-frequency characteristics, while accelerometers and magnetometers have low-frequency characteristics, a complementary filtering algorithm is used to fuse data from multiple sensors. The good low-frequency and high-frequency responses of each sensor are estimated simultaneously, and their outputs are passed through the corresponding low-pass or high-pass filters. The result of directly superimposing the two outputs can fully and accurately reflect the characteristics of the measured object.

[0052] The complementary filtering algorithm essentially filters the noise output from each sensor from a frequency domain perspective. In an attitude measurement system, attitude information comes from two sources: a) angular velocity output from the gyroscope; b) acceleration output from the accelerometer and magnetic field strength output from the magnetometer. Attitude angle estimates are obtained by integrating the angular velocity output from the gyroscope. Furthermore, attitude angle estimates can also be obtained by combining the acceleration and magnetic field strength output from the accelerometer and magnetometer. However, factors such as gyroscope drift and constant error cause the attitude angle to gradually drift over time, with these errors mainly existing in the low-frequency range. While the accelerometer and magnetometer are accurate in static conditions, they are easily affected by external noise and vibration in dynamic conditions, with these errors mainly existing in the high-frequency range. Therefore, a complementary filtering algorithm is proposed. Its main idea is to use the information output from the accelerometer and magnetometer as the feedforward input of the filter, and use a PI feedback controller to compensate and correct the angular velocity measurement value from the gyroscope, obtaining higher-precision angular velocity information, avoiding attitude calculation errors caused by factors such as gyroscope drift, and obtaining stable attitude calculation results. The principle of the complementary filtering algorithm is as follows: Figure 1 As shown;

[0053] according to Figure 1 The schematic diagram shows that the estimated attitude angles (roll angle Φ1, pitch angle θ1, and yaw angle ψ1) are first calculated using acceleration and magnetometer information. Then, the attitude angles (roll angle Φ2, pitch angle θ2, and yaw angle ψ2) are calculated by fusing information from the accelerometer, magnetometer, and gyroscope. The deviation between the two sets of estimated attitude angles is used as the attitude angle error e, which is input to the PI feedback controller. The controller's output corrects the gyroscope's output angular velocity, making the gyroscope output more accurate and thus achieving stable attitude angle information output. However, in the complementary filtering algorithm, the setting of the PI control parameters will affect the accuracy of the attitude calculation and the convergence speed of the filtering algorithm. Specifically, the larger the proportional parameter KP and integral parameter KI are, the faster the rate of output angular velocity correction.

[0054] Unscented Kalman Filter (UKF) algorithm:

[0055] In practical engineering applications, the mathematical model of the system is often a nonlinear equation, so the traditional Kalman filter algorithm is no longer applicable. To address this, the Unscented Kalman Filter (UKF) algorithm is proposed. The Unscented Kalman does not require Taylor expansion and first-order linearization of the nonlinear equation, thus avoiding truncation error and effectively improving the attitude calculation accuracy.

[0056] The core of the unscented Kalman filter algorithm lies in using the unscented transformation (UT) to approximate the probability distribution of the state variables of a nonlinear system. The idea behind the unscented transformation is to select some sampling points in the original state distribution such that the mean X and covariance P of these sampling points are equal to the mean X and covariance P of the original state vector distribution. These point sets are then mapped to a nonlinear function to obtain the corresponding function value set. The mean X and covariance P after the nonlinear function transformation are then calculated using the transformed point set. This transformation ensures that the unscented Kalman filter algorithm has at least second-order accuracy, and can achieve third-order accuracy for state variables that satisfy a Gaussian distribution. The unscented transformation is as follows... Figure 2 As shown;

[0057] The specific implementation process of the unscented transform includes the following two steps:

[0058] (a) Construct 2n+1 sigma points:

[0059]

[0060] in: is the element in the i-th column of the square root matrix, P is a positive definite matrix; n represents the dimension of the state vector, and i represents the i-th sigma point;

[0061] (b) Calculate the sigma point weights:

[0062]

[0063] Where: λ=α 2 (n+k)-n is the scaling factor to reduce the total prediction error; α is set to a small positive value to control the distribution of sigma points; k is a parameter chosen to ensure positive semidefiniteness, usually set to 0; for a Gaussian distribution, β is a non-negative weighting coefficient, usually set to 2; the subscripts m and c represent the mean and covariance, respectively.

[0064] The unscented Kalman filter algorithm uses the unscented transform to propagate the mean and covariance. Its filtering process is similar to that of the linear Kalman filter, employing process and measurement models to predict and correct the mean and covariance of the state variables, achieving optimal estimation of the state variables. The specific algorithm flow is as follows: Figure 3 As shown:

[0065] (a) Set the initial mean X0 and covariance P0;

[0066] (b) Calculate 2n+1 sigma points using unscented transformation;

[0067] (c) Calculate the state prediction value based on the sigma points obtained in step b);

[0068] (d) Perform UT transformation on the obtained state prediction values ​​and calculate the predicted values ​​of the observed variables;

[0069] (e) Calculate the covariance matrices of the system state variables and observed variables;

[0070] (f) Calculate the unscented Kalman filter gain matrix;

[0071] (g) Update the state and covariance according to the formulas in the algorithm flowchart;

[0072] S3: The nonlinear system equations are established based on the attitude calculation algorithm of complementary filtering and unscented Kalman fusion as shown below:

[0073] S3.1, State Equations:

[0074] State prediction is based on the optimal estimate of the current state variables from the previous time step's state variables. The offline, nonlinear state equation for the unscented Kalman filter algorithm is established as follows:

[0075] x k =A k-1 x k-1 +w k-1

[0076] Where, x k Let x be the prior estimate at time k. k-1 Let w be the posterior estimate at time k-1. k For the process noise of covariance Q, A k The state transition matrix is ​​as follows:

[0077]

[0078] Among them, (w x ,w y ,w z This refers to the angular velocity information obtained from the gyroscope output.

[0079] S3.2 Measurement Equation:

[0080] Measurement estimation is a crucial step in the Kalman filter algorithm for correcting the state. The measurement equation is established as follows:

[0081]

[0082] Wherein, H(x) k ) is the measurement output function, v k Let (q0 q1 q2 q3) be the measurement noise with covariance R. T The attitude information predicted by the state equation is represented in the form of quaternions;

[0083] S4. State Update: For the state equations and measurement equations of the above attitude calculation nonlinear system, the core formula of the unscented Kalman filter algorithm is used to predict and update the state variables. The attitude angles calculated by the Mohony complementary filter algorithm are used as measurement information to replace the attitude angles calculated directly using accelerometer and magnetometer information. This key step makes the measurement information used for state correction more accurate and improves the overall attitude calculation accuracy.

[0084] Working principle of this invention:

[0085] Refer to the instruction manual appendix Figure 1-4 This scheme first combines the advantages of the Mohony complementary filtering algorithm with the high-frequency characteristics of the gyroscope and the low-frequency characteristics of the accelerometer and magnetometer. It uses acceleration and magnetic field strength information to correct the angular velocity output by the gyroscope, and uses the corrected angular velocity for attitude calculation. When using the Kalman filter algorithm for attitude calculation, in order to make the algorithm applicable to nonlinear systems and avoid the truncation error of the extended Kalman filter algorithm, the unscented Kalman filter algorithm is used as the overall framework of the attitude calculation algorithm. Then, the solution result of the Mohony complementary filtering algorithm is used as the measurement information of the UKF and the measurement equation is constructed. The state equation is constructed based on the attitude calculation result obtained from the gyroscope angular velocity. The UKF performs attitude update and outputs higher precision attitude angles, thereby meeting the high precision requirements of attitude in some scenarios.

[0086] This solution integrates the Mohony complementary filtering algorithm and the Unscented Kalman Filter (UKF) algorithm. The Mohony complementary filtering algorithm fuses information from the gyroscope, accelerometer, and magnetometer for attitude calculation, outputting more accurate attitude angles. Simultaneously, addressing the shortcomings of nonlinear systems in practical engineering and the high-order truncation error that leads to decreased accuracy in the Extended Kalman Filter (EKF) algorithm, the Unscented Kalman Filter algorithm is used for attitude updates. The Mohony complementary filtering algorithm provides higher-precision measurement information for the Unscented Kalman Filter. The fusion of these two algorithms improves the attitude calculation accuracy in nonlinear systems.

[0087] Finally, the following points should be noted: First, in the description of this application, it should be noted that, unless otherwise specified and limited, the terms "installation", "connection", and "linkage" should be interpreted broadly, and can be mechanical or electrical connections, or internal connections between two components, or direct connections. "Up", "down", "left", "right", etc. are only used to indicate relative positional relationships. When the absolute position of the described object changes, the relative positional relationship may change.

[0088] Secondly: The accompanying drawings of the embodiments disclosed in this invention only involve the structures involved in the embodiments disclosed in this invention. Other structures can refer to the general design. In the absence of conflict, the same embodiment and different embodiments of this invention can be combined with each other.

[0089] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. An attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering, characterized in that: The Mohony complementary filtering algorithm and the unscented Kalman filtering algorithm are combined to establish the nonlinear system equations. The specific solution steps are as follows: S1: For nonlinear systems, an unscented Kalman spectroscopy method is used to iteratively update the system state vector; at the same time, a measurement equation is established to correct the state prediction value. S2: The traditional unscented Kalman filter algorithm directly uses the attitude angle calculated from acceleration and magnetic field strength as the measurement information in the state update process; S3: The nonlinear system equations are established based on the attitude calculation algorithm of complementary filtering and unscented Kalman fusion as shown below: S3.1, State equations: State prediction is based on the optimal estimate of the current state variables from the previous time step's state variables. The offline, nonlinear state equation for the unscented Kalman filter algorithm is established as follows: x k =A k-1 x k-1 +w k-1 Where, x k Let x be the prior estimate at time k. k-1 Let w be the posterior estimate at time k-1. k For the process noise of covariance Q, A k The state transition matrix is ​​as follows: Among them, (w x ,w y ,w z This refers to the angular velocity information obtained from the gyroscope output. S3.2 Measurement Equation: Measurement estimation is a crucial step in the Kalman filter algorithm for correcting the state. The measurement equation is established as follows: Wherein, H(x) k ) is the measurement output function, v k Let (q0 q1 q2 q3) be the measurement noise with covariance R. T The attitude information predicted by the state equation is represented in the form of quaternions; S4. State Update: For the state equations and measurement equations of the above attitude solution nonlinear system, the core formula of the unscented Kalman filter algorithm is used to predict and update the state variables. The attitude angles calculated by the Mohony complementary filter algorithm are used as measurement information to replace the attitude angles calculated directly using accelerometer and magnetometer information.

2. The attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering as described in claim 1, characterized in that: In step S1 of the unscented Kalman algorithm, the state equation is established by correcting the angular velocity with a gyroscope. Substituting this into the quaternion differential equation can solve for the attitude quaternion at the next moment.

3. The attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering as described in claim 1, characterized in that: In step S2, the measurement information is first processed using the Mohony complementary filtering algorithm, which combines the advantages of the high-frequency characteristics of the gyroscope with the low-frequency characteristics of the accelerometer and magnetometer. The gyroscope angular velocity is corrected by a PI feedback controller, and the attitude differential equation is solved to obtain the attitude angle, providing more accurate measurement information for the UKF algorithm. Secondly, the attitude estimation angle obtained from the gyroscope angular velocity information is used as the state prediction information. Finally, the attitude angle is updated according to the Kalman equation.

4. The attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering as described in claim 1, characterized in that: The Mohony complementary filtering algorithm uses acceleration and magnetometer information to calculate estimated attitude angles: roll angle Φ1, pitch angle θ1, and yaw angle ψ1. Then, it uses the fusion information from the accelerometer, magnetometer, and gyroscope to calculate attitude angles: roll angle Φ2, pitch angle θ2, and yaw angle ψ2. The deviation between the two sets of estimated attitude angles is used as the attitude angle error e and input to the PI feedback controller. Its output is used to correct the gyroscope output angular velocity, making the gyroscope output more accurate.

5. The attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering according to claim 4, characterized in that: In the complementary filtering algorithm, the setting of the PI control parameters will affect the accuracy of attitude calculation and the convergence speed of the filtering algorithm. Specifically, the larger the proportional parameter KP and integral parameter KI are, the faster the output angular velocity correction rate will be.

6. The attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering as described in claim 5, characterized in that: The core of the unscented Kalman filter algorithm lies in using the unscented transformation to approximate the probability distribution of the state variables of the nonlinear system. The specific implementation process of the unscented transformation includes the following two steps: (a) Construct 2n+1 sigma points: in: is the element in the i-th column of the square root matrix, P is a positive definite matrix; n represents the dimension of the state vector, and i represents the i-th sigma point; (b) Calculate the weights of the sigma points: Where: λ=α 2 (n+k)-n is the scaling factor to reduce the total prediction error; α is set to a small positive value to control the distribution of sigma points; k is a parameter chosen to ensure positive semidefiniteness, usually set to 0; for Gaussian distribution, β is a non-negative weighting coefficient, usually set to 2; the subscripts m and c represent the mean and covariance, respectively.

7. The attitude calculation method based on the fusion of complementary filtering and unscented Kalman filtering as described in claim 6, characterized in that: The specific process of the unscented Kalman filter algorithm is as follows: (a) Set the initial mean X0 and covariance P0; (b) Calculate 2n+1 sigma points using unscented transformation; (c) Calculate the state prediction value based on the sigma points obtained in step b); (d) Perform UT transformation on the obtained state prediction values ​​and calculate the predicted values ​​of the observed variables; (e) Calculate the covariance matrix of the system state variables and observed variables; (f) Calculate the unscented Kalman filter gain matrix; (g) Update the state and covariance according to the formulas in the algorithm flowchart.

Images