Ask Your Question

Revision history [back]

Offhand, I note three things:

  1. You aren't fusing yaw or yaw velocity from your wheel odometry data. That's going to cause a covariance explosion in the output, which will have all kinds of ugly side effects. In general, every single pose or velocity variable needs at least one reference (measurement source). In the case of two_d_mode, you need a reference for X or X velocity, Y or Y velocity, and yaw or yaw velocity. You are missing the yaw data.
  2. I'm not sure why you're even using an EKF for the odom frame. You only have one input. If your IMU has gyros, I'd bring that back in. Only magnetometers are sensitive to magnetic fields. If you are sticking with just wheel odometry, then I suggest making your wheel odometry node publish the odom->base_link transform, and getting rid of your odom frame EKF.
  3. Even with sensitivity to magnetic distortion in general, if you at least _start_ your robot in a location that doesn't have magnetic fields, you ought to be fine with using your IMU.