Observer design for visual-inertial estimation of pose, linear velocity and gravity direction in planar environments

Vision-aided inertial navigation systems combine data from a camera and an IMU to estimate the position, orientation, and linear velocity of a moving vehicle. In planar environments, existing methods assume knowledge of the vertical direction and ground plane to exploit accelerometer measurements. This paper presents a new solution that extends the estimation to arbitrary planar environments. A deterministic Riccati observer is designed to estimate the direction of gravity along with the vehicle pose, linear velocity, and the normal direction to the plane by fusing bearing correspondences from an image sequence with angular velocity and linear acceleration data. Comprehensive observability and stability analysis establishes an explicit persistent excitation condition under which local exponential stability of the observer is achieved. Simulation and real-world experimental results illustrate the performance and robustness of the proposed approach.