A fusion filter attitude estimation method based on extended Kalman filter (EKF) and Mahony

Abstract

The application discloses a fusion filtering attitude estimation method based on EKF and Mahony, which comprises the following steps: firstly, acquiring original data information of an IMU sensor at a current time, and performing error calculation on normalized attitude information of an accelerometer and a magnetometer; secondly, compensating the error to the angular velocity of a gyroscope through a PI controller, so as to obtain the corrected angular velocity and the corrected attitude quaternion after Mahony filtering; thirdly, constructing the state equation of EKF by using the corrected attitude quaternion and the zero bias value of the gyroscope, and constructing the observation equation by using the data of the accelerometer and the magnetometer, so that the optimal estimated attitude quaternion and the zero bias value of the gyroscope can be obtained; and finally, compensating the optimal estimated zero bias value of the gyroscope to the corrected angular velocity in the Mahony filtering process.

Description

Technical Field

[0001] This invention relates to the field of sensor fusion and attitude estimation, and provides a fusion filtering method combining the Extended Kalman Filter (EKF) and Mahony Filter algorithms. This method addresses the problem of insufficient accuracy of a single algorithm in dynamic environments through multi-sensor data fusion, and is applicable to fields such as drones, robots, virtual reality, and smart wearable devices. It is particularly effective in suppressing gyroscope drift and compensating for accelerometer noise, achieving highly robust and low-latency attitude calculation, thereby improving the dynamic response and measurement reliability of the system in unsteady environments. Background Technology

[0002] In the field of inertial sensor attitude estimation, fusion algorithms based on MEMS gyroscopes, accelerometers, and magnetometers are the core technology for achieving dynamic attitude calculation. Traditional methods are mainly divided into two categories: one is a lightweight algorithm based on complementary filtering, which uses accelerometer and magnetometer observations to perform low-frequency correction on the gyroscope angular velocity. It has the advantages of high real-time performance and low computational resource consumption. However, under violent motion or external acceleration interference, the attitude error accumulates due to the decrease in accelerometer reliability, and it lacks a systematic dynamic compensation mechanism for gyroscope zero bias. The other is a model-driven method based on extended Kalman filtering (EKF), which achieves optimal estimation by establishing state equations and observation equations. However, it relies on precise model parameters and covariance matrix settings. In dynamic and abrupt scenarios, it is prone to insufficient convergence speed due to linearization errors, and its high computational complexity limits its application on embedded platforms. Patent CN201910000422.1 utilizes an adaptive Kalman filter to dynamically adjust the accelerometer noise matrix, ensuring the filter remains in an optimal state. Patent CN201711081317.2 employs an adaptive iterative particle filter, which addresses the issues of small particle weights and difficulty in normalization by dynamically correcting its own iteration under certain conditions, thereby providing high-precision attitude estimation. Patent CN202411603513.1 proposes a quaternion particle filter attitude estimation method based on hemispherical uniform deterministic sampling. It generates deterministic quaternion samples through mirror sampling and exponential mapping, solving the problems of high computational load, poor real-time performance, and particle degradation in traditional particle filters while maintaining accuracy. Summary of the Invention

[0003] The problem addressed by this invention is: to address the limitations of Mahony complementary filtering in optimizing sensor data, its susceptibility to external interference, and its unsuitability for nonlinear systems, a fusion filtering attitude estimation method based on EKF and Mahony is proposed to improve the accuracy of attitude measurement systems.

[0004] To achieve the above objectives, the technical solution of the present invention is as follows: A fusion filtering pose estimation method based on EKF and Mahony, comprising the following steps: Step 1: Obtain the raw values ​​of the IMU sensors at the current moment, including gyroscope angular velocity, accelerometer and magnetometer information; Step 2: Normalize the accelerometer and magnetometer data collected in Step 1; Step 3: Cross-multiply the normalized accelerometer and magnetometer data with the ideal gravity vector and the ideal geomagnetic field respectively to obtain the error between the two. Step 4: Use a PI controller to compensate the gyroscope angular velocity for the error between the two, and obtain the corrected angular velocity; Step 5: Calculate the corrected attitude quaternion using the corrected angular velocity; Step 6: Construct the state equation of EKF using the corrected quaternion and the zero bias value of the gyroscope. The measurement equation uses data from the accelerometer and magnetometer. Step 7: Update the filter equation to obtain the attitude quaternion estimated by EKF and the zero bias value of the gyroscope; Step 8: Convert the attitude quaternion to Euler angles and output them; Step 9: The estimated zero bias value of the gyroscope is compensated into the gyroscope angular velocity corrected by Mahony filtering to further correct the gyroscope data.

