Pose, Velocity and Landmark Position Estimation Using IMU and Bearing Measurements (2407.18099v1)

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

• The paper introduces a globally exponentially stable observer that accurately estimates landmark positions and velocity using IMU and monocular camera data.
• The methodology integrates a linear time-varying observer with a nonlinear pose observer to achieve almost global asymptotic stability using limited landmark information.
• Observers recover unknown landmark positions under persistency of excitation, enhancing navigation reliability in GPS-denied environments.

Overview of Pose, Velocity, and Landmark Estimation Using IMU and Bearing Measurements

This paper addresses the estimation problem of pose, velocity, and landmark positions for a rigid body using an inertial measurement unit (IMU) and monocular camera measurements. The primary objective is the development of robust, globally exponentially and almost globally asymptotically stable estimation algorithms suited for applications in environments where global positioning systems (GPS) are unavailable, such as UAVs or autonomous vehicles.

The paper presents an estimation framework that unifies IMU and vision sensor data. It introduces a globally exponentially stable (GES) linear time-varying (LTV) observer to estimate landmark positions in the body frame and velocity using IMU and monocular bearing measurements. Subsequently, a nonlinear pose observer on $SO(3) \times \mathbb{R}^3$ is devised incorporating gyroscope measurements, known landmarks, and velocity estimates. This observer employs the concepts of almost global input-to-state stability (ISS) to achieve almost globally asymptotically stable (AGAS) performances, even when only a small number of landmark positions are known in the inertial frame.

Key Observations and Numerical Results

The analytical developments assure that with at least three non-aligned landmarks known in the inertial frame, the observer design achieves robust performance. A key result is the ability to recover unknown landmark positions when motion data satisfies a persistency of excitation (PE) condition, thus broadening the observable landscape beyond the limitations posed by monocular cameras.

The experimental simulations provided illustrate the effectiveness of the proposed observers in recovering landmark positions, validating the theoretical claims of system stability and robustness. The estimation algorithm demonstrates reliable convergence of the estimated states—attitude, position, and velocity—towards their true values. Furthermore, the ability to indirectly estimate the position of unknown landmarks demonstrates potential utility in mapping operations.

Theoretical and Practical Implications

From a theoretical perspective, this paper reinforces the validity of combining IMU data with monocular bearing measurements to solve SLAM-like challenges posed by GPS-denied environments. Practically, such methods promise advancements in UAV navigation systems or autonomous vehicle operations, where sensor cost and weight are a priority. The method innovatively uses minimal assumptions about landmark configuration, attributing robustness even with partial sensor information.

Future Directions

The research opens avenues for further exploration into hybrid observer designs that couple Kalman-type filters with nonlinear observers, potentially incorporating machine learning techniques for sensor data fusion. It would also be worthwhile to explore the scalability of these methods to larger-scale systems, investigate real-time robustness against noise and biases, and test applicability under diverse environmental conditions.

In summary, the paper offers a comprehensive approach to landmark position and pose estimation that contributes significantly to the field of robot navigation and control in challenging environments. By leveraging IMU and a monocular camera's simplistic setup, it exemplifies a practical solution of both academic interest and industrial applicability.