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

TL;DR

This paper presents a novel estimation framework combining IMU and monocular camera data to accurately determine pose, velocity, and landmark positions of a rigid body with proven stability properties.

Contribution

It introduces a globally exponentially stable linear observer and a nonlinear pose observer on SO(3), enabling landmark position recovery with minimal known landmarks.

Findings

The proposed observers are almost globally asymptotically stable.

Landmark positions can be estimated with limited known landmarks.

Numerical simulations demonstrate the effectiveness of the estimation scheme.

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.