Estimated output lagging and then overshooting

[sukhrajklair]

I'm using the rolling branch of 'fuse' to fuse the output of lidar scan matching (absolute x, y and yaw) and wheel odometry (linear x and angular yaw velocity). I observe the estimated output lagging followed by overshoot and eventual correction. This causes the stationary objects (walls, parked cars etc.) to appear moving around the robot in the fixed frame (map). I've tried to capture this in the screen recording linked below.

I also recorded a bagfile while recording the video.

My configuration is as follows:

fuse_map:
  ros__parameters:
    # Fixed-lag smoother configuration
    optimization_frequency: 20.0
    transaction_timeout: 0.1
    lag_duration: 1.0

    motion_models:
      unicycle_motion_model:
        type: fuse_models::Unicycle2D

    unicycle_motion_model:
      #                         x      y      yaw    vx     vy     vyaw   ax   ay
      process_noise_diagonal: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.1, 0.1]

    sensor_models:
      initial_localization_sensor:
        type: fuse_models::Unicycle2DIgnition
        motion_models: [unicycle_motion_model]
        ignition: true
      wheel_odometry:
        type: fuse_models::Odometry2D
        motion_models: [unicycle_motion_model]
      ndt_pose:
        type: fuse_models::Pose2D
        motion_models: [unicycle_motion_model]

    initial_localization_sensor:
      publish_on_startup: true
      #                x      y      yaw    vx     vy     vyaw    ax     ay
      initial_state: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000]
      initial_sigma: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.100]

    wheel_odometry:
      topic: 'wheel_odom'
      twist_target_frame: 'base_link'
      linear_velocity_dimensions: ['x', 'y']
      angular_velocity_dimensions: ['yaw']

    ndt_pose:
      topic: 'pcl_pose'
      position_dimensions: ['x', 'y']
      orientation_dimensions: ['yaw']

    publishers:
      filtered_publisher:
        type: fuse_models::Odometry2DPublisher

    filtered_publisher:
      topic: 'odometry/global'
      base_link_frame_id: 'base_link'
      odom_frame_id: 'odom'
      map_frame_id: 'map'
      world_frame_id: 'map'
      publish_tf: true
      publish_frequency: 20.0

I have also tried setting optimization_frequency and publish_frequency to 10, and lag_duration to 0.5 but it did not help.