Can you post your launch file?

Looking at your plots, it looks as if your IMU registers the turn long before your wheel encoder odometry. Additionally, it looks as if the filtered output is tracking much closer to your wheel encoder odometry than your IMU. I'd have to look at their relative covariances (which I realize are available in the bag file) to determine why you're seeing a delay in the filter response, but my first checks would be to (a) make sure that your angular velocity in your controller is reported as soon as the robot starts turning (there's some lag between the two, if I'm reading your plot correctly), and (b) the relative covariance between your IMU angular velocity and odometry angular velocity are set correctly, i.e., I'd expect a much lower variance on yaw velocity for the IMU than for your wheel encoders. Finally, you might want to also try increasing your initial_estimate_covariance for yaw velocity. This will help it converge faster when your robot is first starting.

EDIT 1

Still looking at this. A few things:

1. Did you intentionally turn the yaw off for your IMU0 config?

   imu0_config: [
       false, false, false,
       false, false, false,
       false, false, false,
       false, false, true,
       true, true, true
   ]
   

Just want to make sure that was right.

1. Have you had a chance to look into the delay in your wheel encoder odometry?

2. Regardless, here's a sample odometry message:

   header: 
     seq: 1997
     stamp: 
       secs: 1445291702
       nsecs: 645665414
     frame_id: odom
   child_frame_id: p1_001/base_link
   pose: 
     pose: 
       position: 
         x: -0.00496491246256
         y: 2.86013970781e-05
         z: 0.0
       orientation: 
         x: 0.0
         y: 0.0
         z: -0.158302775866
         w: 0.987390617311
     covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
   twist: 
     twist: 
       linear: 
         x: 0.00780897589398
         y: 0.0
         z: 0.0
       angular: 
         x: 0.0
         y: 0.0
         z: -1.10046405246
     covariance: [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
   

...and here's a sample IMU message:

    header: 
      seq: 1965
      stamp: 
        secs: 1445291701
        nsecs: 25784241
      frame_id: p1_001/imu
    orientation: 
      x: -0.608557879925
      y: -0.377054542303
      z: 0.5998955369
      w: -0.357228845358
    orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
    angular_velocity: 
      x: -0.00852211564779
      y: -0.00372842559591
      z: 0.0145142283291
    angular_velocity_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
    linear_acceleration: 
      x: -8.96927833557
      y: -0.289698392153
      z: -0.223259314895
    linear_acceleration_covariance: [0.2, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.2]

Right now, you're fusing yaw velocity from both the IMU and the wheel encoders. One problem is that your covariance for your wheel encoder odometry's yaw velocity is an order of magnitude smaller than the yaw velocity from your IMU. Kalman Filters really come down to weighted averaging, so if you have two data sources, and one has an error that is much smaller than the other, the one with the lower error will be trusted more by the filter. Pair this with what appears to be a delay in your wheel encoder odometry, and you might have a reason for the perceived lag.

1. On that front, I would set the initial_estimate_covariance parameter. Use the default values in the template launch file, except for the variables you're directly measuring, which I would make an order of magnitude larger than your largest error for any sensor measuring that variable.

2. Try the version that's in the repo (just updated). The way it was before, there was a one-spin-cycle delay between measurement processing and output. There are other improvements I can make there, but give that a shot first.