Fusion filtering attitude estimation method based on extended Kalman filtering (EKF) and Mahony

Abstract

The invention discloses a fusion filtering attitude estimation method based on EKF and Mahony, and the method comprises the steps: firstly obtaining the original data information of an IMU sensor at the current moment, carrying out the normalization of the attitude information of an accelerometer and a magnetometer, carrying out the error calculation, and compensating the error to the angular velocity of a gyroscope through a PI controller, the method comprises the following steps: performing Mahony filtering correction on an angular velocity and a corrected attitude quaternion to obtain an angular velocity subjected to Mahony filtering correction and a corrected attitude quaternion, constructing a state equation of an EKF by using the corrected attitude quaternion and a gyroscope zero offset value, and constructing an observation equation by using data of an accelerometer and a magnetometer so as to obtain an optimally estimated attitude quaternion and a gyroscope zero offset value; and finally, compensating the optimally estimated zero offset value of the gyroscope to the angular velocity correction process 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 that combines the Extended Kalman Filter (EKF) and Mahony filter algorithms. This method solves the problem of insufficient accuracy of a single algorithm in dynamic environments by fusing multi-sensor data. It is suitable for applications such as drones, robotics, virtual reality, and smart wearable devices. It specifically addresses gyroscope drift suppression and accelerometer noise compensation, achieving highly robust, low-latency attitude solution, thereby improving the system's dynamic response and measurement reliability in non-steady-state environments. Background Art

[0002] In the field of inertial sensor attitude estimation, fusion algorithms based on MEMS gyroscopes, accelerometers, and magnetometers are the core technology for dynamic attitude solution. Traditional methods are mainly divided into two categories: one is a lightweight algorithm based on complementary filtering. This algorithm uses accelerometer and magnetometer observations to perform low-frequency corrections to the gyroscope angular velocity. It has the advantages of high real-time performance and low computational resource consumption. However, in the presence of intense motion or external acceleration interference, attitude errors accumulate due to the loss of accelerometer reliability, and there is a lack of a systematic dynamic compensation mechanism for gyroscope zero bias. The other is a model-driven method based on the extended Kalman filter (EKF). It achieves optimal estimation by establishing state equations and observation equations. However, it relies on precise model parameter and covariance matrix settings. In dynamic mutation scenarios, it is prone to linearization errors, resulting in insufficient convergence speed. Moreover, its high computational complexity limits its application in embedded platforms. Patent CN201910000422.1 uses adaptive Kalman filtering to dynamically adjust the accelerometer measurement noise matrix so that the filter is always in the optimal state; Patent CN201711081317.2 uses adaptive iterative particle filtering to solve the problems of small particle weights and difficulty in normalization by dynamically correcting its own iteration under conditions, thereby providing high-precision attitude estimation; Patent CN202411603513.1 proposes a quaternion particle filter attitude estimation method based on super-hemisphere uniform deterministic sampling, which generates deterministic quaternion samples through mirror sampling and exponential mapping, while ensuring accuracy. It solves the problems of large computational complexity, poor real-time performance and particle degradation of traditional particle filters. Summary of the Invention

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

[0004] To achieve the above object, the technical solution of the present invention is as follows: An attitude estimation method based on the fusion of EKF and Mahony filtering, the method comprising the following steps: Step 1: Obtain the original values of the IMU sensor 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 data and magnetometer data with the ideal gravity vector and the ideal geomagnetic field respectively to obtain the errors of both; Step 4: Use the PI controller to compensate the gyroscope angular velocity with the errors of both to obtain the corrected angular velocity; Step 5: Calculate the corrected attitude quaternion using the corrected angular velocity; Step 6: Construct the state equation of the EKF using the corrected quaternion and the gyroscope bias value, and use the data of the accelerometer and magnetometer for the measurement equation; Step 7: Update the filtering equation to obtain the attitude quaternion estimated by the EKF and the gyroscope bias value; Step 8: Convert the attitude quaternion to Euler angles and output; Step 9: Compensate the estimated gyroscope bias value to the gyroscope angular velocity corrected by Mahony filtering to further correct the gyroscope data.

[0005] Furthermore, the specific method for obtaining the original data information of the IMU sensor in Step 1 is: At the initial moment, the attitude angle is represented by a quaternion as: where R(q) represents the attitude transformation matrix; q0, q1, q2, q3 are quaternions; At this time, the outputs of the gyroscope, accelerometer and magnetometer are: where ω is the three-axis angular velocity, a is the three-axis acceleration, and m is the three-axis geomagnetic data; ω x is the angular velocity component of the x-axis, ω y is the angular velocity component of the y-axis, ω z is the angular velocity component of the z-axis, a x is the acceleration component of the x-axis, a y is the acceleration component of the y-axis, a z is the acceleration component of the z-axis, m x is the geomagnetic intensity component of the x-axis, m y is the geomagnetic intensity component of the y-axis, m z is the geomagnetic intensity component of the z-axis.

