LAUSR

Factor graph optimization (FGO) is a well-known approach in the robotics field. Due to multiple iterations and application of large amounts of data, it usually could superior performance than the Kalman Filter (KF) under complex scenarios. However, it is rarely used in the underwater environment, especially for applications based on fiber-optic inertial navigation. Taking the ultra-short baseline system (USBL) as the application background, a novel inertial navigation system (INS)/USBL integrated navigation scheme based on FGO is presented in this paper. The traditional pre-integration factor simplifies the earth rotation and coordinate system, which limits its application in high-precision sensors. This paper proposed an improved pre-integration model using the high precision measurements of fiber optics gyroscope in FGO. Simulation and field tests proved that, with the FGO method, improved accuracy of INS/USBL integration was obtained compared to the traditional KF method. It confirms the potential of FGO in the applications of underwater navigation and positioning.