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.

翻译：本文介绍了用于估计Surena V型仿人机器人运动学基座状态的右不变扩展卡尔曼滤波（RIEKF）的设计与实现。机器人的状态表示定义在$SE_4(3)$李群上，包含基座的位置、速度和姿态，以及左右脚的位置。此外，我们将IMU偏置作为串联状态引入滤波器中。RIEKF的预测步骤采用IMU方程，而更新步骤则利用正运动学。为评估RIEKF的性能，我们使用Choreonoid动力学仿真框架进行实验，并与基于四元数的扩展卡尔曼滤波（QEKF）进行对比。分析结果表明，与QEKF相比，RIEKF在定位中表现出更低的漂移，且能在更短时间内实现状态估计收敛。这些发现凸显了所提出的RIEKF在仿人机器人运动学基座精确状态估计中的有效性。