Geometric State Observers for Autonomous Navigation Systems

The development of reliable state estimation algorithms for autonomous navigation systems is of great interest in the control and robotics communities. This thesis studies the state estimation problem for autonomous navigation systems. The first part of this thesis is devoted to the pose estimation on the Special Euclidean group \SE(3). A generic globally exponentially stable hybrid estimation scheme for pose (orientation and position) and velocity-bias estimation on \SE(3)\times \mathbb{R}^6 is proposed. Moreover, an explicit hybrid observer, using inertial and landmark position measurements, is provided. The second part of this thesis is devoted to the problem of simultaneous estimation of the attitude, position and linear velocity for inertial navigation systems (INSs). Three different types of nonlinear observers are developed to handle the following cases: continuous landmark position measurements, intermittent landmark position measurements and continuous stereo bearing measurements. First, a class of nonlinear geometric hybrid observers on the Lie group \SE_2(3), with GES guarantees, using continuous IMU and landmark position measurements is developed. Then, a class of nonlinear state observers, with strong stability guarantees, using intermittent landmark measurements is proposed. Finally, a class of state observers, with strong stability guarantees, directly incorporating body-frame stereo-bearing measurements, is proposed

Nonlinear Observer for Visual-Inertial Navigation Using Intermittent Landmark Measurements

The development of reliable orientation, position and linear velocity estimation algorithms for the 3D visual-inertial navigation system (VINS) is instrumental in many applications, such as autonomous underwater vehicles (AUVs), and unmanned aerial vehicles (UAVs). It is extremely important when the global position system (GPS) is not available in GPS-denied environments. Recently, observers design for VINS using landmark position measurements from Kinect sensors or stereo cameras has been increasingly investigated in the literature. The aim of this work is to design a nonlinear observer for VINS under the assumption that landmark position measurements are intermittent. In practice, the landmark measurements are not continuous due to computation cost from image processing, which is different from most of the existing results relying on continuous landmark measurements. The proposed nonlinear observer, motivated from the classical linear Kalman filter, has two parts: continuous prediction using inertial measurement unit and previous landmark measurements, and instantaneous state updating upon the arrival of new landmark measurements. Almost global asymptotic stability (AGAS) has been achieved by applying the framework of the hybrid dynamical system, which means that the estimated state will asymptotically converge to the real state of the visual-inertial navigation system for almost all initial conditions. We strongly believe that our developed estimation tool will not only benefit the area of aerial vehicles engineering but also the robotics and biomedical engineering community

A Global Asymptotic Convergent Observer for SLAM

This paper examines the global convergence problem of SLAM algorithms, an issue that faces topological obstructions. This is because the state-space of attitude dynamics is defined on a non-contractible manifold: the special orthogonal group of order three SO(3). Therefore, this paper presents a novel, gradient-based hybrid observer to overcome these topological obstacles. The Lyapunov stability theorem is used to prove the globally asymptotic convergence of the proposed algorithm. Finally, comparative analyses of two simulations were conducted to evaluate the performance of the proposed scheme and to demonstrate the superiority of the proposed hybrid observer to a smooth observer.Comment: 7 pages, 8 figures, conferenc

Hybrid Controller for Robot Manipulators in Task-Space with Visual-Inertial Feedback

This paper presents a visual-inertial-based control strategy to address the task space control problem of robot manipulators. To this end, an observer-based hybrid controller is employed to control end-effector motion. In addition, a hybrid observer is introduced for a visual-inertial navigation system to close the control loop directly at the Cartesian space by estimating the end-effector pose. Accordingly, the robot tip is equipped with an inertial measurement unit (IMU) and a stereo camera to provide task-space feedback information for the proposed observer. It is demonstrated through the Lyapunov stability theorem that the resulting closed-loop system under the proposed observer-based controller is globally asymptotically stable. Besides this notable merit (global asymptotic stability), the proposed control method eliminates the need to compute inverse kinematics and increases trajectory tracking accuracy in task-space. The effectiveness and accuracy of the proposed control scheme are evaluated through computer simulations, where the proposed control structure is applied to a 6 degrees-of-freedom long-reach hydraulic robot manipulator

Nonlinear Attitude Estimation Using Intermittent and Multi-Rate Vector Measurements

This paper considers the problem of nonlinear attitude estimation for a rigid body system using intermittent and multi-rate inertial vector measurements as well as continuous (high-rate) angular velocity measurements. Two types of hybrid attitude observers on Lie group $SO(3)$ are proposed. First, we propose a hybrid attitude observer where almost global asymptotic stability is guaranteed using the notion of almost global input-to-state stability on manifolds. Thereafter, this hybrid attitude observer is extended by introducing a switching mechanism to achieve global asymptotic stability. Both simulation and experimental results are presented to illustrate the performance of the proposed hybrid observers.Comment: 22 pages, 7 figures, submitted to IEEE TAC for possible publicatio

InGVIO: A Consistent Invariant Filter for Fast and High-Accuracy GNSS-Visual-Inertial Odometry

Combining Global Navigation Satellite System (GNSS) with visual and inertial sensors can give smooth pose estimation without drifting in geographical coordinates. The fusion system gradually degrades to Visual-Inertial Odometry (VIO) with the number of satellites decreasing, which guarantees robust global navigation in GNSS unfriendly environments. In this letter, we propose an open-sourced invariant filter-based platform, InGVIO, to tightly fuse monocular/stereo visual-inertial measurements, along with raw data from GNSS, i.e. pseudo ranges and Doppler shifts. InGVIO gives highly competitive results in terms of accuracy and computational load compared to current graph-based and `naive' EKF-based algorithms. Thanks to our proposed key-frame marginalization strategies, the baseline for triangulation is large although only a few cloned poses are kept. Besides, landmarks are anchored to a single cloned pose to fit the nonlinear log-error form of the invariant filter while achieving decoupled propagation with IMU states. Moreover, we exploit the infinitesimal symmetries of the system, which gives equivalent results for the pattern of degenerate motions and the structure of unobservable subspaces compared to our previous work using observability analysis. We show that the properly-chosen invariant error captures such symmetries and has intrinsic consistency properties. InGVIO is tested on both open datasets and our proposed fixed-wing datasets with variable levels of difficulty. The latter, to the best of our knowledge, are the first datasets open-sourced to the community on a fixed-wing aircraft with raw GNSS.Comment: 8 pages, 8 figures; manuscript will be submitted to IEEE RA-L for possible publicatio