Active online self-calibration and accurate navigation via belief space planning and factor graph based incremental smoothing

High accuracy navigation in GPS-deprived environments is prime importance to various robotics applications and has been extensively investigated in the last two decades. Recent approaches have shown that incorporating sensor's calibration states in addition to the 6DOF pose states may cause better performance of the system. However, these approaches typically consider a passive setting, where robot actions are externally defined. On the other hand, belief space planning (BSP) approaches account for different sources of uncertainty, thus identifying actions that improve certain aspects in inference, such as accuracy. Yet, existing BSP approaches typically do not consider sensor calibration, nor a visual-inertial SLAM setup. In this paper we investigate a BSP approach for active online calibration of a visual-inertial SLAM and vision aided navigation system. In particular, our BSP approach determines robot actions (e.g. trajectory) for online self-calibration and high navigation accuracy for IMU-camera system while operating in unknown environments. We demonstrate our approach in high-fidelity synthetic simulation.