I can't seem to get reliable mapping functionality from slam_toolbox.

I am using:

• Ubuntu 22.04
• ROS 2 Humble
• slam_toolbox
• Webots Simulator (R2023a)
• Robot_localization (with an IMU for improved odometry) - This is working well to correct for wheel sliding.

I have a four-wheeled robot in ROS 2 imported into a Webots simulation, and I'm trying to use slamtoolbox to map the environment. It works as expected when ```usescanmatching: falsein mymapperparamsonlineasync.yamlfile. But as soon as I start scan_matching to compensate for odom drift... it goes haywire. Even with small movements, themap -> odom``` frame spins all over the place with every map update.

use_scan_matching: true https://ibb.co/WgfbnRf

use_scan_matching: false https://ibb.co/vxTThdM

Here is the params file I'm using at the moment:

slam_toolbox config

slam_toolbox:
  ros__parameters:

    # Plugin params
    solver_plugin: solver_plugins::CeresSolver
    ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
    ceres_preconditioner: SCHUR_JACOBI
    ceres_trust_strategy: LEVENBERG_MARQUARDT
    ceres_dogleg_type: TRADITIONAL_DOGLEG
    ceres_loss_function: None

    # ROS Parameters
    odom_frame: odom
    map_frame: map
    base_frame: base_link
    scan_topic: /scan #/ Mapped from the lidar
    mode: mapping
    # mode: localization

    debug_logging: false
    throttle_scans: 1
    transform_publish_period: 0.02 #if 0 never publishes odometry
    map_update_interval: 0.01
    resolution: 0.05
    max_laser_range: 50.0 #for rastering images
    minimum_time_interval: 0.1
    transform_timeout: 0.2
    tf_buffer_duration: 30.0
    # scan_queue_size: 1
    stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps
    enable_interactive_mode: true

    # General Parameters
    use_scan_matching: false
    use_scan_barycenter: false
    minimum_travel_distance: 0.05
    minimum_travel_heading: 0.5
    scan_buffer_size: 100
    scan_buffer_maximum_scan_distance: 10.0
    link_match_minimum_response_fine: 0.1  
    link_scan_maximum_distance: 0.5
    loop_search_maximum_distance: 3.0
    do_loop_closing: true
    loop_match_minimum_chain_size: 5
    loop_match_maximum_variance_coarse: 3.0  
    loop_match_minimum_response_coarse: 0.35    
    loop_match_minimum_response_fine: 0.45

    # Correlation Parameters - Correlation Parameters
    correlation_search_space_dimension: 0.5
    correlation_search_space_resolution: 0.01
    correlation_search_space_smear_deviation: 0.1 

    # Correlation Parameters - Loop Closure Parameters
    loop_search_space_dimension: 8.0
    loop_search_space_resolution: 0.05
    loop_search_space_smear_deviation: 0.03

    # Scan Matcher Parameters
    distance_variance_penalty: 0.5
    angle_variance_penalty: 1.0

    fine_search_angle_offset: 0.00349
    coarse_search_angle_offset: 0.349
    coarse_angle_resolution: 0.0349
    minimum_angle_penalty: 0.9
    minimum_distance_penalty: 0.5
    use_response_expansion: true

I have tried the webots turtlebot3 tutorial with the same param file and the default one, with the same inconsistent results. https://github.com/cyberbotics/webotsros2/tree/master/webotsros2_turtlebot. Could this be due to the Webots lidar simulation? Or a timing issue somewhere? I've been trying to figure this out for about two weeks.