ekf_localizer latency issue

[meliketanrikulu]

We observe that the change in the processing time of the ekf_localizer package is very large. In some cases, the processing time is increased. But we couldn't understand why.
Using GNSS-INS data, we publish messages from two different topics, geometry_msgs/PoseWithCovarianceStamped and geometry_msgs/TwistWithCovarianceStamped. In the video below, the red marker is the output of the geometry_msgs/PoseWithCovarianceStamped message given as an input to the EKF. The blue marker is the visualization of the /ekf_pose_with_covariance_without_yawbias topic in ekf_localizer output.

You can observe the change in the process time in the video at the link below.

ekf_localizer_test_video

I am also sharing the ekf_localizer.launch.xml file with you. Here you can see the configurations we have made

ekf_localizer.launch.zip

If you have an idea about what the reason is, share it. Thanks for your recommendations.

[yukkysaito]

Could you please give me a rosbag and branch that I can reproduce?
My guess is that there is a topic with zero speed. and If the deviation is too large, the Mahalanobis distance becomes too large and the velocity is determined to be an outlier, then the ekf pose seems to be catching up input pose.

[meliketanrikulu]

Thanks for your answer. I will check the Twist message.

[meliketanrikulu]

Using ins, I generate a twist message using the base_link's velocities relative to the map. ins fixed to the vehicle. In this case, how can I generate a twist message based on the base_link? Aren't the velocities always zero according to base_link? Thanks for your answer

[yukkysaito]

If the vehicle was moving forward at 4 m/s, the frame id of twist would be base_link and linear.x would be 4.

If you are estimating the velocity in map coordinates, you need to convert the coordinates to base_link coordinates. However, I do not recommend this. The velocity is estimated in order to estimate the position in the map coordinate system, but using the position to convert the velocity will cause a loop structure to be generated. Therefore, it is better to generate the velocity from an internal sensor attached to the vehicle, such as imu or vehicle odometry. If the velocity cannot be estimated by the internal sensor, then ekf_localizer can work without any velocity observation (twist with covariance topic).

[TakaHoribe]

@meliketanrikulu

Aren't the velocities always zero according to base_link?

More precisely, it is the velocity relative to the "stational base_link coordinate system". It sounds a little strange but is often used in robotics systems for its simplicity.

If you want to use the velocity sensor defined in the map coordinate like GNSS, you can do a conversion like this:

twist_in_baselink.linear.x = std::hypot(twist_in_map.linear.x, twist_in_map.linear.y);  // total velocity
twist_in_baselink.linear.y = 0.0;  // expect zero-lateral velocity (no slip)
twist_in_baselink.angular.z = twist_in_map.angular.z;  // no conversion is needed.

(other fields are zero.)

Please note that the above conversion is just one option and only works in the no-slip condition. However, It actually works almost perfectly in the low~midle speed range. The EKF will also fix the errors caused by the no-slip assumption.

[meliketanrikulu]

Hello again. Thanks for your answers. I updated my data with reference to this suggestion. I see the issue is resolved. I don't see any latency problem.