Ask Your Question

Revision history [back]

Please also include a sample sensor message from each sensor input, and maybe a better description (or image) of what the problem is with the filter output.

I see a couple things wrong here.

  • In your odom->base_link EKF instance, you are just fusing IMU data. That's going to be unbelievably noisy. If you watch your odom-frame state estimate, I would think it's probably drifting off endlessly. That won't really matter for your map-frame state estimate, but it makes me question why you are bothering with two EKF instances.
  • You have a similar problem in your map->odom EKF. You have no velocity reference (e.g., no wheel odometry or visual odometry), so you are just feeding the filter poses and accelerations. The GPS positions will keep dragging the EKF back on track, but the acceleration biases are still going to get integrated into velocity and try to drag the state estimate away. I generally don't recommend straight IMU + GPS fusion. The package just doesn't handle it well, frankly.

I think you're going to need a velocity reference for this to work well.