罗亚荣, 郭文飞, 齐书峰, 郭迟. 不变RTS平滑算法及其在5G/SINS组合导航中的应用研究[J]. 武汉大学学报 ( 信息科学版), 2024, 49(5): 747-755. DOI: 10.13203/j.whugis20210587
引用本文: 罗亚荣, 郭文飞, 齐书峰, 郭迟. 不变RTS平滑算法及其在5G/SINS组合导航中的应用研究[J]. 武汉大学学报 ( 信息科学版), 2024, 49(5): 747-755. DOI: 10.13203/j.whugis20210587
LUO Yarong, GUO Wenfei, QI Shufeng, GUO Chi. Invariant RTS Smoother and Its Application in 5G/SINS Integrated Navigation System[J]. Geomatics and Information Science of Wuhan University, 2024, 49(5): 747-755. DOI: 10.13203/j.whugis20210587
Citation: LUO Yarong, GUO Wenfei, QI Shufeng, GUO Chi. Invariant RTS Smoother and Its Application in 5G/SINS Integrated Navigation System[J]. Geomatics and Information Science of Wuhan University, 2024, 49(5): 747-755. DOI: 10.13203/j.whugis20210587

不变RTS平滑算法及其在5G/SINS组合导航中的应用研究

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

  • 摘要: 为了解决导航系统的位置误差随时间累积的问题,提出一种状态空间为矩阵李群的不变RTS(invariant Rauch-Tung-Striebel, InRTS)平滑算法来提高导航状态的精度,其核心在于导航状态及观测都利用矩阵李群进行表示及推导。首先,将系统状态及其误差状态微分方程用矩阵李群来表征,相比于传统扩展卡尔曼滤波算法(extended Kalman filter,EKF),将系统状态构建在矩阵李群上,其动力学方程可以更自然更本质地描述物体在空间中的运动特性,而通过群运算定义的误差状态耦合了姿态误差而不是直接的向量相减;接着,通过李群和李代数之间的对数映射将不变误差状态变量映射到李代数空间,得到一个解析的线性微分方程,该方程与当前状态估计值无关,从而可以获得在任何线性化点处状态无关的雅克比矩阵;然后,根据5G定位中量测模型的左不变特性推导包含杆臂误差的左不变误差状态量测方程,从而得到一个左不变EKF;最后,以左不变EKF为前向滤波,提出不变RTS平滑算法,并将其应用于5G/SINS(strapdown inertial navigation system)组合导航系统,在仿真实验中与左不变EKF算法进行了精度对比。实验结果表明,InRTS算法能对左不变EKF算法得到的轨迹进行平滑,降低了前向滤波过程中导航状态的累积误差,从而证明了所提算法的有效性。

     

    Abstract:
    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.

     

/

返回文章
返回