Pose, Velocity and Landmark Position Estimation Using IMU and Bearing Measurements

This paper investigates the estimation problem of the pose (orientation and position) and linear velocity of a rigid body, as well as the landmark positions, using an inertial measurement unit (IMU) and a monocular camera. First, we propose a globally exponentially stable (GES) linear time-varying (LTV) observer for the estimation of body-frame landmark positions and velocity, using IMU and monocular bearing measurements. Thereafter, using the gyro measurements, some landmarks known in the inertial frame and the estimates from the LTV observer, we propose a nonlinear pose observer on $\SO(3)\times \mathbb{R}^3$. The overall estimation system is shown to be almost globally asymptotically stable (AGAS) using the notion of almost global input-to-state stability (ISS). Interestingly, we show that with the knowledge (in the inertial frame) of a small number of landmarks, we can recover (under some conditions) the unknown positions (in the inertial frame) of a large number of landmarks. Numerical simulation results are presented to illustrate the performance of the proposed estimation scheme.

Hybrid Feedback for Three-dimensional Convex Obstacle Avoidance

We propose a hybrid feedback control scheme for the autonomous robot navigation problem in three-dimensional environments with arbitrarily-shaped convex obstacles. The proposed hybrid control strategy, which consists in switching between the move-to-target mode and the obstacle-avoidance mode, guarantees global asymptotic stability of the target location in the obstacle-free workspace. We also provide a procedure for the implementation of the proposed hybrid controller in a priori unknown environments and validate its effectiveness through simulation results.

Hybrid Feedback for Autonomous Navigation in Environments with Arbitrary Non-Convex Obstacles

We develop an autonomous navigation algorithm for a robot operating in two-dimensional environments containing obstacles, with arbitrary non-convex shapes, which can be in close proximity with each other, as long as there exists at least one safe path connecting the initial and the target location. The proposed navigation approach relies on a hybrid feedback to guarantee global asymptotic stabilization of the robot towards a predefined target location while ensuring the forward invariance of the obstacle-free workspace. The proposed hybrid feedback controller guarantees Zeno-free switching between the move-to-target mode and the obstacle-avoidance mode based on the proximity of the robot with respect to the obstacle-occupied workspace. An instrumental transformation that reshapes (virtually) the non-convex obstacles, in a non-conservative manner, is introduced to facilitate the design of the obstacle-avoidance strategy. Finally, we provide an algorithmic procedure for the sensor-based implementation of the proposed hybrid controller and validate its effectiveness through simulation results.

Hybrid Feedback for Autonomous Navigation in Environments with Arbitrary Convex Obstacles

We develop an autonomous navigation algorithm for a robot operating in two-dimensional environments cluttered with obstacles having arbitrary convex shapes. The proposed navigation approach relies on a hybrid feedback to guarantee global asymptotic stabilization of the robot towards a predefined target location while ensuring the forward invariance of the obstacle-free workspace. The main idea consists in designing an appropriate switching strategy between the move-to-target mode and the obstacle-avoidance mode based on the proximity of the robot with respect to the nearest obstacle. The proposed hybrid controller generates continuous velocity input trajectories when the robot is initialized away from the boundaries of the unsafe regions. Finally, we provide an algorithmic procedure for the sensor-based implementation of the proposed hybrid controller and validate its effectiveness through some simulation results.

Nonlinear Attitude Estimation Using Intermittent Linear Velocity and Vector Measurements

This paper investigates the problem of continuous attitude estimation on $SO(3)$ using continuous angular velocity and linear acceleration measurements as well as intermittent linear velocity and inertial vector measurements. First, we propose a nonlinear observer for the case where all the measurements are continuous and almost global asymptotic stability (AGAS) is shown using the notion of almost global input-to-state stability (ISS) on manifolds. Thereafter, a hybrid attitude observer, with AGAS guarantees, is proposed in terms of intermittent linear velocity and vector measurements. Numerical simulation results are presented to illustrate the performance of the proposed hybrid observer.

