Geometric Stochastic Filter with Guaranteed Performance for Autonomous Navigation based on IMU and Feature Sensor Fusion (2108.11866v1)

Abstract: This paper concerns the estimation problem of attitude, position, and linear velocity of a rigid-body autonomously navigating with six degrees of freedom (6 DoF). The navigation dynamics are highly nonlinear and are modeled on the matrix Lie group of the extended Special Euclidean Group $\mathbb{SE}{2}(3)$. A computationally cheap geometric nonlinear stochastic navigation filter is proposed on $\mathbb{SE}{2}(3)$ with guaranteed transient and steady-state performance. The proposed filter operates based on a fusion of sensor measurements collected by a low-cost inertial measurement unit (IMU) and features (obtained by a vision unit). The closed loop error signals are guaranteed to be almost semi-globally uniformly ultimately bounded in the mean square from almost any initial condition. The equivalent quaternion representation is included in the Appendix. The filter is proposed in continuous form, and its discrete form is tested on a real-world dataset of measurements collected by a quadrotor navigating in three dimensional (3D) space. Keywords: Localization, navigation, position and orientation estimation, stochastic systems, GPS-denied navigation observer, navigation estimator, vision-aided inertial navigation systems (VA-INSs), stochastic differential equation, Gaussian noise, sensor fusion.

• The paper introduces a novel geometric nonlinear stochastic filter that guarantees both transient and steady-state performance.
• It fuses low-cost IMU and feature sensor measurements on SE₂(3) to accurately estimate attitude, position, and velocity in 6 DoF.
• Experimental results show rapid convergence and robust tracking in GPS-denied environments, making it ideal for cost-effective autonomous vehicles.

Geometric Stochastic Filter for Autonomous Navigation

The paper "Geometric Stochastic Filter with Guaranteed Performance for Autonomous Navigation based on IMU and Feature Sensor Fusion," authored by H. A. Hashim, M. Abouheaf, and M. A. Abido, addresses the challenge of developing reliable and accurate navigation solutions for autonomous vehicles operating in environments where GPS signals are unavailable or unreliable. The focus is on a navigation filter that uses low-cost inertial measurement units (IMUs) and feature sensors for estimation processes.

Research Focus

The core objective of the paper is to estimate the attitude, position, and linear velocity of a rigid-body system navigating with six degrees of freedom (6 DoF), encapsulating highly nonlinear navigation dynamics modeled on the Lie group $\mathbb{SE}_{2}(3)$. The paper proposes a novel geometric nonlinear stochastic filter that guarantees transient and steady-state performance. This is realized through sensor measurements fusion—specifically, data obtained from a low-cost IMU and feature observations from vision units.

Methodology

The navigation problem is formulated as a stochastic differential equation, incorporating measurements contaminated with Gaussian noise and bias. The paper introduces a prescribed performance function (PPF) that guides the error dynamics, ensuring systematic reduction from a known large set to a small set, effectively encompassing both transient and steady-state performance specifications.

The proposed filter, evolved on the extended Special Euclidean Group $\mathbb{SE}_{2}(3)$, transforms the error into an unconstrained form through a smooth bounded function, which enables handling large deviations from the intended trajectory. The results emphasize the theoretical underpinning guaranteeing that the closed-loop error signals remain almost semi-globally uniformly ultimately bounded in the mean square. The solution is computationally efficient, making it attractive for low-cost autonomous vehicles such as drones.

Results and Experimental Validation

The filter's practicality and robustness were validated using real-world data from the EuRoC dataset, which includes high-accuracy ground truths synchronized with IMU and stereo camera observations. The experimental results demonstrate that the filter efficiently aligns estimated states with the true states concerning attitude, position, and velocity, even in the presence of noisy data and initial estimation errors.

Significantly, the filter successfully converged to the true trajectory within constraints defined by the PPF, showing rapid recovery and strong tracking capabilities. The filter's robustness against perturbations in uncertain environments was clearly demonstrated.

Implications and Future Directions

Practically, the filter can support autonomous navigation for a variety of robotic systems by prioritizing low-cost sensors’ outputs, expanding the reach of autonomous technology into sectors and applications where premium sensors are not feasible due to cost constraints. Theoretically, the research advances the robust stabilization problem in stochastic systems through the geometry of Lie groups, particularly suited to navigational dynamics of higher complexity.

Further exploration could include extending the methodology to multi-agent systems where decentralized fusion solutions would be necessary. Moreover, adapting the scheme for higher frequency dynamics and real-time applications opens the pathway for sophisticated control systems integrated directly with navigation tasks. As stochastic modeling of sensors and dynamics advances, the presented geometric filtering approach may become a foundational component in robust navigation solutions across various domains.