Nonlinear Observer Design for Landmark-Inertial Simultaneous Localization and Mapping

This paper addresses the problem of Simultaneous Localization and Mapping (SLAM) for rigid body systems in three-dimensional space. We introduce a new matrix Lie group SE_{3+n}(3), whose elements are composed of the pose, gravity, linear velocity and landmark positions, and propose an almost globally asymptotically stable nonlinear geometric observer that integrates Inertial Measurement Unit (IMU) data with landmark measurements. The proposed observer estimates the pose and map up to a constant position and a constant rotation about the gravity direction. Numerical simulations are provided to validate the performance and effectiveness of the proposed observer, demonstrating its potential for robust SLAM applications.

Hybrid Feedback Control for Global Navigation with Locally Optimal Obstacle Avoidance in n-Dimensional Spaces

We present a hybrid feedback control framework for autonomous robot navigation in n-dimensional Euclidean spaces cluttered with spherical obstacles. The proposed approach ensures safe navigation and global asymptotic stability (GAS) of the target location by dynamically switching between two operational modes: motion-to-destination and locally optimal obstacle-avoidance. It produces continuous velocity inputs, ensures collision-free trajectories and generates locally optimal obstacle avoidance maneuvers. Unlike existing methods, the proposed framework is compatible with range sensors, enabling navigation in both a priori known and unknown environments. Extensive simulations in 2D and 3D settings, complemented by experimental validation on a TurtleBot 4 platform, confirm the efficacy and robustness of the approach. Our results demonstrate shorter paths and smoother trajectories compared to state-of-the-art methods, while maintaining computational efficiency and real-world feasibility.

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.