[0006] Furthermore, the normalization processing of the accelerometer and magnetometer data described in step 2 is as follows: In the formula, is the normalized acceleration quantity, is the normalized geomagnetic intensity quantity.

[0007] Furthermore, the cross product of the ideal gravity vector and the ideal geomagnetic field with the measurement data described in step 3 is used to obtain the error between the two: In the formula, e acc is the error quantity of the accelerometer, and e mag is the error quantity of the geomagnetometer; Therefore, the total error of the accelerometer and magnetometer can be obtained: e = e acc + e mag In the formula, e is the sum of the errors of the accelerometer and geomagnetometer.

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

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

[0010] Furthermore, the state equation of the EKF is constructed using the corrected quaternion and the gyroscope bias value described in step 6, and the expression is as follows: In the formula, b ω is the gyroscope bias value; Assume that the gyroscope bias is a random walk, expressed as: In the formula, is the random walk of the gyroscope; After discretization, it is obtained:

[0011] Furthermore, the measurement equation of the system can be obtained: where v(k) is the measurement noise.

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

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

[0014] Furthermore, the filtering equation in step 7 is updated to obtain the attitude quaternion and gyroscope bias value estimated by EKF, and the expression is as follows: where is the prior state estimate; where is the prior state estimation error covariance matrix; where K k+1 is the Kalman gain; where x k+1 is the posterior state estimate; where p k+1 is the posterior state estimation error covariance matrix.

[0015] Furthermore, the result of converting the optimized attitude quaternion to Euler angles and outputting in step 8 is: where θ, γ and represent the pitch angle, roll angle and yaw angle respectively.

[0016] Furthermore, the gyroscope zero bias value estimated in step 9 is compensated to the gyroscope angular velocity corrected by Mahony filtering to further correct the gyroscope data: ω corrected = ω - b w / k+1 + K p ·e + Ki ·∫edt In the formula, b w / k+1 is the gyroscope zero bias value obtained by posterior estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] Figure 1 is the specific process block diagram of the present invention; Figure 2 is the simulation curve 1 of the embodiment; Figure 3 is the simulation curve 2 of the embodiment; Figure 4 is the simulation curve 3 of the embodiment. DETAILED DESCRIPTION OF THE INVENTION

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

[0019] A fusion filtering attitude estimation method based on the Extended Kalman Filter (EKF) and Mahony, as Figure 1 shown, is characterized in that: Step 1, obtain the original values of the IMU sensors at the current moment: At the initial moment, the attitude angle is represented by a quaternion as: In the formula, R(q) represents the attitude transformation matrix; q0, q1, q2, q3 are quaternions; At this time, the outputs of the gyroscope, accelerometer, and magnetometer are: In the formula, ω is the three-axis angular velocity, a is the three-axis acceleration, and m is the three-axis geomagnetic data; ω x is the angular velocity component of the x-axis, ω y is the angular velocity component of the y-axis, ω z is the angular velocity component of the z-axis, a x is the acceleration component of the x-axis, a y is the acceleration component of the y-axis, a z is the acceleration component of the z-axis, m x is the geomagnetic intensity component of the x-axis, m y is the geomagnetic intensity component of the y-axis, m z is the geomagnetic intensity component of the z-axis; Step 2, normalize the accelerometer and magnetometer data as follows: In the formula, is the normalized acceleration quantity, is the normalized geomagnetic intensity quantity; Step 3: Cross-multiply the ideal gravity vector and the ideal geomagnetic field with the measurement data to obtain the error between the two: where e acc is the error of the accelerometer, and e mag is the error of the magnetometer; Therefore, the total error of the accelerometer and magnetometer can be obtained: e = e acc + e mag where e is the sum of the errors of the accelerometer and 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 where K p is the proportional term parameter of the PI controller, and K i is the integral term parameter of the PI controller; Step 5: The formula for calculating the corrected attitude quaternion using the corrected angular velocity is as follows: where q is the attitude quaternion from the body coordinate system to the navigation coordinate system, q new is the updated attitude quaternion at the current moment, and q prev is the attitude quaternion at the previous moment; Step 6: Construct the state equation of the EKF using the corrected quaternion and the gyroscope bias value, and the expression is as follows: where b ω is the gyroscope bias value; Assume that the gyroscope bias is a random walk, expressed as: where is the random walk of the gyroscope; After discretization, we get: Furthermore, the measurement equation of the system can be obtained: where v(k) is the measurement noise; Furthermore, the observation matrix of the system can be obtained: Furthermore, the state transition matrix of the system can be obtained: Step 7, update the filtering equation to obtain the attitude quaternion and gyroscope bias value estimated by EKF. The expressions are as follows: In the formula, is the prior state estimate; In the formula, is the prior state estimation error covariance matrix; In the formula, K k+1 is the Kalman gain; In the formula, x k+1 is the posterior state estimate; In the formula, p k+1 is the posterior state estimation error covariance matrix; Step 8, convert the optimized attitude quaternion to Euler angles, and the output result is: Among them, θ, γ and represent the pitch angle, roll angle and yaw angle respectively; Step 9, compensate the estimated gyroscope bias value 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 is the gyroscope bias value obtained from the posterior estimation.

