The delay of state estimator

[hxydebug]

Sorry to bother you. Right now I am using this drift project to estimate my biped robot. But I found there are many delays of my estimated orientation and COM position in world frame. What can I do to decrease the delay? Is the correction_time_threshold very important for my problem?
Meanwhile, I found the CoM position in z direction is drifting. Is it because this code do not set z = 0 when foot contact the ground?

This picture shows the z direction drifting of CoM position. Dash line is estimated data and solid line is from landmark.

[tzuyuan]

Hi,

By delay do you mean the output frequency is slower than the sensor frequency? If so, what's the frequency of your sensor inputs?

For the Z drifting problem, we observe that this can happen when false positive contact events exist or when the kinematics are inaccurate. I recommend doing some sanity checks on these two measurements.

[hxydebug]

My frequency of IMU input is 400hz, and the frequency of joint state and contact information is 1000hz. My hardware platform is Nvidia tx2, and I'm not sure if the ros publisher and subscriber can work in 1000hz with my platform. The picture below shows the euler angle comparison, the dash line is estimated data and solid line is from vicon. My delay here means from the picture, the estimated data somehow is a little bit later than vicon data. My one of thoughts to solve this problem is trying to write another thread in my original code based on your estimator, avoiding using ros communication to ensure real time. Do you have some advice?

As for the z drifting problem, I will do some checks s you recommended. Thanks a lot!

[hxydebug]

By the way, these days I am looking at the core of Invariant EKF. And I saw the mini_cheetah example and I noticed that you chose left error for InEKF. But in paper I saw they used rigth error to induce the legged robot estimator. Are there some differences between them?

[tzuyuan]

Hi Xinyan,

I'm not too sure why this is happening. If you use the examples in the ros folder, the subscriber is already set as a standalone thread to the main program. You can use rostopic hz /robot/inekf_estimation/pose to see if the estimator is outputting the pose at the correct frequency. If not, you might want to print out the measurement timestamps within the program or the size of the measurement buffer queue to double-check if the bottleneck is within DRIFT.

If you use "init_bias" config, then the first several measurements will be used to estimate the bias. The filter would not start running until it is initialized.

As for the error type, you're right. For legged kinematics, one should use the right invariant error when tracking your states in the world frame. However, we also provide mappings from the left-invariant error to the right. We might have accidentally pushed the wrong code when we were testing the mapping between the errors. Thanks for pointing this out!

[hxydebug]

Thanks very much! I think my kinematics is fine, since I ran the mit cheetah's linear Kalman Filter estiamtor and it can work. And I want to improve the CoM position estimate, so I choose invariant EKF. I will check the frequency and measurement timestamps to see what happening here. Also, I will try to use right error in this Ros folder.