Invariant RTS Smoother and Its Application in 5G/SINS Integrated Navigation System

Objectives In order to solve the problem that the navigation system’s positioning error accumulates over time, we propose an invariant RTS (InRTS) smooting algorithm, in which the state space is the matrix Lie group, to improve the accuracy of the navigation state. The core of this algorithm is that the representation and manipulation of the navigation state and observation is within matrix Lie group.

Methods First, the system states and their corresponding error states are represented by matrix Lie group. Compared with extended Kalman filtering (EKF), the system state is constructed on matrix Lie group, and its dynamic equation can describe the motion characteristic of vehicles in manifold space in a more natural and essential way. The error states defined by group operation are coupled with attitude errors instead of direct vector substraction. Second, the invariant error state is mapped into the Lie algebraic space through a logarithmic mapping between the Lie group and Lie algebra, and an analytic linear differential equation independent of the current state estimation is obtained, thereby obtaining a state-independent Jacobian matrix at any linearization point. Third, the left invariant error state measurement equation including the lever arm error is derived as a result of the left-invariant characteristics of the measurement model in 5G positioning, and a left invariant EKF is obtained. Finally, left invariant EKF was used as the forward filtering, and the InRTS smoothing algorithm was proposed and applied to the 5G/SINS(strapdown inertial navigation system) integrated navigation system. The accuracy of the proposed algorithm was compared with that of the left invariant EKF algorithm in the 5G/SINS simulation experiment.

Results and Conclusions Experimental results show that the InRTS algorithm can smooth the trajectory obtained by left invariant EKF algorithm and reduce the cumulative error of navigation state in the forward filtering process, thus proving the effectiveness of the proposed algorithm.