Riccati Observer Design for Pose, Linear Velocity and Gravity Direction Estimation Using Landmark Position and IMU Measurements

Abstract

This paper revisits the problem of estimating the pose (i.e. position and attitude) of a robotic vehicle by combining landmark position measurements provided by a stereo camera with measurements of an Inertial Measurement Unit. The distinguished features with respect to similar works on the topic are two folds: First, the vehicle's linear velocity is not measured neither in the body frame nor in the inertial frame; Second, no prior knowledge on the gravity direction expressed in the inertial frame is required. Instead both the linear velocity and the gravity direction are estimated together with the pose. Another innovative feature of the paper relies on the idea of over-parametrizing the gravity direction vector evolving on the unit 2-sphere $S^{2}$ by an element of SO(3) so that the error system in first order approximations can be written in an “elegant” linear time-varying form. The proposed deterministic observer is accompanied with an observability analysis that points out an explicit observability condition under which local exponential stability is granted. Reported simulation results further indicate that the observer's domain of convergence is large.