Kinematic Base State Estimation for Humanoid using Invariant Extended Kalman Filter

This paper presents the design and implementation of a Right Invariant Extended Kalman Filter (RIEKF) for estimating the states of the kinematic base of the Surena V humanoid robot. The state representation of the robot is defined on the Lie group $SE_{4}$ (3), encompassing the position, velocity, and orientation of the base, as well as the position of the left and right feet. In addition, we incorporated IMU biases as concatenated states within the filter.The prediction step of the RIEKF utilizes IMU equations, while the update step incorporates forward kinematics. To evaluate the performance of the RIEKF, we conducted experiments using the Choreonoid dynamic simulation framework and compared it against a Quaternion-based Extended Kalman Filter (QEKF). The results of the analysis demonstrate that the RIEKF exhibits reduced drift in localization and achieves estimation convergence in a shorter time compared to the QEKF. These findings highlight the effectiveness of the proposed RIEKF for accurate state estimation of the kinematic base in humanoid robotics.