Invariant Cubature Kalman Filtering-Based Visual-Inertial Odometry for Robot Pose Estimation

To maintain mechanistic stability while tracking the designated walking route, a robot must be cognizant of employed posture. Generally, visual-inertial odometer (VIO) is utilized for robot state estimation; however, the traditional cubature Kalman filter VIO (CKF-VIO) cannot transfer rotational uncertainty and compensate for the system's processing error. To effectively improve the accuracy and stability of robot rigid body pose estimation, this paper proposes a matrix Lie group representation-based CKF framework which characterizes the uncertainty prompting in robotic motion while eliminating the VIO system internalization errors. The robot state, consisting of inertial measurement unit (IMU) pose, velocity, and 3-D landmarks' positions, is deemed to be a single element of a high-dimensional Lie group SE2+p (3), while the accelerometers' and gyrometers' biases are appended to the state and estimated as well. The algorithm is validated by simulations with Monte Carlo and experiment. Results show that the CKF-VIO with a high-dimensional Lie group can improve the accuracy and consistency of robot pose estimation.