Filter and piecewise smoother on the matrix Lie group

Abstract

We propose a filter and a piecewise smoother based on the matrix Lie group of double direct isometries (${\mathrm{SE}}_{2}(3)$) to improve the accuracy of the Global Navigation Satellite System/Inertial Navigation System (GNSS/INS) system based on a low-cost inertial measurement unit (IMU). First, we derive the ${\mathrm{SE}}_{2}(3)$-extended Kalman filter (${\mathrm{SE}}_{2}\left(3\right)$-EKF) based on the group of double direct isometries in the earth-centered earth-fixed (ECEF) frame, and we prove that the left ${\mathrm{SE}}_{2}\left(3\right)$-EKF satisfies the posterior Cramer–Rao lower bound. Next, we develop the extended information filter (EIF) and two-filter smoother (TFS) based on the matrix Lie group of double direct isometries. Finally, we design a piecewise smoother algorithm to improve the real-time performance of the smoother. Experiments show that the plane and 3D positioning accuracy of the proposed filter and smoother based on the Lie group is better than that based on the Euclidean space. Compared to the traditional filter and smoother based on the Euclidean space, the root mean square (RMS) of the plane and 3D positioning errors of our proposed algorithm are reduced by 38.6% and 17.4% before smoothing, and 46.8% and 26.7% after smoothing. Meanwhile, when the piecewise length reaches 1 min on the data of about 27 min, the piecewise smoother we proposed can almost achieve the accuracy of using full data to smooth. Without losing precision, the piecewise smoother can get the smoothing result in a shorter time and thus improve the real-time performance of the smoothing algorithm.