[0005] Furthermore, the specific method for obtaining the raw data information of the IMU sensor at the current moment in step 1 is as follows: The attitude angle is expressed as a quaternion at the initial moment: In the formula, R(q) represents the attitude transformation matrix; q0, q1, q2, and q3 are quaternions; At this time, the outputs of the gyroscope, accelerometer, and magnetometer are: In the formula, ω is the triaxial angular velocity, a is the triaxial acceleration, and m is the triaxial geomagnetic data; ω x Let ω be the angular velocity component along the x-axis. y Let ω be the angular velocity component along the y-axis. z Let a be the angular velocity component along the z-axis. x Let a be the acceleration component along the x-axis. y Let a be the acceleration component along the y-axis. z Let m be the acceleration component along the z-axis. x Let m be the geomagnetic intensity component along the x-axis. y Let m be the y-axis geomagnetic intensity component. z The z-axis represents the geomagnetic intensity component.

[0006] Furthermore, the accelerometer and magnetometer data normalization process described in step 2 is as follows: In the formula, To speed up measurement after normalization, This is the normalized geomagnetic intensity.

[0007] Furthermore, step 3 involves cross-productting the ideal gravity vector and the ideal geomagnetic field with the measured data to obtain the error between the two: In the formula, e acc e represents the error of the accelerometer. mag This represents the error of the magnetometer; Therefore, the total error of the accelerometer and magnetometer can be obtained: e = e acc +e mag In the formula, e represents the sum of the errors of the accelerometer and the magnetometer.

[0008] Furthermore, the formula for compensating the gyroscope angular velocity using a PI controller, as described in step 4, is as follows: ω corrected =ω+K p e+K i ·∫edt In the formula, ω corrected K is the gyroscope angular velocity value corrected by the PI controller. p K is the proportional term parameter of the PI controller. i This refers to the integral term parameter of the PI controller.

[0009] Furthermore, the formula for calculating the corrected attitude quaternion using the corrected angular velocity in step 5 is as follows: In the formula, q is the attitude quaternion from the vehicle coordinate system to the navigation coordinate system. new Let q be the updated current attitude quaternion. prev The attitude quaternion of the previous moment.

[0010] Furthermore, the state equation of the EKF, constructed using the corrected quaternion and the zero bias value of the gyroscope as described in step 6, is expressed as follows: In the formula, b ω This is the zero bias value of the gyroscope; Assuming the gyroscope's zero bias is a random walk, it can be expressed as: In the formula, This refers to the random walk of the gyroscope. After discretizing it, we get:

[0011] Furthermore, the measurement equations for the system can be obtained: In the formula, v(k) represents the measurement noise.

[0012] Furthermore, the system's observation matrix can be obtained:

[0013] Furthermore, the state transition matrix of the system can be obtained:

[0014] Further, the filtering equation is updated in step 7 to obtain the attitude quaternion estimated by EKF and the gyroscope zero bias value, as expressed below: In the formula, For prior state estimation; In the formula, The prior state estimation error covariance matrix; In the formula, K k+1 Kalman gain; In the formula, x k+1 For posterior state estimation; In the formula, p k+1 Let be the covariance matrix of the posterior state estimation error.

[0015] Furthermore, the result of converting the optimized pose quaternion into Euler angles and outputting it in step 8 is as follows: In the formula, θ, γ and These represent pitch angle, roll angle, and yaw angle, respectively.

[0016] Furthermore, step 9 involves compensating the estimated gyroscope zero bias value into the Mahony-filtered corrected gyroscope angular velocity to further correct the gyroscope data. ω corrected =ω-b w / k+1 +K p e+Ki ·∫edt In the formula, b w / k+1 This is the zero bias value of the gyroscope obtained from the posterior estimation. Attached Figure Description

