Ask Your Question

How to fix the EKF pose when the robot is definitely not moving?

asked 2016-02-29 00:03:12 -0500

updated 2016-02-29 00:03:46 -0500

I have a robot with

  • cameras providing absolute poses and
  • wheel encoders providing odometry (i.e. x, y and yaw velocities).

A robot_localization_ekf node fuses both measurements and yields good results.

However, I'm struggling with the following situation: The robot is standing still (the wheel encoders yield very certain zero velocities) and the cameras don't yield any poses (there are no visual markers in the current field of view). How can we prevent the EKF from predicting a filtered odometry with huge uncertainties, when we know the robot is not moving? I thought about

  • pausing the entire EKF node or
  • dynamically decreasing the process noise

until I either receive camera poses or non-zero velocities. But, I guess, both is impossible with the current version of robot_localization.

Sure, since there is no absolute information for a period of time, the EKF pose uncertainty grows unboundedly. But there should be a possibility to circumvent this behavior in certain situations. Although the absolute pose is unknown at the moment, it doesn't change for sure and it's uncertainty doesn't change as well.

Do you have any ideas how to handle this case?

(By the way, I know of other applications, e.g. with advanced driver assistance systems, where they adjust the process noise depending on the situation: driving straight, turning at an intersection, preventing a collision, ...)

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2016-03-01 11:44:22 -0500

Tom Moore gravatar image

You're correct, there is no way to handle this situation currently. The process noise covariance matrix is currently assumed to be static, but there are clearly situations where this isn't ideal. This should really be handled by monitoring the velocity and freezing the error growth when the robot stops moving. There are forks that already do this, and I will look into merging those changes upstream. Feel free to file a ticket on the r_l GitHub page.

edit flag offensive delete link more


Thanks Tom, that would be great! :)

Falko gravatar image Falko  ( 2016-03-01 12:48:22 -0500 )edit

Just for cross-referring the github ticket:

Falko gravatar image Falko  ( 2016-03-01 16:39:02 -0500 )edit

answered 2016-03-01 09:28:50 -0500

vaziri gravatar image

updated 2016-03-01 10:47:03 -0500

I have not worked with this package but here are some guesses:

  • Make a pose "sensor" [edit: I previously said odometery instead of pose] which sends measurements of zero velocity with zero* variance. It only sends out such messages when the odometry message has had zero velocty for a short time. When the robot starts moving again there will be a short delay before the EKF acknowledges movement based on the sensor_timeout value.
  • Use the manual state reset to continually put the system back to its values before forward prediction

*Dont literally use 0, or it will be set to 1e-6 (see the last note on the sensor data page)

edit flag offensive delete link more


Thanks @vaziri, but the sensor is already sending zero velocities. This doesn't prevent the pose uncertainty to grow due to the process noise. The state reset might work, but I guess it would destroy the current variance information by nailing it to a fixed pose.

Falko gravatar image Falko  ( 2016-03-01 09:54:41 -0500 )edit

The manual state reset includes the covariance matrix; I think you might not have to destroy anything.

vaziri gravatar image vaziri  ( 2016-03-01 10:15:13 -0500 )edit

Yes, you're right: The set_pose topic is of type PoseWithCovarianceStamped. So this would probably suffice as a workaround.

Falko gravatar image Falko  ( 2016-03-01 16:43:11 -0500 )edit

I'm not sure what set_state might do to the covariance of the rest of the filter state (velocities, accelerations, cross-covariances). Try a virtual pose sensor and send pose measurements with small covariance when the odometry is 0, as suggested in the (updated) answer above.

demmeln gravatar image demmeln  ( 2016-03-01 17:55:52 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2016-02-29 00:03:12 -0500

Seen: 721 times

Last updated: Mar 01 '16