Unscented Particle Filter for Visual-inertial Navigation using IMU and Landmark Measurements

TL;DR

The paper tackles GPS-denied 6-DoF navigation by fusing a low-cost IMU with landmark measurements through a quaternion-based unscented particle filter (QUPF-VIN). It develops a discrete, geometry-aware UKF-PF framework operating on the manifold $S^3$, with per-particle UKFs, augmented state handling IMU biases, and landmark updates from stereo vision. Key contributions include a quaternion-centric formulation, specialized sigma-point propagation and quaternion subtraction/addition operators, and a robust, low-sampling-rate implementation validated on the EuRoC indoor UAV dataset with clear accuracy gains over ground truth and a standard EKF. The method demonstrates practical impact for GPS-denied autonomous systems, enabling improved attitude, position, and velocity tracking in 6-DoF using affordable sensors.

Abstract

This paper introduces a geometric Quaternion-based Unscented Particle Filter for Visual-Inertial Navigation (QUPF-VIN) specifically designed for a vehicle operating with six degrees of freedom (6 DoF). The proposed QUPF-VIN technique is quaternion-based capturing the inherently nonlinear nature of true navigation kinematics. The filter fuses data from a low-cost inertial measurement unit (IMU) and landmark observations obtained via a vision sensor. The QUPF-VIN is implemented in discrete form to ensure seamless integration with onboard inertial sensing systems. Designed for robustness in GPS-denied environments, the proposed method has been validated through experiments with real-world dataset involving an unmanned aerial vehicle (UAV) equipped with a 6-axis IMU and a stereo camera, operating with 6 DoF. The numerical results demonstrate that the QUPF-VIN provides superior tracking accuracy compared to ground truth data. Additionally, a comparative analysis with a standard Kalman filter-based navigation technique further highlights the enhanced performance of the QUPF-VIN.

Figures (5)

• Figure 1: Illustrative diagram of QUPF-VIN implementation algorithm.
• Figure 2: A sample of matched landmark data points from left to right frame of EuRoC dataset Burri25012016.
• Figure 3: Performance assessment using the EuRoC V1_02_medium dataset Burri25012016. The left side shows UAV navigation (estimation) trajectory 3D space where the position is depicted in black solid line while the orientation is represented by red, green, and blue dashed lines. The right side presents normalized values of error vectors: orientation error $\|r_{e,k}\|$, position error $\|p_{e,k}\|$, and linear velocity error $\|v_{e,k}\|$ in blue solid lines.
• Figure 4: Estimation error: Rotation (left portion), position (middle portion), and linear velocity (rightp protion).
• Figure 5: Comparison results of EKF (literature in red) and the proposed QUPF-VIN (in blue).