Dual-model reverse CKF algorithm in cooperative navigation for USV

As one of the most promising research directions, cooperative location with high precision and low-cost IMU is becoming an emerging research topic in many positioning fields. Low-cost MEMS/DVL is a preferred solution for dead-reckoning in multi-USV cooperative network. However, large misalignment angles and large gyro drift coexist in low-cost MEMS that leads to the poor observability. Based on cubature Kalman filter (CKF) algorithm that has access to high accuracy and relative small computation, dual-model filtering scheme is proposed. It divides the whole process into two subsections that cut off the coupling relations and improve the observability of MEMS errors: it first estimates large misalignment angle and then estimates the gyro drift. Furthermore, to improve the convergence speed of large misalignment angle estimated in the first subsection, "time reversion" concept is introduced. It uses a short period time to forward and backward several times to improve convergence speed effectively. Finally, simulation analysis and experimental verification is conducted. Simulation and experimental results show that the algorithm can effectively improve the cooperative navigation performance.