Quaternion-based Unscented Kalman Filter for 6-DoF Vision-based Inertial Navigation in GPS-denied Regions

TL;DR

This work tackles GPS-denied 6 DoF navigation by jointly estimating orientation, position, and velocity using a quaternion-based unscented filter that operates on $\mathbb{S}^{3}\times\mathbb{R}^{3}\times\mathbb{R}^{3}$ and fuses stereo vision with a 6-axis IMU. The proposed QNUKF accounts for quaternion geometry in prediction and update, employing quaternion-aware sigma-point generation, a quaternion-weighted mean, and custom quaternion addition/subtraction to maintain global consistency. Validation on the EuRoC drone dataset demonstrates superior accuracy and robustness compared to EKF and MSCKF, with significantly lower RMSE and SSRMSE across multiple sequences, even at low sampling rates and with variable feature counts. The approach offers a practical solution for GPS-denied VIN in UAVs, providing reliable attitude, position, and velocity estimates critical for autonomous flight in challenging environments.

Abstract

This paper investigates the orientation, position, and linear velocity estimation problem of a rigid-body moving in three-dimensional (3D) space with six degrees-of-freedom (6 DoF). The highly nonlinear navigation kinematics are formulated to ensure global representation of the navigation problem. A computationally efficient Quaternion-based Navigation Unscented Kalman Filter (QNUKF) is proposed on $\mathbb{S}^{3}\times\mathbb{R}^{3}\times\mathbb{R}^{3}$ imitating the true nonlinear navigation kinematics and utilize onboard Visual-Inertial Navigation (VIN) units to achieve successful GPS-denied navigation. The proposed QNUKF is designed in discrete form to operate based on the data fusion of photographs garnered by a vision unit (stereo or monocular camera) and information collected by a low-cost inertial measurement unit (IMU). The photographs are processed to extract feature points in 3D space, while the 6-axis IMU supplies angular velocity and accelerometer measurements expressed with respect to the body-frame. Robustness and effectiveness of the proposed QNUKF have been confirmed through experiments on a real-world dataset collected by a drone navigating in 3D and consisting of stereo images and 6-axis IMU measurements. Also, the proposed approach is validated against standard state-of-the-art filtering techniques. IEEE Keywords: Localization, Navigation, Unmanned Aerial Vehicle, Sensor-fusion, Inertial Measurement Unit, Vision Unit.

Figures (6)

• Figure 1: Navigation problem visualization in the 6 DoF.
• Figure 2: Schematic diagram of the proposed QNUKF. First, the sigma points based on the last estimated state vector and its covariance matrix are calculated. Using these sigma points and IMU measurements, the prediction step is performed. If camera data exists at that sample point, the correction (also known as update) step is performed. The state and covariance estimation at that time step is the result of the correction step if camera data exists, and the prediction step if it does not.
• Figure 3: Matched feature points from the left and right views of the EuRoC dataset Burri25012016.
• Figure 4: Validation results of the proposed QNUKF using the EuRoC V1_02_medium dataset Burri25012016. On the left, the navigation trajectory of the drone moving in 3D space is shown with a solid black line, and the drone orientation with respect to $x$, $y$, and $z$ axes is represented by red, green, and blue dashed lines, respectively. On the right, the magnitudes of the orientation error vector $||r_{e,k}||$, position error vector $||p_{e,k}||$, and linear velocity error vector $||v_{e,k}||$ are plotted over time in solid red lines.
• Figure 5: Estimation errors in rotation vector (left), position (middle), and linear velocity (right) components, from top to bottom of the proposed QNUKF.

Theorems & Definitions (1)

• Remark 1