Windowing-Based Factor Graph Optimization With Anomaly Detection Using Mahalanobis Distance for Underwater INS/DVL/USBL Integration

Factor graph optimization (FGO) provides a new means for asynchronous data fusion of integrated underwater vehicle navigation in a plug-and-play unified framework. However, in complex underwater environments, FGO suffers from observation anomalies, leading to deteriorated navigation solutions. This article proposes an improved factor graph with anomaly detection using Mahalanobis distance to overcome the above issue for inertial navigation system/Doppler velocity log/ultra-short baseline (INS/DVL/USBL) integrated underwater vehicle navigation. This method constructs a new factor graph model embedded with the node of anomaly detection for INS/DVL/USBL integration. Since the standard FGO computational load is increased with the number of the factor nodes, a sliding window technique is established to restrict the factor node number to improve the FGO computational efficiency. Based on above, a scheme of anomaly detection and regulation is presented for handling the disturbance of observation anomaly on system state estimation via the concept of Mahalanobis distance. Results of simulation and ground test experimentation show that the proposed methodology not only has the real-time performance but also has a strong robustness against observation anomaly for INS/DVL/USBL integrated navigation of underwater vehicles.