Invariant Extended Kalman Filtering for Underwater Navigation

Recent advances in the utilization of Lie Groups for robotic localization have led to dramatic increases in the accuracy of estimation and uncertainty characterization. One of the novel methods, the Invariant Extended Kalman Filter (InEKF) extends the Extended Kalman Filter (EKF) by leveraging the fact that some error dynamics defined on matrix Lie Groups satisfy a log-linear differential equation. Utilization of these observations result in linearization with minimal approximation error, no dependence on current state estimates, and excellent convergence and accuracy properties. In this letter we show that the primary sensors used for underwater localization, inertial measurement units (IMUs) and doppler velocity logs (DVLs) meet the requirements of the InEKF. Furthermore, we show that singleton measurements, such as depth, can also be used in the InEKF update with minor modifications, thus expanding the set of measurements usable in an InEKF. We compare convergence, accuracy and timing results of the InEKF to a quaternion-based EKF using a Monte Carlo simulation and show notable improvements in long-term localization and much faster convergence with negligible difference in computation time.