Stochastic cloning Kalman filter for visual odometry and inertial/magnetic data fusion

The work demonstrates the fusion of the position and the orientation information from Visual Odometry (VO) with the orientation information obtained from low-cost inertial and magnetic sensors. The proposed approach is based on the stochastic cloning (SC) Kalman filter formulation which is able to incorporate independent incremental measurements in a statistically consistent way. The algorithm was tested on realistic trajectories and compared to the results of a pure VO as well as to those of a decoupled system. A drift in the heading estimation is addressed by incorporating the Earth´s magnetic field measurements with associated heuristics for more robust disturbance detection.