A Cubature Kalman Filtering Algorithm for Robot Pose Estimation

To maintain stability while executing the walking paths, robots have to know its own pose. However, the visual sensor of the visual/inertial navigation odometer (VIO) system is easily affected by the environmental and lighting conditions, and the measurement data of the Inertial Measurement Units (IMU) have a drift problem caused by accumulated errors, which makes the VIO system transmit the robot rotation uncertainty inaccurately. Aiming at the defects of vision sensor and IMU in robot positioning, to effectively improve the accuracy and stability of rigid body pose estimation while eliminating system internalization errors, a matrix Lie group representation based Cubature Kalman filter (CKF) VIO framework is proposed in this paper. By utilizing the high-dimensional matrix Lie group, which is composed of IMU's position, orientation, velocity and spatial three-dimensional (3D) feature points, to represent the system state variables, the accurate estimation of the robot's pose is achieved under the CKF framework. This paper derives the motion model of the IMU and the camera observation, and describes the process of constructing the Cubature point of the exponential map when the state vector dimension is high, and also transfers the state uncertainty and observation update. Finally, pose estimation accuracy verification experiments are carried out. Results show that the proposed algorithm not only inherits the advantages of the invariant extended Kalman filter (In-EKF) in terms of convergence and consistency, but also avoids the complexity of matrix solution process, improving the accuracy of robot pose estimation effectively.