Schmidt-EKF-based Visual-Inertial Moving Object Tracking

In this paper we investigate the effect of tightly-coupled estimation on the performance of visual-inertial localization and dynamic object pose tracking. In particular, we show that while a joint estimation system outperforms its decoupled counterpart when given a "proper" model for the target’s motion, inconsistent modeling, such as choosing improper levels for the target’s propagation noises, can actually lead to a degradation in ego-motion accuracy. To address the realistic scenario where a good prior knowledge of the target’s motion model is not available, we design a new system based on the Schmidt-Kalman Filter (SKF), in which target measurements do not update the navigation states, however all correlations are still properly tracked. This allows for both consistent modeling of the target errors and the ability to update target estimates whenever the tracking sensor receives non-target data such as bearing measurements to static, 3D environmental features. We show in extensive simulation that this system, along with a robot-centric representation of the target, leads to robust estimation performance even in the presence of an inconsistent target motion model. Finally, the system is validated in a real-world experiment, and is shown to offer accurate localization and object pose tracking performance.