Discrete-time Rigid Body Pose Estimation based on Lagrange-d'Alembert principle

TL;DR

This paper develops a novel discrete-time pose estimation method for rigid bodies using the Lagrange-d'Alembert principle, combining vision and inertial sensors, and proves its stability through Lyapunov analysis.

Contribution

It introduces a new optimal pose estimation filter based on discrete Lagrangian mechanics and stability analysis, integrating sensor data effectively.

Findings

The proposed filter is almost globally asymptotically stable.

Simulation results confirm robustness with noisy sensor data.

The method accurately estimates pose in discrete-time settings.

Abstract

The problem of rigid body pose estimation is treated in discrete-time via discrete Lagrange-d'Alembert principle and discrete Lyapunov methods. The position and attitude of the rigid body are to be estimated simultaneously with the help of vision and inertial sensors. For the discrete-time estimation of pose, the continuous-time rigid body kinematics equations are discretized appropriately. We approach the pose estimation problem as minimising the energies stored in the errors of estimated quantities. With the help of measurements obtained through optical sensors, artificial rotational and translation potential energy-like terms have been designed. Similarly, artificial rotational and translation kinetic energy-like terms have been devised using inertial sensor measurements. This allows us to construct a discrete-time Lagrangian as the difference of the kinetic and potential energy like terms, to which a Lagrange-d'Alembert principle is applied to obtain an optimal pose estimation filter. The dissipation terms in the optimal filter are designed through discrete-Lyapunov analysis on a suitably constructed Morse-Lyapunov function and the overall scheme is proven to be almost globally asymptotically stable. The filtering scheme is simulated using noisy sensor data to verify the theoretical properties.