[0017] Figure 1 This is a specific flowchart of the present invention; Figure 2 Simulation curve 1 for the example; Figure 3 Simulation curve 2 for the example; Figure 4 Simulation curve 3 is shown in the example. Detailed Implementation

[0018] The present invention will now be described in detail with reference to the accompanying drawings.

[0019] A fusion filtering attitude estimation method based on Extended Kalman Filter (EKF) and Mahony, such as... Figure 1 As shown, its characteristics are: Step 1, obtain the raw values ​​from the IMU sensor at the current moment: The attitude angle is expressed as a quaternion at the initial moment: In the formula, R(q) represents the attitude transformation matrix; q0, q1, q2, and q3 are quaternions; At this time, the outputs of the gyroscope, accelerometer, and magnetometer are: In the formula, ω is the triaxial angular velocity, a is the triaxial acceleration, and m is the triaxial geomagnetic data; ω x Let ω be the angular velocity component along the x-axis. y Let ω be the angular velocity component along the y-axis. z Let a be the angular velocity component along the z-axis. x Let a be the acceleration component along the x-axis. y Let a be the acceleration component along the y-axis. z Let m be the acceleration component along the z-axis. x Let m be the geomagnetic intensity component along the x-axis. y Let m be the y-axis geomagnetic intensity component. z The z-axis is the component of the geomagnetic intensity. Step 2, the accelerometer and magnetometer data normalization process is as follows: In the formula, To speed up measurement after normalization, This is the normalized geomagnetic intensity. Step 3: Perform a cross product of the ideal gravity vector and the ideal geomagnetic field with the measured data to obtain the error between the two: In the formula, e acc e represents the error of the accelerometer. mag This represents the error of the magnetometer; Therefore, the total error of the accelerometer and magnetometer can be obtained: e = e acc +e mag In the formula, e represents the sum of the errors of the accelerometer and the magnetometer; Step 4, the formula for compensating the gyroscope angular velocity using a PI controller is as follows: ω corrected =ω+K p e+K i ·∫edt In the formula, K p K is the proportional term parameter of the PI controller. i For the integral term parameters of the PI controller; Step 5, the formula for calculating the corrected attitude quaternion using the corrected angular velocity is as follows: In the formula, q is the attitude quaternion from the vehicle coordinate system to the navigation coordinate system. new Let q be the updated current attitude quaternion. prev The attitude quaternion from the previous moment; Step 6: Construct the state equation of EKF using the corrected quaternion and the zero bias value of the gyroscope, as shown in the following expression: In the formula, b ω This is the zero bias value of the gyroscope; Assuming the gyroscope's zero bias is a random walk, it can be expressed as: In the formula, This refers to the random walk of the gyroscope. After discretizing it, we get: Furthermore, the measurement equations for the system can be obtained: In the formula, v(k) represents the measurement noise; Furthermore, the system's observation matrix can be obtained: Furthermore, the state transition matrix of the system can be obtained: Step 7: Update the filtering equation to obtain the EKF-estimated attitude quaternion and gyroscope zero bias value, as shown in the following expressions: In the formula, For prior state estimation; In the formula, The prior state estimation error covariance matrix; In the formula, K k+1 Kalman gain; In the formula, x k+1 For posterior state estimation; In the formula, p k+1 The error covariance matrix for the posterior state estimation; Step 8: Convert the optimized pose quaternion into Euler angles and output the result as follows: Where θ, γ and These represent pitch angle, roll angle, and yaw angle, respectively. Step 9: The estimated zero bias value of the gyroscope is compensated for and added to the gyroscope angular velocity corrected by Mahony filtering to further correct the gyroscope data. ω corrected =ω-b w / k+1 +K p e+K i ·∫edt Among them, b w / k+1 This is the zero bias value of the gyroscope obtained from the posterior estimation.

[0020] To verify the tracking performance of Extended Kalman Filter (EKF) and Mahony-EKF filter in attitude estimation, a random Euler angle motion trajectory was generated in Matlab and random Gaussian white noise was added. This trajectory simulates the three-dimensional rotational motion of the object by setting a series of known angle changes as inputs, and attitude estimation is performed based on this.