[0020] To verify the tracking effects of the Extended Kalman Filter (EKF) and Mahony-EKF filter in attitude estimation, a random Euler angle motion trajectory with random Gaussian white noise added was generated in Matlab. This trajectory simulates the three-dimensional rotational motion of an object by setting a series of known angle change inputs, and attitude estimation is carried out 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 lags a bit 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 only illustrates the technical idea of the present invention and cannot limit the protection scope of the present invention by this. For those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements all fall within the protection scope of the claims of the present invention.

Claims

1. A fusion filtering attitude estimation method based on EKF and Mahony, characterized in that, It includes the following steps: Step 1: Obtain the original data of the IMU sensor 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 on the normalized accelerometer data with the ideal gravity vector and the magnetometer data with the ideal geomagnetic field respectively, calculate the accelerometer error and the magnetometer error, and fuse them to obtain the total error; Step 4: Use a PI controller to compensate the total error to the gyroscope angular velocity to generate a corrected angular velocity; Step 5: Update the attitude quaternion based on the corrected angular velocity; Step 6: Construct the state equation of the EKF with the corrected attitude quaternion and the gyroscope bias value as state variables, and construct the measurement equation with the accelerometer and magnetometer data as observations; Step 7: Update the filtering equation through the EKF to estimate the optimal attitude quaternion and the gyroscope bias value; Step 8: Convert the estimated attitude quaternion into Euler angles for output; Step 9: Feed back the gyroscope bias value estimated by the EKF into the corrected angular velocity of the Mahony filter to achieve dynamic bias compensation.

2. The method according to claim 1, wherein The specific normalization process in Step 2 is as follows: Calculate the norm of the accelerometer data and normalize it: Where a is the acceleration and m is the geomagnetic data, and are the normalized acceleration value and geomagnetic quantity, a x is the acceleration component on the x-axis, a y is the acceleration component on the y-axis, a z is the acceleration component on the z-axis, m x is the geomagnetic intensity component on the x-axis, m y is the geomagnetic intensity component on the y-axis, m z is the geomagnetic intensity component on the z-axis.

3. The method according to claim 1, characterized in that The calculation formula for the total error in Step 3 is: e = e acc +e mag where \(R(q)\) represents the attitude transformation matrix, \(e\) acc is the accelerometer error, \(e\) mag is the magnetometer error, and \(e\) is the total error of the system.

4. The method according to claim 1, wherein The compensation formula of the PI controller in Step 4 is: ω corrected = ω + K p ·e + K i ·∫edt where ω corrected is the gyro angular velocity value after correction, and K p is the proportional term parameter of the PI controller, and K i is the integral term parameter of the PI controller.

5. The method according to claim 1, wherein The update formula for the attitude quaternion in Step 5 is: Where q is the attitude quaternion from the vehicle coordinate system to the navigation coordinate system, q new is the attitude quaternion at the updated current moment, q prev is the attitude quaternion at the previous moment.

6. The method according to claim 1, wherein The state equation of the EKF defined in Step 6 is: where b ω is the gyroscope zero bias value.

7. The method according to claim 1, characterized in that, The update process of the EKF in Step 7 includes: In the formula, is the prior state estimate; In the formula, is the prior state estimation error covariance matrix; where K k+1 is the Kalman gain: where x k+1 is the posterior state estimate: where p k+1 is the posterior state estimation error covariance matrix.

8. The method according to claim 1, characterized in that The Euler angle conversion formula in Step 8 is: where θ, γ, and represent the pitch angle, roll angle, and yaw angle, respectively.

9. The method according to claim 1, characterized in that The gyroscope bias compensation formula in Step 9 is: ω corrected = ω - b w / k+1 + K p · e + K i · ∫ e dt where b w / k+1 is the gyro bias value obtained by posterior estimation.

Images