Dual-Stage Quaternion Estimator: An advanced method for orientation and angular kinematics estimation based on IMU sensor fusion

Abstract

In modern orientation and control applications such as robots, unmanned vehicles, or pan-tilt stabilization systems, accurately estimating orientation angles, angular velocities, and angular accelerations from inertial measurement unit (IMU) data is a significant challenge due to noise, gyroscope bias, and system nonlinearity. Solutions involving Extended Kalman Filter (EKF) have shown many advantages in significantly improving "filtering" quality; however, most only focus on orientation angles and angular velocities, neglecting angular acceleration—a crucial quantity in complex motion conditions. This paper proposes an improved filter, the "Dual-Stage Quaternion Estimator" (DSQE), which uses a two-stage structure with stage 1 applying an Unscented Kalman Filter (UKF) based on quaternions and stage 2 using a linear Kalman filter to estimate angular dynamics, including angular jerk. This method improves the accuracy of orientation estimation thanks to the second-order Taylor expansion and the UKF's ability to handle nonlinearity, while also providing accurate orientation angles in complex motion conditions. Simulation results show that DSQE with UKF outperforms traditional methods, especially in scenarios with strong vibrations or high accelerations.