An Extended Stochastic Cloning Method for Fusing Multi-relative Measurements

One of the most important tasks for visual inertial odometry systems is pose estimation. By integrating system poses, motion trajectory of the system can be obtained. Due to errors existing in calculations, the accumulated errors grow unbounded. To decrease the drift, keyframes and loop-closure information can be used as additional references for the system. To use this information, the system should able to handle multi-relative measurements which cross many periods of filter cycles. In Kalman filter based system the fusion of such information is one of the toughest problems. In this paper, we propose an extended stochastic cloning method to overcome this problem. The proposed method is based on the error state Kalman filter. It also can be used in other Kalman filters. The experimental results show that based on the proposed method trajectory errors and uncertainties of filtered results are decreased significantly. At the same time, the IMU’s biases are modeled as a random-walk noise and be updated as well. This way, by using keyframes and loop-closure information, the proposed method is able to improve the accuracy of the sensor models.