Relative Pose-Velocity Estimation Using Dual IMU Measurements and Relative Position Sensing

This paper addresses the problem of estimating the relative pose (position and orientation) and velocity of a vehicle with respect to a moving target, where both are equipped with Inertial Measurement Units (IMUs), assuming the availability of relative position or bearing measurements. The body-target relative dynamics are formulated on $\mathbf{SE}_2(3)$ and recast into a linear time-varying (LTV) model in the ambient space $\mathbb{R}^{15}$, on which a deterministic Riccati observer is designed. We analyze the uniform observability (UO) conditions required to guarantee global exponential convergence of the estimation error in the ambient space for both measurement cases. In the case of relative position measurements, UO requires only a persistence-of-excitation condition on the target acceleration, whereas for bearing measurements, additional conditions are required. Building on this, a nonlinear complementary filter on $\mathbf{SO}(3)$ is designed to provide a smooth estimate of the orientation component of the state with almost global asymptotic stability. Finally, simulation results are provided to validate the proposed solution.

Complementary Filtering on SO(3) for Attitude Estimation with Scalar Measurements

Attitude estimation using scalar measurements, corresponding to partial vectorial observations, arises naturally when inertial vectors are not fully observed but only measured along specific body-frame vectors. Such measurements arise in problems involving incomplete vector measurements or attitude constraints derived from heterogeneous sensor information. Building on the classical complementary filter on SO(3), we propose an observer with a modified innovation term tailored to this scalar-output structure. The main result shows that almost-global asymptotic stability is recovered, under suitable persistence of excitation conditions, when at least three inertial vectors are measured along a common body-frame vector, which is consistent with the three-dimensional structure of SO(3). For two-scalar configurations - corresponding either to one inertial vector measured along two body-frame vectors, or to two inertial vectors measured along a common body-frame vector - we further derive sufficient conditions guaranteeing convergence within a reduced basin of attraction. Different examples and numerical results demonstrate the effectiveness of the proposed scalar-based complementary filter for attitude estimation in challenging scenarios involving reduced sensing and/or novel sensing modalities.

Scalar-Measurement Attitude Estimation on $\mathbf{SO}(3)$ with Bias Compensation

Attitude estimation methods typically rely on full vector measurements from inertial sensors such as accelerometers and magnetometers. This paper shows that reliable estimation can also be achieved using only scalar measurements, which naturally arise either as components of vector readings or as independent constraints from other sensing modalities. We propose nonlinear deterministic observers on $\mathbf{SO}(3)$ that incorporate gyroscope bias compensation and guarantee uniform local exponential stability under suitable observability conditions. A key feature of the framework is its robustness to partial sensing: accurate estimation is maintained even when only a subset of vector components is available. Experimental validation on the BROAD dataset confirms consistent performance across progressively reduced measurement configurations, with estimation errors remaining small even under severe information loss. To the best of our knowledge, this is the first work to establish fundamental observability results showing that two scalar measurements under suitable excitation suffice for attitude estimation, and that three are enough in the static case. These results position scalar-measurement-based observers as a practical and reliable alternative to conventional vector-based approaches.

Vision-Aided Relative State Estimation for Approach and Landing on a Moving Platform with Inertial Measurements

This paper tackles the problem of estimating the relative position, orientation, and velocity between a UAV and a planar platform undergoing arbitrary 3D motion during approach and landing. The estimation relies on measurements from Inertial Measurement Units (IMUs) mounted on both systems, assuming there is a suitable communication channel to exchange data, together with visual information provided by an onboard monocular camera, from which the bearing (line-of-sight direction) to the platform's center and the normal vector of its planar surface are extracted. We propose a cascade observer with a complementary filter on SO(3) to reconstruct the relative attitude, followed by a linear Riccati observer for relative position and velocity estimation. Convergence of both observers is established under persistently exciting conditions, and the cascade is shown to be almost globally asymptotically and locally exponentially stable. We further extend the design to the case where the platform's rotation is restricted to its normal axis and show that its measured linear acceleration can be exploited to recover the remaining unobservable rotation angle. A sufficient condition to ensure local exponential convergence in this setting is provided. The performance of the proposed observers is validated through extensive simulations.

Observer Design for Optical Flow-Based Visual-Inertial Odometry with Almost-Global Convergence

This paper presents a novel cascaded observer architecture that combines optical flow and IMU measurements to perform continuous monocular visual-inertial odometry (VIO). The proposed solution estimates body-frame velocity and gravity direction simultaneously by fusing velocity direction information from optical flow measurements with gyro and accelerometer data. This fusion is achieved using a globally exponentially stable Riccati observer, which operates under persistently exciting translational motion conditions. The estimated gravity direction in the body frame is then employed, along with an optional magnetometer measurement, to design a complementary observer on $\mathbf{SO}(3)$ for attitude estimation. The resulting interconnected observer architecture is shown to be almost globally asymptotically stable. To extract the velocity direction from sparse optical flow data, a gradient descent algorithm is developed to solve a constrained minimization problem on the unit sphere. The effectiveness of the proposed algorithms is validated through simulation results.

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.

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 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.

Obstacle Avoidance via Hybrid Feedback

In this paper we present a hybrid feedback approach to solve the navigation problem of a point mass in the n-dimensional space containing an arbitrary number of ellipsoidal shape obstacles. The proposed hybrid control algorithm guarantees both global asymptotic stabilization to a reference and avoidance of the obstacles. The intuitive idea of the proposed hybrid feedback is to switch between two modes of control: stabilization and avoidance. The geometric construction of the flow and jump sets of the proposed hybrid controller, exploiting hysteresis regions, guarantees Zeno-free switching between the stabilization and the avoidance modes. Simulation results illustrate the performance of the proposed hybrid control approach for 2-dimensional and 3-dimensional scenarios.