State estimation

[cinogul]

Hi,

I noticed that you said the hardware implementation was going to follow soon. Although having ALL the hardware code would be ideal, I was wondering whether your state estimation code is available anywhere? I want to implement a state estimator linked to mujoco to simulate my robot and find optimal control values for the robot using an MPC. Although mujoco and mujoco_mpc have sample code in their repo it would be nice to have an actual implementation to answer some of the following questions:

1. How do you load your state estimate into mujoco? Do you just set all qpos, qvel and qacc values manually before stepping the simulation?
2. Do you smooth the state estimate at all? I can imagine that there is cases where the sim somewhat diverges from what happens on the hardware, leading to a "jump" in sim leading to suboptimal control values.
3. You mentioned in your paper that parameter tuning, especially the height of the robot, is essential at minimizing the mismatch between estimator and planner. Do you only use motion capture for getting the height? Is there another smart way to do this, with some smart sensor fusion and an IKS?

Thanks again for any info/code you can give me!

[jrapudg]

Hi,
Thank you for reaching out. We expect to release the hardware code in the coming weeks. In the meantime, you can check out the state estimation component we used for our pipeline at:
https://github.com/ShuoYangRobotics/legged_control/tree/master/legged_estimation
This repository contains the implementation of an Extended Kalman Filter (EKF) based on the MIT Cheetah project (for more details, see the paper:
https://dspace.mit.edu/bitstream/handle/1721.1/126619/IROS.pdf)

Answering your questions:

1. We update MuJoCo by loading the simulation state with our estimated values. The body position and orientation are obtained from our motion capture system (raw measurements), the body linear and angular velocities come from the EKF (which smooths these estimates), and the joint positions and velocities are provided by the hardware encoders (raw measurements). Then, we step the controller that runs the parallel simulations in MuJoCo.
2. We do not apply additional smoothing to the state estimates. We recognize that discrepancies between the simulation and hardware, particularly during contact events, can lead to sudden jumps in the simulation, resulting in suboptimal control outputs. Addressing this real-to-sim gap is an active area of research in our lab.
3. We primarily use motion capture to obtain an accurate global reference. However, the EKF also estimates height by fusing data from the IMU and forward kinematics.

I hope this answers your questions. Please feel free to ask if you need any additional details.