A novel Rauch-Tung-Streibel smoothing scheme based on the factor graph for autonomous underwater vehicles

In submarine surveying and mapping applications, a novel Rauch-Tung-Streibel smoothing (RTSS) scheme based on the factor graph for autonomous underwater vehicles is presented to gain a better offline navigation solution in this paper. The factor graph method is applied to optimally use observation information of multi-sensors with the asynchronous and short-term failure problems to overcome deficiencies of the federal Kalman filter in information fusion processing. Furthermore, the revised RTSS as a post-mission smoothing algorithm is performed by combining the results of the factor graph and one backward data processing through recursively updating the smoothed state and its covariance. From the simulation analysis, it is found that the factor graph mainly owns plug and play capability and contributes to the real-time navigation accuracy over the federal Kalman filtering. The RTSS provides better accuracy and smoothness for the position, velocity, and attitude at the same time compared to the corresponding real-time navigation solution, especially when signals are lost or sensors fail for a short time. With the best of both methods, a novel smoothing scheme combining the factor graph with the RTSS is built. Semi-physical experiment results verify the reliability and effectiveness of the proposed method.