[0021] The simulation results are as follows: from Figures 2 to 4It can be seen that although both algorithms can track the pitch angle curve relatively well most of the time, the EKF filter is slightly lagging behind the Mahony-EKF filter in the initial stage, and the Mahony-EKF filter is smoother than the EKF filter when the angle changes drastically.

[0022] It should be noted that the above content merely illustrates the technical concept of the present invention and cannot be used to limit the scope of protection of the present invention. For those skilled in the art, several improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A fusion filtering pose estimation method based on EKF and Mahony, characterized in that, Includes the following steps: Step 1: Obtain the raw IMU sensor data at the current moment, including gyroscope angular velocity, accelerometer and magnetometer information; Step 2: Normalize the accelerometer and magnetometer data; Step 3: Perform cross products between the normalized accelerometer data and the ideal gravity vector, and between the magnetometer data and the ideal geomagnetic field, respectively, to calculate the accelerometer error and the magnetometer error, and then fuse them to obtain the total error; Step 4: Use a PI controller to compensate the total error to the gyroscope angular velocity, and generate the corrected angular velocity; Step 5: Update the attitude quaternion based on the corrected angular velocity; Step 6: Construct the state equation of EKF using the corrected attitude quaternion and the gyroscope zero bias as state variables, and construct the measurement equation using accelerometer and magnetometer data as observations; Step 7: Update the filter equations using EKF to estimate the optimal attitude quaternion and the gyroscope zero bias value; Step 8: Convert the estimated attitude quaternion into Euler angles output; Step 9: Feed back the gyroscope zero bias value estimated by EKF to the corrected angular velocity of the Mahony filter to achieve dynamic zero bias compensation.

2. The method according to claim 1, characterized in that, The normalization process described in step 2 specifically includes: Calculate and normalize the magnitude of the accelerometer data: ； In the formula, For acceleration, For geomagnetic data, and These are the normalized acceleration values ​​and geomagnetic quantities. Let x be the acceleration component along the x-axis. The acceleration component along the y-axis is... Let z be the acceleration component along the z-axis. The x-axis represents the geomagnetic intensity component. The y-axis represents the geomagnetic intensity component. The z-axis represents the geomagnetic intensity component.

3. The method according to claim 2, characterized in that, The formula for calculating the total error in step 3 is as follows: ； ； In the formula, Represents the attitude transformation matrix. For accelerometer error, This is due to the error of the magnetometer. This represents the total error of the system.

4. The method according to claim 3, characterized in that, The PI controller compensation formula mentioned in step 4 is: ； In the formula, This is the corrected gyroscope angular velocity value. For the proportional term parameter of the PI controller, For the integral term parameters of the PI controller, The sum of the errors of the accelerometer and magnetometer, It represents the angular velocity of the three axes.

5. The method according to claim 4, characterized in that, The update formula for the attitude quaternion mentioned in step 5 is: ； ； In the formula, Let be the attitude quaternion from the vehicle coordinate system to the navigation coordinate system. The updated pose quaternion for the current moment. Let be the attitude quaternion from the previous moment. This is the gyroscope angular velocity value corrected by the PI controller.

6. The method according to claim 5, characterized in that, The state equation of EKF mentioned in step 6 is defined as follows: ； In the formula, This is the zero bias value of the gyroscope.

7. The method according to claim 6, characterized in that, The EKF update process described in step 7 includes: ； In the formula, For prior state estimation; ； In the formula, The prior state estimation error covariance matrix; ； In the formula, Kalman gain; ‘； In the formula, For posterior state estimation; ； In the formula, Let be the covariance matrix of the posterior state estimation error.

8. The method according to claim 7, characterized in that, The Euler angle transformation formula mentioned in step 8 is: ； In the formula, , and These represent pitch angle, roll angle, and yaw angle, respectively. , , , It is a quaternion.

9. The method according to claim 8, characterized in that, The gyroscope zero-bias compensation formula mentioned in step 9 is: ； In the formula, The zero bias value of the gyroscope is obtained from the posterior estimation. For the proportional term parameter of the PI controller, This refers to the integral term parameter of the PI controller.

Images