Nonlinear Observers Design for Vision-Aided Inertial Navigation Systems

This paper deals with the simultaneous estimation of the attitude, position and linear velocity for vision-aided inertial navigation systems. We propose a nonlinear observer on $SO(3)\times \mathbb{R}^{15}$ relying on body-frame acceleration, angular velocity and (stereo or monocular) bearing measurements of some landmarks that are constant and known in the inertial frame. Unlike the existing local Kalman-type observers, our proposed nonlinear observer guarantees almost global asymptotic stability and local exponential stability. A detailed uniform observability analysis has been conducted and sufficient conditions are derived. Moreover, a hybrid version of the proposed observer is provided to handle the intermittent nature of the measurements in practical applications. Simulation and experimental results are provided to illustrate the effectiveness of the proposed state observer.

Hybrid Feedback for Global Attitude Tracking

We introduce a new hybrid control strategy, which is conceptually different from the commonly used synergistic hybrid approaches, to efficiently deal with the problem of the undesired equilibria that precludes smooth vectors fields on $SO(3)$ from achieving global stability. The key idea consists in constructing a suitable potential function on $SO(3)\times \mathbb{R}$ involving an auxiliary scalar variable, with flow and jump dynamics, which keeps the state away from the undesired critical points while, at the same time, guarantees a decrease of the potential function over the flows and jumps. Based on this new hybrid mechanism, a hybrid feedback control scheme for the attitude tracking problem on $SO(3)$, endowed with global asymptotic stability and semi-global exponential stability guarantees, is proposed. This control scheme is further improved through a smoothing mechanism that removes the discontinuities in the input torque. The third hybrid control scheme, proposed in this paper, removes the requirement of the angular velocity measurements, while preserving the strong stability guarantees of the first hybrid control scheme. Finally, some simulation results are presented to illustrate the performance of the proposed hybrid controllers.

Observers Design for Inertial Navigation Systems: A Brief Tutorial

The design of navigation observers able to simultaneously estimate the position, linear velocity and orientation of a vehicle in a three-dimensional space is crucial in many robotics and aerospace applications. This problem was mainly dealt with using the extended Kalman filter and its variants which proved to be instrumental in many practical applications. Although practically efficient, the lack of strong stability guarantees of these algorithms motivated the emergence of a new class of geometric navigation observers relying on Riemannian geometry tools, leading to provable strong stability properties. The objective of this brief tutorial is to provide an overview of the existing estimation schemes, as well as some recently developed geometric nonlinear observers, for autonomous navigation systems relying on inertial measurement unit (IMU) and landmark measurements.

Nonlinear State Estimation for Inertial Navigation Systems With Intermittent Measurements

This paper considers the problem of simultaneous estimation of the attitude (orientation), position and linear velocity for vehicles navigating in a three-dimensional space. We propose two types of hybrid nonlinear observers using continuous angular velocity and linear acceleration measurements as well as intermittent landmark position measurements. The first one relies on a fixed-gain design approach based on an infinite-dimensional optimization, while the second one relies on a variable-gain design approach based on a continuous-discrete Riccati equation. For each case, we provide two different observers with and without the estimation of the gravity vector, respectively. The proposed observers are shown to be exponentially stable with a large domain of attraction. Simulation and experimental results are presented to illustrate the performance of the proposed observers.

Hybrid Nonlinear Observers for Inertial Navigation Using Landmark Measurements

This paper considers the problem of attitude, position and linear velocity estimation for rigid body systems relying on landmark measurements. We propose two hybrid nonlinear observers on the matrix Lie group $SE_2(3)$, leading to global exponential stability. The first observer relies on fixed gains, while the second one uses variable gains depending on the solution of a continuous Riccati equation (CRE). These observers are then extended to handle biased angular velocity measurements. Both simulation and experimental results are presented to illustrate the performance of the proposed observers.