Revision history [back]

So one thing to change right off the bat is that if you are using pose data from amcl and you need the filter to more or less immediately converge to that same pose, then you need to increase the process_noise_covariance for X, Y, and yaw. Otherwise, the filter will trust its own estimate more than the measurement, and since there's no yaw velocity being measured, that's going to be sluggish. Also, note that amcl does not constantly update its pose. It only sends out pose updates when it reaches the thresholds that are defined in its configuration. You should have some velocity references being fused as well.

So one thing to change right off the bat is that if you are using pose data from amcl and you need the filter to more or less immediately converge to that same pose, then you need to increase the process_noise_covariance for X, Y, and yaw. Otherwise, the filter will trust its own estimate more than the measurement, and since there's no ~~yaw~~ velocity being measured, that's going to be sluggish. Also, note that amcl does not constantly update its pose. It only sends out pose updates when it reaches the thresholds that are defined in its configuration. You should have some velocity references being fused as well.

So one thing to change right off the bat is that if you are using pose data from amcl and you need the filter to more or less immediately converge to that same pose, then you need to increase the process_noise_covariance for X, Y, and yaw. Otherwise, the filter will trust its own estimate more than the measurement, and since there's no velocity being measured, that's going to be sluggish. Also, note that amcl does not constantly update its pose. It only sends out pose updates when it reaches the thresholds that are defined in its configuration. You should have some velocity references being fused as well.

EDIT 1:

I still haven't had a chance to look at your bag (sorry, getting set up and reviewing someone's bagged data takes a considerable amount of time that I can't afford right now), but just looking at the EKF config that you linked, I can see some problems right off the bat:

I'm referring to this block:
```
odom0: /odom
odom0_config: [true,true,false,
               false, false, false,
               false, false, false,
               false, false, true,
               false, false, false]
odom0_differential: false
odom0_relative: false
odom0_queue_size: 2
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1
odom0_nodelay: false

pose0: /amcl_pose
pose0_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
pose0_differential: true
pose0_relative: false
pose0_queue_size: 5
pose0_rejection_threshold: 2
pose0_nodelay: false
```
First, you are fusing absolute pose variables from two different sources. Don't do that. If you drive forward 10 meters and your amcl pose puts you at (10, 0) and your wheel odometry puts you at (9.1, 0), then the filter is going to rapidly jump back and forth between those two readings. Change your wheel odometry config to only fuse velocities.

Second, get rid of the rejection thresholds. Those are parameters that should not be included until you've had a change to solidly characterize the noise in your measurements.

Third, enable nodelay for the odometry data. The odometry message size is above a threshold that causes trouble with Nagle's algorithm, causing messages to arrive at strange intervals.
The EKF is, obviously, a filter. All filters will induce a non-zero delay in output. If your amcl data is fused in an EKF, it's going to have at least a small amount of lag. You can minimize this, as I said, by increasing the process_noise_covariance for a given variable. I realize you've done this.
Can you repeat your rotation test by eliminating the second EKF (the one fusing the amcl data) and let amcl publish the map->odom transform?