An Adaptive Visual/Inertial Integrated Navigation Algorithm Based on Lie Groups

To improve the accuracy and stability of the visual/inertial integrated navigation system under the condition of data's abrupt change, we propose an adaptive integrated navigation algorithm based on Lie group and unscented Kalman filter. The pose and motion state were modeled based on Lie group/Lie algebra, and then the position and state are estimated using unscented Kalman filter. In the filtering process, the fading factor is introduced to adaptively correct the state estimation covariance, thereby improving the robustness of the integrated navigation algorithm under data jumping. Moreover, to ensure numerical stability in the process of filtering, the square root-based implementation of the proposed algorithm is given. Experimental results demonstrate the effectiveness and superiority of the proposed algorithm.