Discrete-Time Rigid Body Pose Estimation Based on Lagrange–d’Alembert Principle

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 minimizing 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 proved to be almost globally asymptotically stable. The filtering scheme is simulated using noisy sensor data to verify the theoretical properties.