Ask Your Question

Revision history [back]

This is expected behavior.

When fusing measurements, an EKF is concerned primarily with two quantities:

  1. The error (covariance) in the state estimate
  2. The error (covariance) in the measurement

So, as you drive your robot around, if you have no pose data coming in, the pose error increases in your state estimate as you integrate velocity measurements. This is what happens between your GPS measurements. Then you receive a GPS measurement, and the filter does a weighted average between that GPS data and your current state estimate. The weights it uses are based on the relative covariances. So if your GPS is jumping around, then unless the covariances are accurate, you're going to see a fair amount of jumping around.

So, to that end, you need to fill out covariance data in all your sensor measurements. Even then, the GPS is very likely to violate the covariance it reports, so your motion won't always be smooth.

This is the very reason for having two separate world frames (e.g., _map_ and _odom_), and for running two EKFs. The _odom_-frame EKF fuses only continuous data, and its output is used by, e.g., the local planner in move_base. The global pose estimate is not subject to drift, thanks to the GPS, but it IS subject to discrete discontinuities.

I recommend reading over REP-105, if you haven't yet.