A Factor Graph Optimization-Based In-Motion Alignment Method for INS/DVL Integration

An accurate initial attitude is of great importance for the integration of Doppler velocity log (DVL) and the strapdown inertial navigation system (SINS). The speed and accuracy of the initial alignment are two important indicators. The process is usually divided into two stages, coarse alignment and fine alignment, to ensure the speed and precision of the alignment. To simplify the alignment steps and improve accuracy, a factor graph optimization(FGO)-based in-motion alignment method is proposed in the paper. First, an IMU factor based on the exact preintegration measurement model is proposed, which is especially suitable for initial attitude alignment of high precision strapdown inertial navigation system. Second, a nonlinear optimization problem based on graph model is constructed with the IMU factor and DVL factor. An incremental fixed lag smoothing method is adopted to solve the nonlinear optimization problem. Due to multiple iterations and batch optimization of the historical data, it can be superior to the traditional alignment method. Both the simulation and field tests show that FGO-based alignment speed is fast enough. It is comparable to that of the coarse alignment and faster than the filter-based method. Besides, the alignment accuracy is still the highest. It confirmed that FGO-based alignment method has great application potential in INS/DVL integration.